Copter: fixes for Frsky_Telem API changes

ArduCopter/Copter.h

@@ -424,11 +424,6 @@ private:
     AP_Frsky_Telem frsky_telemetry;
 #endif
 
-    // Variables for extended status MAVLink messages
-    uint32_t control_sensors_present;
-    uint32_t control_sensors_enabled;
-    uint32_t control_sensors_health;
-
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;

ArduCopter/GCS_Mavlink.cpp

@@ -130,6 +130,10 @@ NOINLINE void Copter::send_limits_status(mavlink_channel_t chan)
 
 NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
 {
+    uint32_t control_sensors_present;
+    uint32_t control_sensors_enabled;
+    uint32_t control_sensors_health;
+
     // default sensors present
     control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
 
@@ -291,6 +295,12 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan)
         0, // comm drops in pkts,
         0, 0, 0, 0);
 
+#if FRSKY_TELEM_ENABLED == ENABLED
+    // give mask of error flags to Frsky_Telemetry
+    uint32_t sensors_error_flags = (control_sensors_health ^ control_sensors_enabled) & control_sensors_present;
+    frsky_telemetry.update_sensor_status_flags(sensors_error_flags);
+#endif
+    
 }
 
 void NOINLINE Copter::send_location(mavlink_channel_t chan)

ArduCopter/flight_mode.cpp

@@ -136,6 +136,11 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
         // but it should be harmless to disable the fence temporarily in these situations as well
         fence.manual_recovery_start();
 #endif
+        
+#if FRSKY_TELEM_ENABLED == ENABLED
+        frsky_telemetry.update_control_mode(control_mode);
+#endif
+        
     }else{
         // Log error that we failed to enter desired flight mode
         Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);

ArduCopter/system.cpp

@@ -171,7 +171,7 @@ void Copter::init_ardupilot()
 #else
   #error Unrecognised frame type
 #endif
-    &g.fs_batt_voltage, &g.fs_batt_mah, (uint8_t *)&control_mode, &ap.value, &control_sensors_present, &control_sensors_enabled, &control_sensors_health, &home_distance, &home_bearing);
+    &g.fs_batt_voltage, &g.fs_batt_mah, &ap.value, &home_distance, &home_bearing);
 #endif
 
     // identify ourselves correctly with the ground station