From bc4cd41a332533e9a2d7392f07c73802c7f9c8c7 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 30 Dec 2012 12:08:25 +0900 Subject: [PATCH] ArduCopter: added ERR dataflash message Failsafe events changed to errors so they are more obvious. Errors recorded to dataflash for failure to init compass and optical flow sensor. Errors recorded for pwm failure. Resolved a compile error when dataflash logging is disabled. --- ArduCopter/AP_State.pde | 6 ----- ArduCopter/Log.pde | 59 +++++++++++++++++++++++++++++++++++++++++ ArduCopter/defines.h | 21 ++++++++++++--- ArduCopter/events.pde | 14 +++++++--- ArduCopter/radio.pde | 1 + ArduCopter/sensors.pde | 2 ++ 6 files changed, 90 insertions(+), 13 deletions(-) diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 46649c37a2..714235df4a 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -57,13 +57,10 @@ static void set_failsafe(bool mode) // We've regained radio contact // ---------------------------- failsafe_off_event(); - Log_Write_Event(DATA_FAILSAFE_OFF); - }else{ // We've lost radio contact // ------------------------ failsafe_on_event(); - Log_Write_Event(DATA_FAILSAFE_ON); } } } @@ -72,9 +69,6 @@ static void set_failsafe(bool mode) // --------------------------------------------- void set_low_battery(bool b) { - if(ap.low_battery != b && true == b){ - Log_Write_Event(DATA_LOW_BATTERY); - } ap.low_battery = b; } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 30521573a6..2f5f4c6dd0 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -1093,6 +1093,53 @@ static void Log_Read_Camera() (unsigned int)temp7); // 7 yaw in centidegrees } +// Write an error packet. Total length : 6 bytes +static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_ERROR_MSG); + + DataFlash.WriteByte(sub_system); // 1 sub system + DataFlash.WriteByte(error_code); // 2 error code + DataFlash.WriteByte(END_BYTE); +} + +// Read an error packet +static void Log_Read_Error() +{ + uint8_t sub_system = DataFlash.ReadByte(); // 1 sub system + uint8_t error_code = DataFlash.ReadByte(); // 2 error code + + cliSerial->print_P(PSTR("ERR, ")); + + // print subsystem + switch(sub_system) { + case ERROR_SUBSYSTEM_MAIN: + cliSerial->print_P(PSTR("MAIN")); + break; + case ERROR_SUBSYSTEM_RADIO: + cliSerial->print_P(PSTR("RADIO")); + break; + case ERROR_SUBSYSTEM_COMPASS: + cliSerial->print_P(PSTR("COM")); + break; + case ERROR_SUBSYSTEM_OPTFLOW: + cliSerial->print_P(PSTR("OF")); + break; + case ERROR_SUBSYSTEM_FAILSAFE: + cliSerial->print_P(PSTR("FS")); + break; + default: + cliSerial->printf_P(PSTR("%d"),(int)sub_system); // 1 sub system + break; + } + + // print error code + cliSerial->printf_P(PSTR(", %d\n"),(int)error_code); // 2 error code +} + + // Read the DataFlash log memory static void Log_Read(int16_t start_page, int16_t end_page) { @@ -1232,6 +1279,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page) case LOG_CAMERA_MSG: Log_Read_Camera(); break; + + case LOG_ERROR_MSG: + Log_Read_Error(); + break; } break; case 3: @@ -1263,6 +1314,8 @@ static void Log_Write_Mode(byte mode) { } static void Log_Write_Raw() { } +void print_latlon(BetterStream *s, int32_t lat_or_lon) { +} static void Log_Write_GPS() { } static void Log_Write_Current() { @@ -1271,6 +1324,8 @@ static void Log_Write_Iterm() { } static void Log_Write_Attitude() { } +static void Log_Write_INAV() { +} static void Log_Write_Data(uint8_t _index, float _data){ } static void Log_Write_Data(uint8_t _index, int32_t _data){ @@ -1295,6 +1350,10 @@ static void Log_Write_PID(int8_t pid_id, int32_t error, int32_t p, int32_t i, in } static void Log_Write_DMP() { } +static void Log_Write_Camera() { +} +static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) { +} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 87102b6193..abb485250d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -288,6 +288,7 @@ enum gcs_severity { #define LOG_DMP_MSG 0x10 #define LOG_INAV_MSG 0x11 #define LOG_CAMERA_MSG 0x12 +#define LOG_ERROR_MSG 0x13 #define LOG_INDEX_MSG 0xF0 #define MAX_NUM_LOGS 50 @@ -320,9 +321,6 @@ enum gcs_severity { #define DATA_INIT_SIMPLE_BEARING 9 #define DATA_ARMED 10 #define DATA_DISARMED 11 -#define DATA_FAILSAFE_ON 12 -#define DATA_FAILSAFE_OFF 13 -#define DATA_LOW_BATTERY 14 #define DATA_AUTO_ARMED 15 #define DATA_TAKEOFF 16 #define DATA_DID_REACH_ALT 17 @@ -436,4 +434,21 @@ enum gcs_severity { #define AP_BARO_BMP085 1 #define AP_BARO_MS5611 2 +// Error message sub systems and error codes +#define ERROR_SUBSYSTEM_MAIN 1 +#define ERROR_SUBSYSTEM_RADIO 2 +#define ERROR_SUBSYSTEM_COMPASS 3 +#define ERROR_SUBSYSTEM_OPTFLOW 4 +#define ERROR_SUBSYSTEM_FAILSAFE 5 +// general error codes +#define ERROR_CODE_ERROR_RESOLVED 0 +#define ERROR_CODE_FAILED_TO_INITIALISE 1 +// subsystem specific error codes -- radio +#define ERROR_CODE_RADIO_LATE_FRAME 2 +// subsystem specific error codes -- failsafe +#define ERROR_CODE_RADIO_FAILSAFE_THROTTLE 2 +#define ERROR_CODE_RADIO_FAILSAFE_BATTERY 3 + + + #endif // _DEFINES_H diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index e20de56724..06fd32d175 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -46,6 +46,10 @@ static void failsafe_on_event() } break; } + + // log the error to the dataflash + Log_Write_Error(ERROR_CODE_RADIO_FAILSAFE_THROTTLE, ERROR_CODE_RADIO_FAILSAFE_THROTTLE); + } // failsafe_off_event - respond to radio contact being regained @@ -53,15 +57,13 @@ static void failsafe_on_event() // or Stabilize or ACRO mode but with motors disarmed static void failsafe_off_event() { - // no need to do anything + // no need to do anything except log the error as resolved // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode + Log_Write_Error(ERROR_CODE_RADIO_FAILSAFE_THROTTLE, ERROR_CODE_ERROR_RESOLVED); } static void low_battery_event(void) { - // warn the ground station - gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!")); - // failsafe check if (g.failsafe_battery_enabled && !ap.low_battery && motors.armed()) { switch(control_mode) { @@ -91,6 +93,10 @@ static void low_battery_event(void) // set the low battery flag set_low_battery(true); + // warn the ground station and log to dataflash + gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!")); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE, ERROR_CODE_RADIO_FAILSAFE_BATTERY); + #if COPTER_LEDS == ENABLED if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only piezo_on(); diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 0f2bad5360..b39f3e6422 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -148,6 +148,7 @@ static void read_radio() }else{ // turn on throttle failsafe if no update from ppm encoder for 2 seconds if ((millis() - APM_RC.get_last_update() >= RADIO_FS_TIMEOUT_MS) && g.failsafe_throttle && motors.armed() && !ap.failsafe) { + Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME); set_failsafe(true); } } diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index cd03492ad4..6df654e728 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -65,6 +65,7 @@ static void init_compass() if (!compass.init() || !compass.read()) { // make sure we don't pass a broken compass to DCM cliSerial->println_P(PSTR("COMPASS INIT ERROR")); + Log_Write_Error(ERROR_SUBSYSTEM_COMPASS,ERROR_CODE_FAILED_TO_INITIALISE); return; } ahrs.set_compass(&compass); @@ -79,6 +80,7 @@ static void init_optflow() if( optflow.init(false, &timer_scheduler, &spi_semaphore, &spi3_semaphore) == false ) { g.optflow_enabled = false; cliSerial->print_P(PSTR("\nFailed to Init OptFlow ")); + Log_Write_Error(ERROR_SUBSYSTEM_OPTFLOW,ERROR_CODE_FAILED_TO_INITIALISE); }else{ // suspend timer while we set-up SPI communication timer_scheduler.suspend_timer();