Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 18:03:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: replace land with surface mode

Browse Source
This commit is contained in:
Jacob Walser 2016-08-24 23:08:30 -04:00 committed by Andrew Tridgell
parent e1bf217422
commit e2a7ba5b0c
12 changed files with 256 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -396,7 +396,7 @@ void Sub::ten_hz_logging_loop()
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) {
if (should_log(MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) {
Log_Write_Nav_Tuning();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {

57
ArduSub/GCS_Mavlink.cpp
View File

@ -170,7 +170,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
case LOITER:
case RTL:
case CIRCLE:
case LAND:
case SURFACE:
case OF_LOITER:
case POSHOLD:
case BRAKE:
@ -1278,11 +1278,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
case MAV_CMD_NAV_LAND:
if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// Not supported in sub
// case MAV_CMD_NAV_LAND:
// if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
// result = MAV_RESULT_ACCEPTED;
// }
// break;
case MAV_CMD_CONDITION_YAW:
// param1 : target angle [0-360]
@ -1652,28 +1653,30 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
/* Solo user holds down Fly button for a couple of seconds */
case MAV_CMD_SOLO_BTN_FLY_HOLD: {
result = MAV_RESULT_ACCEPTED;
// Not supported in Sub
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 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: {

12
ArduSub/Sub.h
View File

@ -791,16 +791,7 @@ private:
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
void guided_limit_init_time_and_pos();
bool guided_limit_check();
bool land_init(bool ignore_checks);
void land_run();
void land_gps_run();
void land_nogps_run();
void land_run_vertical_control(bool pause_descent = false);
void land_run_horizontal_control();
float get_land_descent_speed();
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();
bool loiter_init(bool ignore_checks);
void loiter_run();
bool poshold_init(bool ignore_checks);
@ -1054,6 +1045,9 @@ private:
void set_leak_status(bool status);
bool surface_init(bool ignore_flags);
void surface_run();
public:
void mavlink_delay_cb();
void failsafe_check();

19
ArduSub/commands_logic.cpp
View File

@ -246,23 +246,8 @@ void Sub::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land
if(!ap.land_complete) {
// try to enter loiter but if that fails land
if(!auto_loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END);
}
}else{
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.throttle_zero || failsafe.radio) {
init_disarm_motors();
}
#else
// if we've landed it's safe to disarm
init_disarm_motors();
#endif
}
init_disarm_motors();
}
/********************************************************************************/

64
ArduSub/control_surface.cpp Normal file
View File

@ -0,0 +1,64 @@
#include "Sub.h"
bool Sub::surface_init(bool ignore_checks)
{
if(!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately.
gcs_send_text(MAV_SEVERITY_WARNING, "Surface mode requires external pressure sensor.");
return false;
}
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
return true;
}
void Sub::surface_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
// if not armed set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock()) {
motors.output_min();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
return;
}
// Already at surface, hold depth at surface
if(ap.at_surface)
set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE);
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// set target climb rate
float cmb_rate = constrain_float(abs(g.land_speed), 1, pos_control.get_speed_up());
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// pilot has control for repositioning
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}

5
ArduSub/defines.h
View File

@ -105,7 +105,7 @@ enum control_mode_t {
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
SURFACE = 9, // automatic landing with horizontal position control
OF_LOITER = 10, // deprecated
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
@ -131,7 +131,8 @@ enum mode_reason_t {
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_SURFACE_COMPLETE
};
// Tuning enumeration

72
ArduSub/ekf_check.cpp
View File

@ -118,42 +118,42 @@ bool Sub::ekf_over_threshold()
// failsafe_ekf_event - perform ekf failsafe
void Sub::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
return;
}
// do nothing if motors disarmed
if (!motors.armed()) {
return;
}
// do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
return;
}
// EKF failsafe event has occurred
failsafe.ekf = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
// take action based on fs_ekf_action parameter
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
}
break;
default:
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
break;
}
// if flight mode is already LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
}
// // return immediately if ekf failsafe already triggered
// if (failsafe.ekf) {
// return;
// }
//
// // do nothing if motors disarmed
// if (!motors.armed()) {
// return;
// }
//
// // do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
// if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
// return;
// }
//
// // EKF failsafe event has occurred
// failsafe.ekf = true;
// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
//
// // take action based on fs_ekf_action parameter
// switch (g.fs_ekf_action) {
// case FS_EKF_ACTION_ALTHOLD:
// // AltHold
// if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
// }
// break;
// default:
// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
// break;
// }
//
// // if flight mode is already LAND ensure it's not the GPS controlled LAND
// if (control_mode == LAND) {
// land_do_not_use_GPS();
// }
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared

110
ArduSub/events.cpp
View File

@ -8,29 +8,29 @@
*/
void Sub::failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
}
// log the error to the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
// // if motors are not armed there is nothing to do
// if( !motors.armed() ) {
// return;
// }
//
// if (should_disarm_on_failsafe()) {
// init_disarm_motors();
// } else {
// if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// // continue mission
// } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// // continue landing
// } else {
// if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
// } else {
// set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
// }
// }
// }
//
// // log the error to the dataflash
// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
}
@ -46,30 +46,30 @@ void Sub::failsafe_radio_off_event()
void Sub::failsafe_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else {
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
}
// set the low battery flag
set_failsafe_battery(true);
// warn the ground station and log to dataflash
gcs_send_text(MAV_SEVERITY_WARNING,"Low battery");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
// // return immediately if low battery event has already been triggered
// if (failsafe.battery) {
// return;
// }
//
// // failsafe check
// if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
// if (should_disarm_on_failsafe()) {
// init_disarm_motors();
// } else {
// if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
// set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
// } else {
// set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
// }
// }
// }
//
// // set the low battery flag
// set_failsafe_battery(true);
//
// // warn the ground station and log to dataflash
// gcs_send_text(MAV_SEVERITY_WARNING,"Low battery");
// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
}
@ -195,14 +195,14 @@ void Sub::failsafe_terrain_on_event()
// this is always called from a failsafe so we trigger notification to pilot
void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
{
// attempt to switch to RTL, if this fails then switch to Land
if (!set_mode(RTL, reason)) {
// set mode to land will trigger mode change notification to pilot
set_mode_land_with_pause(reason);
} else {
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
// // attempt to switch to RTL, if this fails then switch to Land
// if (!set_mode(RTL, reason)) {
// // set mode to land will trigger mode change notification to pilot
// set_mode_land_with_pause(reason);
// } else {
// // alert pilot to mode change
// AP_Notify::events.failsafe_mode_change = 1;
// }
}
bool Sub::should_disarm_on_failsafe() {

32
ArduSub/fence.cpp
View File

@ -29,22 +29,22 @@ void Sub::fence_check()
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
}else{
// if we are within 100m of the fence, RTL
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}else{
// if more than 100m outside the fence just force a land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}
//
// // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
// if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
// init_disarm_motors();
// }else{
// // if we are within 100m of the fence, RTL
// if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
// if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
// set_mode(LAND, MODE_REASON_FENCE_BREACH);
// }
// }else{
// // if more than 100m outside the fence just force a land
// set_mode(LAND, MODE_REASON_FENCE_BREACH);
// }
// }
}
// log an error in the dataflash

14
ArduSub/flight_mode.cpp
View File

@ -55,8 +55,8 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
case SURFACE:
success = surface_init(ignore_checks);
break;
case RTL:
@ -172,8 +172,8 @@ void Sub::update_flight_mode()
guided_run();
break;
case LAND:
land_run();
case SURFACE:
surface_run();
break;
case RTL:
@ -304,7 +304,7 @@ void Sub::notify_flight_mode(control_mode_t mode) {
case GUIDED:
case RTL:
case CIRCLE:
case LAND:
case SURFACE:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
@ -345,8 +345,8 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case CIRCLE:
port->print("CIRCLE");
break;
case LAND:
port->print("LAND");
case SURFACE:
port->print("SURFACE");
break;
case OF_LOITER:
port->print("OF_LOITER");

55
ArduSub/landing_gear.cpp
View File

@ -6,32 +6,35 @@
// Run landing gear controller at 10Hz
void Sub::landinggear_update(){
// If landing gear control is active, run update function.
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
// Do nothing for Sub
// last status (deployed or retracted) used to check for changes
static bool last_deploy_status;
// if we are doing an automatic landing procedure, force the landing gear to deploy.
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
if (control_mode == LAND ||
(control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
(control_mode == AUTO && auto_mode == Auto_Land) ||
(control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
landinggear.force_deploy(true);
}
landinggear.update();
// send event message to datalog if status has changed
if (landinggear.deployed() != last_deploy_status){
if (landinggear.deployed()) {
Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
} else {
Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
}
}
last_deploy_status = landinggear.deployed();
}
// // If landing gear control is active, run update function.
// if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
//
// // last status (deployed or retracted) used to check for changes
// static bool last_deploy_status;
//
// // if we are doing an automatic landing procedure, force the landing gear to deploy.
// // To-Do: should we pause the auto-land procedure to give time for gear to come down?
// if (control_mode == LAND ||
// (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
// (control_mode == AUTO && auto_mode == Auto_Land) ||
// (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
// landinggear.force_deploy(true);
// }
//
// landinggear.update();
//
// // send event message to datalog if status has changed
// if (landinggear.deployed() != last_deploy_status){
// if (landinggear.deployed()) {
// Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
// } else {
// Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
// }
// }
//
// last_deploy_status = landinggear.deployed();
// }
}

18
ArduSub/switches.cpp
View File

@ -453,14 +453,16 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
#endif
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(LAND, MODE_REASON_TX_COMMAND);
}else{
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
// Do nothing for Sub
// if (ch_flag == AUX_SWITCH_HIGH) {
// set_mode(LAND, MODE_REASON_TX_COMMAND);
// }else{
// // return to flight mode switch's flight mode if we are currently in LAND
// if (control_mode == LAND) {
// reset_control_switch();
// }
// }
break;
#if PARACHUTE == ENABLED