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

@@ -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;
 }
 

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(uint8_t 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;
 }

ArduCopter/defines.h

@@ -290,6 +290,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
 
@@ -322,9 +323,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
@@ -425,4 +423,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

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();

ArduCopter/radio.pde

@@ -152,6 +152,7 @@ static void read_radio()
         // turn on throttle failsafe if no update from ppm encoder for 2 seconds
         if ((elapsed >= 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);
         }
     }

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 ) {
         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
         hal.scheduler->suspend_timer_procs();