From 8c77356864b6130e3717b92317a07b6183dd9e40 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 15 Jun 2023 14:49:24 +0200 Subject: [PATCH] Stop high level commander instead of disable, when receiving low level set points. --- src/modules/src/commander.c | 5 ++--- src/modules/src/crtp_commander_high_level.c | 5 +++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c index 5bd85035c3..e5d1eab79d 100644 --- a/src/modules/src/commander.c +++ b/src/modules/src/commander.c @@ -84,9 +84,8 @@ void commanderSetSetpoint(setpoint_t *setpoint, int priority) xQueueOverwrite(setpointQueue, setpoint); xQueueOverwrite(priorityQueue, &priority); if (priority > COMMANDER_PRIORITY_HIGHLEVEL) { - // Disable the high-level planner so it will forget its current state and - // start over if we switch from low-level to high-level in the future. - crtpCommanderHighLevelDisable(); + // Stop the high-level planner so it will forget its current state + crtpCommanderHighLevelStop(); } } } diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index ce79882f7c..83161463f6 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -321,8 +321,9 @@ bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat yaw = radians(state->attitude.yaw); if (plan_is_stopped(&planner)) { // Return a null setpoint - when the HLcommander is stopped, it wants the - // motors to be off. Only reason they should be spinning is if the - // HLcommander has been preempted by a streaming setpoint command. + // motors to be off. + // Note: this set point will be overridden by low level set points, for instance received from an external source, + // due to the priority. To switch back to the high level commander, use the `commanderRelaxPriority()` functionality. *setpoint = nullSetpoint; return true; }