Fixed all kinds of fbw related problems

conf/telemetry/default_rotorcraft_slow.xml

@@ -27,7 +27,8 @@
       <message name="AIR_DATA"               period="2.3"/>
       <message name="SURVEY"                 period="4.5"/>
       <message name="OPTIC_FLOW_EST"         period="0.5"/>
-      <message name="VECTORNAV_INFO"      period="0.5"/>
+      <message name="VECTORNAV_INFO"         period="0.5"/>
+      <message name="FBW_STATUS"             period="2"/>
     </mode>
 
     <mode name="ppm">

sw/airborne/firmwares/rotorcraft/main_fbw.c

@@ -47,7 +47,8 @@
 #include "firmwares/rotorcraft/main_fbw.h"
 #include "firmwares/rotorcraft/autopilot_rc_helpers.h"
 
-//#include "generated/modules.h"
+#define MODULES_C
+#include "generated/modules.h"
 
 /** Fly by wire modes */
 typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
@@ -59,7 +60,7 @@ fbw_mode_enum fbw_mode;
 //PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
 tid_t main_periodic_tid; ///< id for main_periodic() timer
-//tid_t modules_tid;     ///< id for modules_periodic_task() timer
+tid_t modules_tid;     ///< id for modules_periodic_task() timer
 tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer
 tid_t electrical_tid;    ///< id for electrical_periodic() timer
 tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
@@ -85,17 +86,17 @@ STATIC_INLINE void main_init(void)
 
   mcu_init();
 
+  actuators_init();
+
   electrical_init();
 
-  actuators_init();
 #if USE_MOTOR_MIXING
   motor_mixing_init();
 #endif
 
   radio_control_init();
 
-  // TODO
+  modules_init();
-  //modules_init();
 
   mcu_int_enable();
 
@@ -103,7 +104,7 @@ STATIC_INLINE void main_init(void)
 
   // register the timers for the periodic functions
   main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-//  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
+  modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
   radio_control_tid = sys_time_register_timer((1. / 60.), NULL);
   electrical_tid = sys_time_register_timer(0.1, NULL);
   telemetry_tid = sys_time_register_timer((1. / 20.), NULL);
@@ -115,13 +116,15 @@ STATIC_INLINE void main_init(void)
 
 STATIC_INLINE void handle_periodic_tasks(void)
 {
+
+
   if (sys_time_check_and_ack_timer(main_periodic_tid)) {
     main_periodic();
   }
-  //if (sys_time_check_and_ack_timer(modules_tid)) {
+  if (sys_time_check_and_ack_timer(modules_tid)) {
-  // TODO
+
-  //modules_periodic_task();
+    modules_periodic_task();
-  //}
+  }
   if (sys_time_check_and_ack_timer(radio_control_tid)) {
     radio_control_periodic_task();
   }
@@ -175,9 +178,17 @@ STATIC_INLINE void main_periodic(void)
     }
   }
 
+
+  static uint16_t dv = 0;
+  // TODO make module out of led blink?
   /* set failsafe commands     */
   if (fbw_mode == FBW_MODE_FAILSAFE) {
     SetCommands(commands_failsafe);
+    if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
+  } else if (fbw_mode == FBW_MODE_MANUAL) {
+    if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
+  } else if (fbw_mode == FBW_MODE_AUTO) {
+    LED_TOGGLE(FBW_MODE_LED); // toggle instead of on, because then it is still visible when fbw_mode switches very fast
   }
 
   /* set actuators     */
@@ -208,6 +219,7 @@ static void autopilot_on_rc_frame(void)
 
   /* if manual */
   if (fbw_mode == FBW_MODE_MANUAL) {
+    autopilot_motors_on = TRUE;
 #ifdef SetCommandsFromRC
     SetCommandsFromRC(commands, radio_control.values);
 #else
@@ -216,13 +228,15 @@ static void autopilot_on_rc_frame(void)
   }
 
   /* Forward radiocontrol to AP */
-  intermcu_on_rc_frame();
+  intermcu_on_rc_frame(fbw_mode);
 }
 
 static void autopilot_on_ap_command(void)
 {
   if (fbw_mode != FBW_MODE_MANUAL) {
     SetCommands(intermcu_commands);
+  } else {
+    autopilot_motors_on = TRUE;
   }
 }
 
@@ -237,6 +251,6 @@ STATIC_INLINE void main_event(void)
   // InterMCU
   InterMcuEvent(autopilot_on_ap_command);
 
-  // TODO Modules
+  //Modules
-  //modules_event_task();
+  modules_event_task();
 }

sw/airborne/subsystems/intermcu.h

@@ -33,7 +33,9 @@
 #define INTERMCU_AP   0
 #define INTERMCU_FBW  1
 
+#ifndef INTERMCU_LOST_CNT
 #define INTERMCU_LOST_CNT 25  /* 50ms with a 512Hz timer TODO fixed value */
+#endif
 
 enum intermcu_status {
   INTERMCU_OK,

sw/airborne/subsystems/intermcu/intermcu_ap.c

@@ -45,9 +45,21 @@ static struct pprz_transport intermcu_transport;
 struct intermcu_t inter_mcu;
 static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void));
 
+
+
+static void send_status(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
+                           &(radio_control.status), &(radio_control.frame_rate), &(inter_mcu.status), &electrical.vsupply,
+                           &electrical.current); // due to limitation of GCS, send the electrical from ap as if it comes from fbw...
+}
+
 void intermcu_init(void)
 {
   pprz_transport_init(&intermcu_transport);
+
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status);
+
 }
 
 void intermcu_periodic(void)
@@ -81,6 +93,7 @@ void intermcu_send_spektrum_bind(void)
   }
 }
 
+
 static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_frame_handler)(void))
 {
   /* Parse the Inter MCU message */
@@ -89,6 +102,7 @@ static inline void intermcu_parse_msg(struct transport_rx *trans, void (*rc_fram
     case DL_IMCU_RADIO_COMMANDS: {
       uint8_t i;
       uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(trans->payload);
+      inter_mcu.status = DL_IMCU_RADIO_COMMANDS_status(trans->payload);
       int16_t *rc_values = DL_IMCU_RADIO_COMMANDS_values(trans->payload);
       for (i = 0; i < size; i++) {
         radio_control.values[i] = rc_values[i];

sw/airborne/subsystems/intermcu/intermcu_ap.h

@@ -40,18 +40,12 @@ void disable_inter_comm(bool_t value);
 #define RADIO_ROLL       1
 #define RADIO_PITCH      2
 #define RADIO_YAW        3
-#define RADIO_GEAR       4
+#define RADIO_MODE       4
-#define RADIO_FLAP       5
+#define RADIO_KILL_SWITCH 5
 #define RADIO_AUX1       5
 #define RADIO_AUX2       6
 #define RADIO_AUX3       7
 #define RADIO_CONTROL_NB_CHANNEL 8
 
-#ifndef RADIO_MODE
-#define RADIO_MODE       RADIO_GEAR
-#endif
-//#ifndef RADIO_KILL_SWITCH
-//#define RADIO_KILL_SWITCH       RADIO_FLAP
-//#endif
 
 #endif /* INTERMCU_AP_ROTORCRAFT_H */

sw/airborne/subsystems/intermcu/intermcu_fbw.c

@@ -76,10 +76,38 @@ void intermcu_periodic(void)
   }
 }
 
-void intermcu_on_rc_frame(void)
+void intermcu_on_rc_frame(uint8_t fbw_mode)
 {
+
+  pprz_t  values[8];
+
+  values[INTERMCU_RADIO_THROTTLE] = radio_control.values[RADIO_THROTTLE];
+  values[INTERMCU_RADIO_ROLL] = radio_control.values[RADIO_ROLL];
+  values[INTERMCU_RADIO_PITCH] = radio_control.values[RADIO_PITCH];
+  values[INTERMCU_RADIO_YAW] = radio_control.values[RADIO_YAW];
+#ifdef RADIO_MODE
+  values[INTERMCU_RADIO_MODE] = radio_control.values[RADIO_MODE];
+#endif
+#ifdef RADIO_KILL_SWITCH
+  values[INTERMCU_RADIO_KILL_SWITCH] = radio_control.values[RADIO_KILL];
+#endif
+
+#if defined (RADIO_AUX1) && defined (RADIO_KILL_SWITCH)
+#warning "RC AUX1 and KILL_SWITCH are on the same channel."
+#endif
+
+#ifdef RADIO_AUX1
+  values[INTERMCU_RADIO_AUX1] = radio_control.values[RADIO_AUX1];
+#endif
+#ifdef RADIO_AUX2
+  values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2];
+#endif
+#ifdef RADIO_AUX3
+  values[INTERMCU_RADIO_AUX2] = radio_control.values[RADIO_AUX2];
+#endif
+
   pprz_msg_send_IMCU_RADIO_COMMANDS(&(intermcu_transport.trans_tx), intermcu_device,
-                                    INTERMCU_FBW, 0, RADIO_CONTROL_NB_CHANNEL, radio_control.values); //TODO: Fix status
+                                    INTERMCU_FBW, &fbw_mode, RADIO_CONTROL_NB_CHANNEL, values);
 }
 
 void intermcu_send_status(uint8_t mode)

sw/airborne/subsystems/intermcu/intermcu_fbw.h

@@ -29,9 +29,23 @@
 
 #include "subsystems/intermcu.h"
 
+extern uint8_t autopilot_motors_on;
 extern pprz_t intermcu_commands[COMMANDS_NB];
-void intermcu_on_rc_frame(void);
+void intermcu_on_rc_frame(uint8_t fbw_mode);
 void intermcu_send_status(uint8_t mode);
 void InterMcuEvent(void (*frame_handler)(void));
 
+
+/* We need radio defines for the Autopilot */
+#define INTERMCU_RADIO_THROTTLE   0
+#define INTERMCU_RADIO_ROLL       1
+#define INTERMCU_RADIO_PITCH      2
+#define INTERMCU_RADIO_YAW        3
+#define INTERMCU_RADIO_MODE       4
+#define INTERMCU_RADIO_KILL_SWITCH 5
+#define INTERMCU_RADIO_AUX1       5
+#define INTERMCU_RADIO_AUX2       6
+#define INTERMCU_RADIO_AUX3       7
+#define INTERMCU_RADIO_CONTROL_NB_CHANNEL 8
+
 #endif /* INTERMCU_FBW_ROTORCRAFT_H */