diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 175e351ea5..153acb04c8 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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: diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index c128852712..9bc3ead61f 100644 --- a/ArduSub/Parameters.cpp +++ b/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 diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 21178c15e6..d622b207db 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 876f575fbf..3bafc831b5 100644 --- a/ArduSub/commands_logic.cpp +++ b/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 diff --git a/ArduSub/control_autotune.cpp b/ArduSub/control_autotune.cpp index 6c7cfd75bd..2b4cf444a3 100644 --- a/ArduSub/control_autotune.cpp +++ b/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 diff --git a/ArduSub/control_brake.cpp b/ArduSub/control_brake.cpp index 43f44b13b0..b7f90df4bd 100644 --- a/ArduSub/control_brake.cpp +++ b/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; } diff --git a/ArduSub/control_drift.cpp b/ArduSub/control_drift.cpp index e422bab9e4..5ee48a7f9a 100644 --- a/ArduSub/control_drift.cpp +++ b/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 diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index c94ef8dbcb..7871937446 100644 --- a/ArduSub/control_rtl.cpp +++ b/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); } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 0b823e97fc..9c46d1ac28 100644 --- a/ArduSub/defines.h +++ b/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) diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp index f91acf069e..4d2fba0507 100644 --- a/ArduSub/land_detector.cpp +++ b/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