Merge pull request #1298 from bitcraze/krichardsson/hl-commander-prio

Stop high level commander instead of disable

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();
     }
   }
 }

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;
     }