intermcu

sw/airborne/firmwares/rotorcraft/main_fbw.c

@@ -55,7 +55,6 @@
 #define FBW_MODE_FAILSAFE 2
 
 uint8_t fbw_mode;
-bool_t intermcu_realy_lost;
 
 
 /* MODULES_FREQUENCY is defined in generated/modules.h
@@ -88,7 +87,6 @@ STATIC_INLINE void main_init(void)
 {
   // fbw_init
   fbw_mode = FBW_MODE_FAILSAFE;
-  intermcu_realy_lost = TRUE;
 
   mcu_init();
 
@@ -143,7 +141,6 @@ STATIC_INLINE void main_periodic(void)
   // TODO
   //autopilot_periodic();
   /* set actuators     */
-  //actuators_set(autopilot_motors_on);
   SetActuatorsFromCommands(commands, autopilot_mode);
 
   RunOnceEvery(10, LED_PERIODIC());
@@ -170,7 +167,7 @@ static uint8_t fbw_mode_of_3way_switch(void)
 void fbw_set_mode(uint8_t new_fbw_mode);
 void fbw_set_mode(uint8_t new_fbw_mode) {
   if (new_fbw_mode == FBW_MODE_AUTO)
-  if (intermcu_realy_lost && (new_fbw_mode == FBW_MODE_AUTO))
+  if ((inter_mcu.status == INTERMCU_LOST) && (new_fbw_mode == FBW_MODE_AUTO))
   {
     new_fbw_mode = AP_LOST_FBW_MODE;
   }
@@ -195,17 +192,22 @@ void autopilot_on_rc_frame(void)
 
   /* if manual */
   if (fbw_mode == FBW_MODE_MANUAL) {
-
-    /* if not in NAV_MODE set commands from the rc */
 #ifdef SetCommandsFromRC
     SetCommandsFromRC(commands, radio_control.values);
 #else
 #warning "FBW: needs commands from RC in order to be useful."
 #endif
-
   }
+
+  /* Formward radiocontrol to AP */
+  intermcu_on_rc_frame();
 }
 
+void autopilot_on_ap_command(void);
+void autopilot_on_ap_command(void)
+{
+
+}
 
 /** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */
 #ifndef RC_LOST_FBW_MODE
@@ -215,16 +217,15 @@ void autopilot_on_rc_frame(void)
 STATIC_INLINE void failsafe_check(void)
 {
   // Loose RC while using RC
-  if (radio_control.status == RC_REALLY_LOST &&
-      fbw_mode != FBW_MODE_AUTO) {
+  if ((radio_control.status == RC_REALLY_LOST) &&
+      (fbw_mode != FBW_MODE_AUTO)) {
     fbw_set_mode(RC_LOST_FBW_MODE);
   }
-/*  // Loose AP while using AP
-  if (intermcu_realy_lost && (fbw_mode == FBW_MODE_AUTO))
-  {
-     fbw_set_mode(AP_LOST_FBW_MODE);
+  // Loose AP while using AP
+  if ((inter_mcu.status == INTERMCU_LOST) &&
+      (fbw_mode == FBW_MODE_AUTO)) {
+    fbw_set_mode(AP_LOST_FBW_MODE);
   }
-*/
 }
 
 
@@ -238,7 +239,7 @@ STATIC_INLINE void main_event(void)
   RadioControlEvent(autopilot_on_rc_frame);
 
   // InterMCU
-  intermcu_event();
+  InterMcuEvent(autopilot_on_ap_command);
 
   // TODO Modules
   //modules_event_task();

sw/airborne/subsystems/intermcu.h

@@ -30,8 +30,9 @@
 
 
 void intermcu_init(void);
+void intermcu_on_rc_frame(void);
 void intermcu_periodic(void);
-void intermcu_event(void);
+void InterMcuEvent(void (*frame_handler)(void));
 
 #include "subsystems/commands.h"
 #include RADIO_CONTROL_TYPE_H
@@ -56,5 +57,20 @@ struct ap_state {
 };
 
 
+///////////////////////////////////////////////////////////////////////////////
+// InterMCU Watchdog
+
+#define INTERMCU_LOST_CNT 25  /* 50ms with a 512Hz timer */
+
+#define INTERMCU_OK          0
+#define INTERMCU_LOST        1
+
+struct InterMCU {
+  uint8_t status;
+  uint8_t time_since_last_frame;
+};
+
+extern struct InterMCU inter_mcu;
+
 
 #endif

sw/airborne/subsystems/intermcu/intermcu.c

@@ -363,31 +363,37 @@ void link_mcu_send(void)
 #endif
 
 #ifdef INTER_MCU_FBW
-// 60 Hz
-static uint8_t FivehundredHzCounter = 0;
 
-void intermcu_periodic(void)
+void intermcu_on_rc_frame(void)
 {
-  FivehundredHzCounter++;
-  if (FivehundredHzCounter >= 10) {
-    // 50.12 Hz
-    FivehundredHzCounter = 0;
-    //TODO
-    //inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
+  //TODO
+  //inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
 
-    InterMcuSend_INTERMCU_FBW(
-      fbw_state->ppm_cpt,
-      fbw_state->status,
-      fbw_state->nb_err,
-      fbw_state->vsupply,
-      fbw_state->current);
-    InterMcuSend_INTERMCU_RADIO(fbw_state->channels);
+  InterMcuSend_INTERMCU_FBW(
+    fbw_state->ppm_cpt,
+    fbw_state->status,
+    fbw_state->nb_err,
+    fbw_state->vsupply,
+    fbw_state->current);
+  InterMcuSend_INTERMCU_RADIO(fbw_state->channels);
 
-  }
 }
 #endif
 
-void intermcu_event(void)
+
+struct InterMCU inter_mcu;
+
+
+void intermcu_periodic(void)
+{
+  if (inter_mcu.time_since_last_frame >= INTERMCU_LOST_CNT) {
+    inter_mcu.status = INTERMCU_LOST;
+  } else {
+    inter_mcu.time_since_last_frame++;
+  }
+}
+
+void InterMcuEvent(void (*frame_handler)(void))
 {
   /* A message has been received */
   if (InterMcuLinkChAvailable()) {
@@ -396,6 +402,8 @@ void intermcu_event(void)
       if (intermcu_data.msg_available) {
         parse_mavpilot_msg();
         intermcu_data.msg_available = FALSE;
+        inter_mcu.time_since_last_frame = 0;
+        (*frame_handler)();
       }
     }
   }