Remove command_pitch and command_roll and command_yaw

Update sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c Update sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c Update sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c Update sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c rebased
paparazzi · Dec 19, 2023 · cfb2d38 · cfb2d38
1 parent 94c79d4
commit cfb2d38
Show file tree

Hide file tree

Showing 7 changed files with 32 additions and 7 deletions.
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml
@@ -236,9 +236,6 @@
         <axis name="SKEW"        failsafe_value="0"/>
         <!-- default commands -->
         <axis name="THRUST"      failsafe_value="0"/>
-        <axis name="ROLL"        failsafe_value="0"/>
-        <axis name="PITCH"       failsafe_value="0"/>
-        <axis name="YAW"         failsafe_value="0"/>
     </commands>
 
     <command_laws>

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
@@ -227,11 +227,13 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *d
 
 static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
 {
+#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
   pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
                                &stabilization_cmd[COMMAND_ROLL],
                                &stabilization_cmd[COMMAND_PITCH],
                                &stabilization_cmd[COMMAND_YAW],
                                &stabilization_cmd[COMMAND_THRUST]);
+#endif
 }
 
 

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_utils.c b/sw/airborne/firmwares/rotorcraft/autopilot_utils.c
@@ -121,7 +121,7 @@ uint8_t ap_mode_of_two_switches(void)
 void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on __attribute__((unused)))
 {
 #if !ROTORCRAFT_IS_HELI
-#if !ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
+#if !ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED && defined(COMMAND_YAW)
   if (!in_flight) {
     cmd_in[COMMAND_YAW] = 0;
   }
@@ -130,9 +130,15 @@ void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flig
     cmd_in[COMMAND_THRUST] = 0;
   }
 #endif
+#ifdef COMMAND_ROLL
   cmd_out[COMMAND_ROLL] = cmd_in[COMMAND_ROLL];
+#endif
+#ifdef COMMAND_PITCH
   cmd_out[COMMAND_PITCH] = cmd_in[COMMAND_PITCH];
+#endif
+#ifdef COMMAND_YAW
   cmd_out[COMMAND_YAW] = cmd_in[COMMAND_YAW];
+#endif
   cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
@@ -64,6 +64,8 @@ void guidance_flip_enter(void)
 
 void guidance_flip_run(void)
 {
+#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW) 
+
   uint32_t timer;
   int32_t phi;
   static uint32_t timer_save = 0;
@@ -138,4 +140,8 @@ void guidance_flip_run(void)
       stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
       break;
   }
+#else
+  autopilot_set_mode(autopilot_mode_old);
+  stab_att_sp_euler.psi = heading_save;
+#endif
 }
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -85,6 +85,7 @@ static void send_href(struct transport_tx *trans, struct link_device *dev)
 
 static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
 {
+#if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
   pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
                                       &radio_control.values[RADIO_ROLL],
                                       &radio_control.values[RADIO_PITCH],
@@ -96,6 +97,7 @@ static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
                                       &(stateGetNedToBodyEulers_i()->phi),
                                       &(stateGetNedToBodyEulers_i()->theta),
                                       &(stateGetNedToBodyEulers_i()->psi));
+#endif
 }
 
 #endif
@@ -426,9 +428,15 @@ void guidance_h_from_nav(bool in_flight)
   }
 
   if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_MANUAL) {
+    #ifdef COMMAND_ROLL
     stabilization_cmd[COMMAND_ROLL]  = nav.cmd_roll;
+    #endif
+    #ifdef COMMAND_PITCH
     stabilization_cmd[COMMAND_PITCH] = nav.cmd_pitch;
+    #endif
+    #ifdef COMMAND_YAW
     stabilization_cmd[COMMAND_YAW]   = nav.cmd_yaw;
+    #endif
   } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
     if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
       // directly apply quat setpoint

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -727,9 +727,9 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   }
 
   // Set the stab_cmd to 42 to indicate that it is not used
-  stabilization_cmd[COMMAND_ROLL] = 42;
-  stabilization_cmd[COMMAND_PITCH] = 42;
-  stabilization_cmd[COMMAND_YAW] = 42;
+#if defined(COMMAND_ROLL) || defined(COMMAND_PITCH) || defined(COMMAND_YAW) 
+#warning "COMMAND_ROLL, PITCH, YAW not used in INDI: please remove it from your airframe file"
+#endif
 }
 
 /**

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c
@@ -56,7 +56,13 @@ void stabilization_none_enter(void)
 void stabilization_none_run(bool in_flight __attribute__((unused)))
 {
   /* just directly pass rc commands through */
+#ifdef COMMAND_ROLL
   stabilization_cmd[COMMAND_ROLL]  = stabilization_none_rc_cmd.p;
+#endif
+#ifdef COMMAND_PITCH
   stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q;
+#endif
+#ifdef COMMAND_YAW
   stabilization_cmd[COMMAND_YAW]   = stabilization_none_rc_cmd.r;
+#endif
 }