Sub: Changes to match Copter

ArduSub/GCS_Mavlink.cpp

@@ -528,7 +528,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
     switch(id) {
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
-        sub.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis();
+        last_heartbeat_time = AP_HAL::millis();
         sub.send_heartbeat(chan);
         break;
 
@@ -539,13 +539,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
             CHECK_PAYLOAD_SIZE(SYS_STATUS);
             sub.send_extended_status1(chan);
             CHECK_PAYLOAD_SIZE(POWER_STATUS);
-            sub.gcs[chan-MAVLINK_COMM_0].send_power_status();
+            send_power_status();
         }
         break;
 
     case MSG_EXTENDED_STATUS2:
         CHECK_PAYLOAD_SIZE(MEMINFO);
-        sub.gcs[chan-MAVLINK_COMM_0].send_meminfo();
+        send_meminfo();
         break;
 
     case MSG_ATTITUDE:
@@ -569,11 +569,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         break;
 
     case MSG_GPS_RAW:
-        return sub.gcs[chan-MAVLINK_COMM_0].send_gps_raw(sub.gps);
+        send_gps_raw(sub.gps);
 
     case MSG_SYSTEM_TIME:
         CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
-        sub.gcs[chan-MAVLINK_COMM_0].send_system_time(sub.gps);
+        send_system_time(sub.gps);
         break;
 
     case MSG_SERVO_OUT:
@@ -583,7 +583,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_RADIO_IN:
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
-        sub.gcs[chan-MAVLINK_COMM_0].send_radio_in(sub.receiver_rssi);
+        send_radio_in(sub.receiver_rssi);
         break;
 
     case MSG_RADIO_OUT:
@@ -598,17 +598,17 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_RAW_IMU1:
         CHECK_PAYLOAD_SIZE(RAW_IMU);
-        sub.gcs[chan-MAVLINK_COMM_0].send_raw_imu(sub.ins, sub.compass);
+        send_raw_imu(sub.ins, sub.compass);
         break;
 
     case MSG_RAW_IMU2:
         CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
-        sub.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(sub.barometer);
+        send_scaled_pressure(sub.barometer);
         break;
 
     case MSG_RAW_IMU3:
         CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
-        sub.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(sub.ins, sub.compass, sub.barometer);
+        send_sensor_offsets(sub.ins, sub.compass, sub.barometer);
         break;
 
     case MSG_CURRENT_WAYPOINT:
@@ -618,12 +618,12 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_NEXT_PARAM:
         CHECK_PAYLOAD_SIZE(PARAM_VALUE);
-        sub.gcs[chan-MAVLINK_COMM_0].queued_param_send();
+        queued_param_send();
         break;
 
     case MSG_NEXT_WAYPOINT:
         CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
-        sub.gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
+        queued_waypoint_send();
         break;
 
     case MSG_RANGEFINDER:
@@ -665,7 +665,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_AHRS:
         CHECK_PAYLOAD_SIZE(AHRS);
-        sub.gcs[chan-MAVLINK_COMM_0].send_ahrs(sub.ahrs);
+        send_ahrs(sub.ahrs);
         break;
 
     case MSG_SIMSTATE:
@@ -674,7 +674,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
         sub.send_simstate(chan);
 #endif
         CHECK_PAYLOAD_SIZE(AHRS2);
-        sub.gcs[chan-MAVLINK_COMM_0].send_ahrs2(sub.ahrs);
+        send_ahrs2(sub.ahrs);
         break;
 
     case MSG_HWSTATUS:
@@ -691,13 +691,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 
     case MSG_BATTERY2:
         CHECK_PAYLOAD_SIZE(BATTERY2);
-        sub.gcs[chan-MAVLINK_COMM_0].send_battery2(sub.battery);
+        send_battery2(sub.battery);
         break;
 
     case MSG_OPTICAL_FLOW:
 #if OPTFLOW == ENABLED
         CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
-        sub.gcs[chan-MAVLINK_COMM_0].send_opticalflow(sub.ahrs, sub.optflow);
+        send_opticalflow(sub.ahrs, sub.optflow);
 #endif
         break;
 
@@ -1574,7 +1574,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
             if (is_equal(packet.param1,1.0f)) {
-                sub.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
+                send_autopilot_version(FIRMWARE_VERSION);
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1607,6 +1607,76 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+        /* Solo user presses Fly button */
+		case MAV_CMD_SOLO_BTN_FLY_CLICK: {
+			result = MAV_RESULT_ACCEPTED;
+
+			if (sub.failsafe.radio) {
+				break;
+			}
+
+			// set mode to Loiter or fall back to AltHold
+			if (!sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
+				sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
+			}
+			break;
+		}
+
+		/* Solo user holds down Fly button for a couple of seconds */
+		case MAV_CMD_SOLO_BTN_FLY_HOLD: {
+			result = MAV_RESULT_ACCEPTED;
+
+			if (sub.failsafe.radio) {
+				break;
+			}
+
+			if (!sub.motors.armed()) {
+				// if disarmed, arm motors
+				sub.init_arm_motors(true);
+			} else if (sub.ap.land_complete) {
+				// if armed and landed, takeoff
+				if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
+					sub.do_user_takeoff(packet.param1*100, true);
+				}
+			} else {
+				// if flying, land
+				sub.set_mode(LAND, MODE_REASON_GCS_COMMAND);
+			}
+			break;
+		}
+
+		/* Solo user presses pause button */
+		case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
+			result = MAV_RESULT_ACCEPTED;
+
+			if (sub.failsafe.radio) {
+				break;
+			}
+
+			if (sub.motors.armed()) {
+				if (sub.ap.land_complete) {
+					// if landed, disarm motors
+					sub.init_disarm_motors();
+				} else {
+					// assume that shots modes are all done in guided.
+					// NOTE: this may need to change if we add a non-guided shot mode
+					bool shot_mode = (!is_zero(packet.param1) && sub.control_mode == GUIDED);
+
+					if (!shot_mode) {
+						if (sub.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {
+							sub.brake_timeout_to_loiter_ms(2500);
+						} else {
+							sub.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
+						}
+					} else {
+						// SoloLink is expected to handle pause in shots
+					}
+				}
+			}
+			break;
+		}
+
+
         default:
             result = MAV_RESULT_UNSUPPORTED;
             break;
@@ -1970,7 +2040,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
 
     case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
-        sub.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION);
+        send_autopilot_version(FIRMWARE_VERSION);
         break;
 
     case MAVLINK_MSG_ID_LED_CONTROL:

ArduSub/Parameters.cpp

@@ -105,7 +105,8 @@ const AP_Param::Info Sub::var_info[] = {
     // @DisplayName: Throttle stick behavior
     // @Description: Bits for: Feedback starts from mid stick
     // @User: Standard
-    // @Values: 0:None,1:FeedbackFromMid
+	// @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection
+	// @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection
     GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),
 
     // @Group: SERIAL

ArduSub/Sub.h

@@ -391,6 +391,10 @@ private:
     uint16_t loiter_time_max;                // How long we should stay in Loiter Mode for mission scripting (time in seconds)
     uint32_t loiter_time;                    // How long have we been loitering - The start time in millis
 
+    // Brake
+    uint32_t brake_timeout_start;
+    uint32_t brake_timeout_ms;
+
     // Flip
     Vector3f flip_orig_attitude;         // original Sub attitude before flip
 
@@ -748,6 +752,7 @@ private:
     void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
     bool brake_init(bool ignore_checks);
     void brake_run();
+    void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
     bool circle_init(bool ignore_checks);
     void circle_run();
     bool drift_init(bool ignore_checks);

ArduSub/commands_logic.cpp

@@ -283,13 +283,31 @@ void Sub::do_takeoff(const AP_Mission::Mission_Command& cmd)
 // do_nav_wp - initiate move to next waypoint
 void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
+	Location_Class target_loc(cmd.content.location);
+    // use current lat, lon if zero
+    if (target_loc.lat == 0 && target_loc.lng == 0) {
+        target_loc.lat = current_loc.lat;
+        target_loc.lng = current_loc.lng;
+    }
+    // use current altitude if not provided
+    if (target_loc.alt == 0) {
+        // set to current altitude but in command's alt frame
+        int32_t curr_alt;
+        if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
+            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
+        } else {
+            // default to current altitude as alt-above-home
+            target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
+        }
+    }
+
     // this will be used to remember the time in millis after we reach or pass the WP.
     loiter_time = 0;
     // this is the delay, stored in seconds
     loiter_time_max = cmd.p1;
 
     // Set wp navigation target
-    auto_wp_start(Location_Class(cmd.content.location));
+    auto_wp_start(target_loc);
 
     // if no delay set the waypoint as "fast"
     if (loiter_time_max == 0 ) {
@@ -344,7 +362,9 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
         // To-Do: make this simpler
         Vector3f temp_pos;
         wp_nav.get_wp_stopping_point_xy(temp_pos);
-        target_loc.offset(temp_pos.x * 100.0f, temp_pos.y * 100.0f);
+        Location_Class temp_loc(temp_pos);
+        target_loc.lat = temp_loc.lat;
+        target_loc.lng = temp_loc.lng;
     }
 
     // use current altitude if not provided

ArduSub/control_autotune.cpp

@@ -307,7 +307,7 @@ void Copter::autotune_run()
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
     }else{
         // check if pilot is overriding the controls
-        if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || !is_zero(target_climb_rate)) {
+        if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || target_climb_rate != 0) {
             if (!autotune_state.pilot_override) {
                 autotune_state.pilot_override = true;
                 // set gains to their original values

ArduSub/control_brake.cpp

@@ -25,6 +25,8 @@ bool Sub::brake_init(bool ignore_checks)
         pos_control.set_alt_target(inertial_nav.get_altitude());
         pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
+        brake_timeout_ms = 0;
+
         return true;
     }else{
         return false;
@@ -70,4 +72,16 @@ void Sub::brake_run()
     // update altitude target and call position controller
     pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
     pos_control.update_z_controller();
+
+    if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) {
+        if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
+            set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
+        }
+    }
+}
+
+void Sub::brake_timeout_to_loiter_ms(uint32_t timeout_ms)
+{
+    brake_timeout_start = millis();
+    brake_timeout_ms = timeout_ms;
 }

ArduSub/control_drift.cpp

@@ -14,7 +14,7 @@
 #endif
 
 #ifndef DRIFT_THR_ASSIST_GAIN
- # define DRIFT_THR_ASSIST_GAIN 1.8f    // gain controlling amount of throttle assistance
+ # define DRIFT_THR_ASSIST_GAIN 0.0018f    // gain controlling amount of throttle assistance
 #endif
 
 #ifndef DRIFT_THR_ASSIST_MAX

ArduSub/control_rtl.cpp

@@ -97,7 +97,7 @@ void Sub::rtl_climb_start()
     wp_nav.wp_and_spline_init();
 
     // RTL_SPEED == 0 means use WPNAV_SPEED
-    if (!is_zero(g.rtl_speed_cms)) {
+    if (g.rtl_speed_cms != 0) {
         wp_nav.set_speed_xy(g.rtl_speed_cms);
     }
 

ArduSub/defines.h

@@ -127,7 +127,8 @@ enum mode_reason_t {
     MODE_REASON_MISSION_END,
     MODE_REASON_THROTTLE_LAND_ESCAPE,
     MODE_REASON_FENCE_BREACH,
-	MODE_REASON_TERRAIN_FAILSAFE
+	MODE_REASON_TERRAIN_FAILSAFE,
+	MODE_REASON_BRAKE_TIMEOUT
 };
 
 // Tuning enumeration
@@ -474,3 +475,4 @@ enum ThrowModeState {
 // for PILOT_THR_BHV parameter
 #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
 #define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)
+#define THR_BEHAVE_DISARM_ON_LAND_DETECT (1<<2)

ArduSub/land_detector.cpp

@@ -45,6 +45,14 @@ void Sub::set_land_complete(bool b)
         Log_Write_Event(DATA_NOT_LANDED);
     }
     ap.land_complete = b;
+
+    // trigger disarm-on-land if configured
+    bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
+    bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
+
+    if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) {
+        init_disarm_motors();
+    }
 }
 
 // update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state