Sub: Changes to match recent Copter updates.

ArduSub/GCS_Mavlink.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+
+class GCS_MAVLINK_Sub : public GCS_MAVLINK
+{
+
+public:
+
+    void data_stream_send(void) override;
+
+protected:
+
+    uint32_t telem_delay() const override;
+
+private:
+
+    void handleMessage(mavlink_message_t * msg) override;
+    bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
+    void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
+    bool try_send_message(enum ap_message id) override;
+
+};

ArduSub/Log.cpp

@@ -215,7 +215,7 @@ void Sub::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
 // Write a Current data packet
 void Sub::Log_Write_Current()
 {
-    DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle()));
+    DataFlash.Log_Write_Current(battery);
 
     // also write power status
     DataFlash.Log_Write_Power();

ArduSub/Parameters.cpp

@@ -146,7 +146,7 @@ const AP_Param::Info Sub::var_info[] = {
 	// @Increment: .1
 	// @Values: 0:Disabled,1:Shallow,3:Steep
 	// @User: Standard
-	GSCALAR(rtl_cone_slope,   "RTL_CONE_SLOPE",     RTL_CONE_SLOPE),
+	GSCALAR(rtl_cone_slope,   "RTL_CONE_SLOPE",     RTL_CONE_SLOPE_DEFAULT),
 
     // @Param: RTL_SPEED
     // @DisplayName: RTL speed
@@ -1128,7 +1128,10 @@ void Sub::convert_pid_parameters(void)
         { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" },
         { Parameters::k_param_pid_rate_yaw,   5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }
     };
-    AP_Param::ConversionInfo filt_conversion_info[] = {
+    AP_Param::ConversionInfo angle_and_filt_conversion_info[] = {
+        { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" },
+        { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" },
+        { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" },
         { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" },
         { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" },
         { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }
@@ -1153,9 +1156,9 @@ void Sub::convert_pid_parameters(void)
     for (uint8_t i=0; i<table_size; i++) {
         AP_Param::convert_old_parameter(&imax_conversion_info[i], 1.0f/4500.0f);
     }
-    // convert filter without scaling
-    table_size = ARRAY_SIZE(filt_conversion_info);
+    // convert angle controller gain and filter without scaling
+    table_size = ARRAY_SIZE(angle_and_filt_conversion_info);
     for (uint8_t i=0; i<table_size; i++) {
-        AP_Param::convert_old_parameter(&filt_conversion_info[i], 1.0f);
+    	AP_Param::convert_old_parameter(&angle_and_filt_conversion_info[i], 1.0f);
     }
 }

ArduSub/Parameters.h

@@ -53,7 +53,8 @@ public:
         k_param_software_type,
         k_param_ins_old,                        // *** Deprecated, remove with next eeprom number change
         k_param_ins,                            // libraries/AP_InertialSensor variables
-        k_param_NavEKF2,
+		k_param_NavEKF2_old, // deprecated
+		k_param_NavEKF2,
 
         // simulation
         k_param_sitl = 10,

ArduSub/Sub.h

@@ -42,7 +42,6 @@
 
 // Application dependencies
 #include <GCS_MAVLink/GCS.h>
-#include <GCS_MAVLink/GCS_MAVLink.h>        // MAVLink GCS definitions
 #include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
 #include <AP_GPS/AP_GPS.h>             // ArduPilot GPS library
 #include <DataFlash/DataFlash.h>          // ArduPilot Mega Flash Memory Library
@@ -96,6 +95,8 @@
 #include "defines.h"
 #include "config.h"
 
+#include "GCS_Mavlink.h"
+
 // libraries which are dependent on #defines in defines.h and/or config.h
 #if SPRAYER == ENABLED
 #include <AC_Sprayer/AC_Sprayer.h>         // crop sprayer library
@@ -120,7 +121,7 @@
 
 class Sub : public AP_HAL::HAL::Callbacks {
 public:
-    friend class GCS_MAVLINK;
+    friend class GCS_MAVLINK_Sub;
     friend class Parameters;
 
     Sub(void);
@@ -214,7 +215,7 @@ private:
     AP_SerialManager serial_manager;
     static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
 
-    GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
+    GCS_MAVLINK_Sub gcs[MAVLINK_COMM_NUM_BUFFERS];
 
     // User variables
 #ifdef USERHOOK_VARIABLES
@@ -640,7 +641,6 @@ private:
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();
     void send_pid_tuning(mavlink_channel_t chan);
-    bool telemetry_delayed(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
     void gcs_send_mission_item_reached_message(uint16_t mission_index);
     void gcs_data_stream_send(void);
@@ -918,7 +918,6 @@ private:
     uint16_t perf_info_get_num_long_running();
     uint32_t perf_info_get_num_dropped();
     Vector3f pv_location_to_vector(const Location& loc);
-    Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec);
     float pv_alt_above_origin(float alt_above_home_cm);
     float pv_alt_above_home(float alt_above_origin_cm);
     float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);

ArduSub/commands.cpp

@@ -137,6 +137,13 @@ void Sub::set_system_time_from_GPS()
     if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         // set system clock for log timestamps
         hal.util->set_system_clock(gps.time_epoch_usec());
+        uint64_t gps_timestamp = gps.time_epoch_usec();
+
+        hal.util->set_system_clock(gps_timestamp);
+
+        // update signing timestamp
+        GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
+
         ap.system_time_set = true;
         Log_Write_Event(DATA_SYSTEM_TIME_SET);
     }

ArduSub/config.h

@@ -221,7 +221,7 @@
  #ifndef COMPASS_OFFSETS_MAX
   # define COMPASS_OFFSETS_MAX          600         // PX4 onboard compass has high offsets
  #endif
-#else   // SITL, FLYMAPLE, etc
+#else   // SITL, etc
  #ifndef COMPASS_OFFSETS_MAX
   # define COMPASS_OFFSETS_MAX          500
  #endif
@@ -417,8 +417,8 @@
  # define RTL_ABS_MIN_CLIMB         250     // absolute minimum initial climb
 #endif
 
-#ifndef RTL_CONE_SLOPE
- # define RTL_CONE_SLOPE            3.0f    // slope of RTL cone (height / distance). 0 = No cone
+#ifndef RTL_CONE_SLOPE_DEFAULT
+ # define RTL_CONE_SLOPE_DEFAULT    3.0f    // slope of RTL cone (height / distance). 0 = No cone
 #endif
 
 #ifndef RTL_MIN_CONE_SLOPE

ArduSub/control_flip.cpp

@@ -20,8 +20,8 @@
  *          Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
  */
 
-#define FLIP_THR_INC        200     // throttle increase during Flip_Start stage (under 45deg lean angle)
-#define FLIP_THR_DEC        240     // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
+#define FLIP_THR_INC        0.20f   // throttle increase during Flip_Start stage (under 45deg lean angle)
+#define FLIP_THR_DEC        0.24f   // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
 #define FLIP_ROTATION_RATE  40000   // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
 #define FLIP_TIMEOUT_MS     2500    // timeout after 2.5sec.  Vehicle will switch back to original flight mode
 #define FLIP_RECOVERY_ANGLE 500     // consider successful recovery when roll is back within 5 degrees of original
@@ -34,7 +34,6 @@
 
 FlipState flip_state;               // current state of flip
 control_mode_t   flip_orig_control_mode;   // flight mode when flip was initated
-mode_reason_t	 flip_orig_control_mode_reason;
 uint32_t  flip_start_time;          // time since flip began
 int8_t    flip_roll_dir;            // roll direction (-1 = roll left, 1 = roll right)
 int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
@@ -64,7 +63,6 @@ bool Sub::flip_init(bool ignore_checks)
 
     // capture original flight mode so that we can return to it after completion
     flip_orig_control_mode = control_mode;
-    flip_orig_control_mode_reason = control_mode_reason;
 
     // initialise state
     flip_state = Flip_Start;
@@ -145,9 +143,7 @@ void Sub::flip_run()
         // between 45deg ~ -90deg request 400deg/sec roll
         attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
         // decrease throttle
-        if (throttle_out >= g.throttle_min) {
-            throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
-        }
+        throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 
         // beyond -90deg move on to recovery
         if ((flip_angle < 4500) && (flip_angle > -9000)) {
@@ -157,11 +153,9 @@ void Sub::flip_run()
 
     case Flip_Pitch_A:
         // between 45deg ~ -90deg request 400deg/sec pitch
-        attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
+    	attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
         // decrease throttle
-        if (throttle_out >= g.throttle_min) {
-            throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
-        }
+    	throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 
         // check roll for inversion
         if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
@@ -173,9 +167,7 @@ void Sub::flip_run()
         // between 45deg ~ -90deg request 400deg/sec pitch
         attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
         // decrease throttle
-        if (throttle_out >= g.throttle_min) {
-            throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
-        }
+        throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 
         // check roll for inversion
         if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
@@ -201,7 +193,7 @@ void Sub::flip_run()
         // check for successful recovery
         if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
             // restore original flight mode
-            if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
+            if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
                 // this should never happen but just in case
                 set_mode(STABILIZE, MODE_REASON_UNKNOWN);
             }
@@ -212,7 +204,7 @@ void Sub::flip_run()
 
     case Flip_Abandon:
         // restore original flight mode
-        if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
+        if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
             // this should never happen but just in case
             set_mode(STABILIZE, MODE_REASON_UNKNOWN);
         }
@@ -222,9 +214,5 @@ void Sub::flip_run()
     }
 
     // output pilot's throttle without angle boost
-    if (is_zero(throttle_out)) {
-        attitude_control.set_throttle_out_unstabilized(0,false,g.throttle_filt);
-    } else {
-        attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
-    }
+    attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
 }

ArduSub/defines.h

@@ -128,7 +128,8 @@ enum mode_reason_t {
     MODE_REASON_THROTTLE_LAND_ESCAPE,
     MODE_REASON_FENCE_BREACH,
 	MODE_REASON_TERRAIN_FAILSAFE,
-	MODE_REASON_BRAKE_TIMEOUT
+	MODE_REASON_BRAKE_TIMEOUT,
+	MODE_REASON_FLIP_COMPLETE
 };
 
 // Tuning enumeration

ArduSub/motors.cpp

@@ -194,9 +194,6 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
     sprayer.test_pump(false);
 #endif
 
-    // short delay to allow reading of rc inputs
-    delay(30);
-
     // enable output to motors
     enable_motor_output();
 

ArduSub/position_vector.cpp

@@ -17,26 +17,6 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
     return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
 }
 
-// pv_location_to_vector_with_default - convert lat/lon coordinates to a position vector,
-//     defaults to the default_posvec's lat/lon and alt if the provided lat/lon or alt are zero
-Vector3f Sub::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec)
-{
-    Vector3f pos = pv_location_to_vector(loc);
-
-    // set target altitude to default if not provided
-    if (loc.alt == 0) {
-        pos.z = default_posvec.z;
-    }
-
-    // set target position to default if not provided
-    if (loc.lat == 0 && loc.lng == 0) {
-        pos.x = default_posvec.x;
-        pos.y = default_posvec.y;
-    }
-
-    return pos;
-}
-
 // pv_alt_above_origin - convert altitude above home to altitude above EKF origin
 float Sub::pv_alt_above_origin(float alt_above_home_cm)
 {

ArduSub/system.cpp

@@ -138,17 +138,10 @@ void Sub::init_ardupilot()
     ap.usb_connected = true;
     check_usb_mux();
 
-    // init the GCS connected to the console
-    gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
-
-    // init telemetry port
-    gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
-
-    // setup serial port for telem2
-    gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
-
-    // setup serial port for fourth telemetry port (not used by default)
-    gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
+    // setup telem slots with serial ports
+    for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
+        gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
+    }
 
 #if FRSKY_TELEM_ENABLED == ENABLED
     // setup frsky