Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Changes to match Copter

This commit is contained in:
Rustom Jehangir 2016-05-17 08:10:02 -07:00 committed by Andrew Tridgell
parent cf8e5f1337
commit e857f20784
10 changed files with 144 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
ArduSub/GCS_Mavlink.cpp
View File

@ -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:

3
ArduSub/Parameters.cpp
View File

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

5
ArduSub/Sub.h
View File

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

24
ArduSub/commands_logic.cpp
View File

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

2
ArduSub/control_autotune.cpp
View File

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

14
ArduSub/control_brake.cpp
View File

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

2
ArduSub/control_drift.cpp
View File

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

2
ArduSub/control_rtl.cpp
View File

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

4
ArduSub/defines.h
View File

@ -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)

8
ArduSub/land_detector.cpp
View File

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