From 2031cfe4d0ba01ceb0930043e920ec07eea1f4bd Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 20 Aug 2010 22:52:13 +0000 Subject: [PATCH] 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 | 2 +- flight/OpenPilot/Makefile | 1 + .../OpenPilot/Modules/AHRSComms/ahrs_comms.c | 19 ++++++++++++++----- flight/OpenPilot/Modules/GPS/GPS.c | 4 ++++ flight/PiOS/Common/pios_opahrs.c | 2 +- flight/PiOS/STM32F10x/pios_wdg.c | 2 ++ 6 files changed, 23 insertions(+), 7 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index a6949d19d..b97da0a66 100644 --- a/flight/AHRS/ahrs.c +++ b/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); diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 2dd7ca225..71c8d5e58 100644 --- a/flight/OpenPilot/Makefile +++ b/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 diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index 56a5ec555..e3f076204 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/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; } } diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c index e84d2a54a..a4f0b77d6 100644 --- a/flight/OpenPilot/Modules/GPS/GPS.c +++ b/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); } diff --git a/flight/PiOS/Common/pios_opahrs.c b/flight/PiOS/Common/pios_opahrs.c index 791a8a41e..ce1098df7 100644 --- a/flight/PiOS/Common/pios_opahrs.c +++ b/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) diff --git a/flight/PiOS/STM32F10x/pios_wdg.c b/flight/PiOS/STM32F10x/pios_wdg.c index ca8299994..3b9e59f1c 100644 --- a/flight/PiOS/STM32F10x/pios_wdg.c +++ b/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 );