[fixedwing] fix initialzation of trim commands, including yaw

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -84,6 +84,19 @@
 
 #include "led.h"
 
+/* Default trim commands for roll, pitch and yaw */
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+#ifndef COMMAND_YAW_TRIM
+#define COMMAND_YAW_TRIM 0
+#endif
+
 /* if PRINT_CONFIG is defined, print some config options */
 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
@@ -230,6 +243,13 @@ void init_ap( void ) {
 #ifdef TRAFFIC_INFO
   traffic_info_init();
 #endif
+
+  /* set initial trim values.
+   * these are passed to fbw via inter_mcu.
+   */
+  ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
+  ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
+  ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
 }
 
 

sw/airborne/firmwares/fixedwing/main_fbw.c

@@ -58,16 +58,9 @@ uint8_t fbw_mode;
 #include "inter_mcu.h"
 
 
-/** Trim commands for roll and pitch/
+/** Trim commands for roll, pitch and yaw.
+ * These are updated from the trim commands in ap_state via inter_mcu
  */
-#ifndef COMMAND_ROLL_TRIM
-#define COMMAND_ROLL_TRIM 0
-#endif
-
-#ifndef COMMAND_PITCH_TRIM
-#define COMMAND_PITCH_TRIM 0
-#endif
-
 pprz_t command_roll_trim;
 pprz_t command_pitch_trim;
 pprz_t command_yaw_trim;
@@ -106,10 +99,6 @@ void init_fbw( void ) {
 
   fbw_mode = FBW_MODE_FAILSAFE;
 
-  command_roll_trim = COMMAND_ROLL_TRIM;
-  command_pitch_trim = COMMAND_PITCH_TRIM;
-
-
   /**** start timers for periodic functions *****/
   fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
   electrical_tid = sys_time_register_timer(0.1, NULL);