move HLcmd rate limit out of stabilizer.c

src/modules/interface/crtp_commander_high_level.h

@@ -59,7 +59,7 @@ void crtpCommanderHighLevelInit(void);
 
 // Retrieves the current setpoint. Returns false if the high-level commander is
 // disabled, i.e. it does not have an "opinion" on what the setpoint should be.
-bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state);
+bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state, uint32_t tick);
 
 // When flying sequences of high-level commands, the high-level commander uses
 // its own history of commands to determine the initial conditions of the next

src/modules/src/crtp_commander_high_level.c

@@ -298,8 +298,12 @@ int crtpCommanderHighLevelDisable()
   return 0;
 }
 
-bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
+bool crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state, uint32_t tick)
 {
+  if (!RATE_DO_EXECUTE(RATE_HL_COMMANDER, tick)) {
+    return false;
+  }
+
   xSemaphoreTake(lockTraj, portMAX_DELAY);
   float t = usecTimestamp() / 1e6;
   struct traj_eval ev = plan_current_goal(&planner, t);

src/modules/src/stabilizer.c

@@ -259,10 +259,8 @@ static void stabilizerTask(void* param)
       stateEstimator(&state, tick);
       compressState();
 
-      if (RATE_DO_EXECUTE(RATE_HL_COMMANDER, tick)) {
-        if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state)) {
-          commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL);
-        }
+      if (crtpCommanderHighLevelGetSetpoint(&tempSetpoint, &state, tick)) {
+        commanderSetSetpoint(&tempSetpoint, COMMANDER_PRIORITY_HIGHLEVEL);
       }
 
       commanderGetSetpoint(&setpoint, &state);