OP-119 AHRS: Added error counters for the messages as well as changed an error on AHRS echo back for the HomeLocation message.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1344 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/AHRS/ahrs.c

@@ -656,7 +656,7 @@ void process_spi_request(void)
     lfsm_user_set_tx_v1 (&user_tx_v1);
     break;
   case OPAHRS_MSG_V1_REQ_NORTH:
-    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ALTITUDE);
+    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_NORTH);
     INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
     dump_spi_message(PIOS_COM_AUX, "N", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
     lfsm_user_set_tx_v1 (&user_tx_v1);

flight/OpenPilot/Makefile

@@ -232,6 +232,7 @@ SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
 SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
 SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
 SRC += $(STMSPDSRCDIR)/stm32f10x_iwdg.c
+SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
 SRC += $(STMSPDSRCDIR)/misc.c
 
 ## STM32 USB Library

flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c

@@ -124,6 +124,10 @@ int32_t AHRSCommsInitialize(void)
  */
 static void ahrscommsTask(void* parameters)
 {
+  enum opahrs_result result;
+  uint16_t attitude_errors = 0, attituderaw_errors = 0, 
+            position_errors = 0, home_errors = 0, altitude_errors = 0;
+  
   // Main task loop
   while (1) {
     struct opahrs_msg_v1 rsp;
@@ -157,17 +161,19 @@ static void ahrscommsTask(void* parameters)
       /* Update settings with latest value */
       AttitudeSettingsGet(&settings);
   
-      if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
+      if ((result = PIOS_OPAHRS_GetAttitude(&rsp)) == OPAHRS_RESULT_OK) {
         update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
       } else {
         /* Comms error */
+        attitude_errors++;
         break;
       }
 
-      if (PIOS_OPAHRS_GetAttitudeRaw(&rsp) == OPAHRS_RESULT_OK) {
+      if ((result = PIOS_OPAHRS_GetAttitudeRaw(&rsp)) == OPAHRS_RESULT_OK) {
 	      update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
       } else {
         /* Comms error */
+        attituderaw_errors++;
         break;
       }
 
@@ -175,10 +181,11 @@ static void ahrscommsTask(void* parameters)
         struct opahrs_msg_v1 req;
 
         load_altitude_actual(&(req.payload.user.v.req.altitude));
-        if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
+        if ((result = PIOS_OPAHRS_SetAltitudeActual(&req)) == OPAHRS_RESULT_OK) {
           AltitudeActualIsUpdatedFlag = false;
         } else {
           /* Comms error */
+          altitude_errors++;
           break;
         }
       }
@@ -187,10 +194,11 @@ static void ahrscommsTask(void* parameters)
         struct opahrs_msg_v1 req;
         
         load_position_actual(&(req.payload.user.v.req.gps));
-        if (PIOS_OPAHRS_SetPositionActual(&req) == OPAHRS_RESULT_OK) {
+        if ((result = PIOS_OPAHRS_SetPositionActual(&req)) == OPAHRS_RESULT_OK) {
           PositionActualIsUpdatedFlag = false;
         } else {
           /* Comms error */
+          position_errors++;
           break;
         }
       }
@@ -199,11 +207,12 @@ static void ahrscommsTask(void* parameters)
         struct opahrs_msg_v1 req;
         
         load_magnetic_north(&(req.payload.user.v.req.north));
-        if (PIOS_OPAHRS_SetMagNorth(&req) == OPAHRS_RESULT_OK) {
+        if ((result = PIOS_OPAHRS_SetMagNorth(&req)) == OPAHRS_RESULT_OK) {
           HomeLocationIsUpdatedFlag = false;
           AHRSKnowsHome = TRUE;
         } else {
           /* Comms error */
+          home_errors++;
           break;
         }
       }    

flight/OpenPilot/Modules/GPS/GPS.c

@@ -201,6 +201,10 @@ static void setHomeLocation(PositionActualData * gpsData)
   //WMM_Initialize(); // Set default values and constants
   // TODO: Extract time/date from GPS to seed this
   //WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, home.Be);
+  // Hard code flux until get this updated - will get wrong orientations
+  home.Be[0] = 1;
+  home.Be[1] = 0;
+  home.Be[2] = 0;
   
   HomeLocationSet(&home);
 }

flight/PiOS/Common/pios_opahrs.c

@@ -285,7 +285,7 @@ enum opahrs_result PIOS_OPAHRS_SetMagNorth(struct opahrs_msg_v1 *req)
     return rc;
   }
   
-  return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_GPS, &rsp);
+  return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_NORTH, &rsp);
 }
 
 enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)

flight/PiOS/STM32F10x/pios_wdg.c

@@ -35,6 +35,7 @@
 
 #include "pios.h"
 #include "stm32f10x_iwdg.h"
+#include "stm32f10x_dbgmcu.h"
 
 /** 
  * @brief Initialize the watchdog timer for a specified timeout
@@ -59,6 +60,7 @@ uint8_t PIOS_WDG_Init( uint8_t delayMs )
   delay = 60 / 16 * delayMs;
   if( delay > 0xfff )
     delay = 0xfff;
+  DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE);  // make the watchdog stop counting in debug mode
   IWDG_WriteAccessCmd( IWDG_WriteAccess_Enable );
   IWDG_SetPrescaler( IWDG_Prescaler_16 );
   IWDG_SetReload( delay );