mirror of https://github.com/ArduPilot/ardupilot
289 lines
10 KiB
C++
289 lines
10 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include "Copter.h"
|
|
|
|
static bool land_with_gps;
|
|
|
|
static uint32_t land_start_time;
|
|
static bool land_pause;
|
|
|
|
// land_init - initialise land controller
|
|
bool Copter::land_init(bool ignore_checks)
|
|
{
|
|
// check if we have GPS and decide which LAND we're going to do
|
|
land_with_gps = position_ok();
|
|
if (land_with_gps) {
|
|
// set target to stopping point
|
|
Vector3f stopping_point;
|
|
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
|
wp_nav.init_loiter_target(stopping_point);
|
|
}
|
|
|
|
// 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 altitude target to stopping point
|
|
pos_control.set_target_to_stopping_point_z();
|
|
|
|
land_start_time = millis();
|
|
|
|
land_pause = false;
|
|
|
|
// reset flag indicating if pilot has applied roll or pitch inputs during landing
|
|
ap.land_repo_active = false;
|
|
|
|
return true;
|
|
}
|
|
|
|
// land_run - runs the land controller
|
|
// should be called at 100hz or more
|
|
void Copter::land_run()
|
|
{
|
|
if (land_with_gps) {
|
|
land_gps_run();
|
|
}else{
|
|
land_nogps_run();
|
|
}
|
|
}
|
|
|
|
// land_run - runs the land controller
|
|
// horizontal position controlled with loiter controller
|
|
// should be called at 100hz or more
|
|
void Copter::land_gps_run()
|
|
{
|
|
int16_t roll_control = 0, pitch_control = 0;
|
|
float target_yaw_rate = 0;
|
|
|
|
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
|
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
|
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
|
// call attitude controller
|
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
|
|
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
|
#else
|
|
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
|
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
|
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
#endif
|
|
wp_nav.init_loiter_target();
|
|
|
|
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
|
// disarm when the landing detector says we've landed and throttle is at minimum
|
|
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
|
|
init_disarm_motors();
|
|
}
|
|
#else
|
|
// disarm when the landing detector says we've landed
|
|
if (ap.land_complete) {
|
|
init_disarm_motors();
|
|
}
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
// relax loiter target if we might be landed
|
|
if (ap.land_complete_maybe) {
|
|
wp_nav.loiter_soften_for_landing();
|
|
}
|
|
|
|
// process pilot inputs
|
|
if (!failsafe.radio) {
|
|
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
|
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
|
|
// exit land if throttle is high
|
|
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
|
|
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
|
|
}
|
|
}
|
|
|
|
if (g.land_repositioning) {
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// process pilot's roll and pitch input
|
|
roll_control = channel_roll->get_control_in();
|
|
pitch_control = channel_pitch->get_control_in();
|
|
|
|
// record if pilot has overriden roll or pitch
|
|
if (roll_control != 0 || pitch_control != 0) {
|
|
ap.land_repo_active = true;
|
|
}
|
|
}
|
|
|
|
// get pilot's desired yaw rate
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
}
|
|
|
|
// set motors to full range
|
|
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
|
|
|
// process roll, pitch inputs
|
|
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
|
|
|
|
#if PRECISION_LANDING == ENABLED
|
|
// run precision landing
|
|
if (!ap.land_repo_active) {
|
|
wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));
|
|
}
|
|
#endif
|
|
|
|
// run loiter controller
|
|
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
|
|
|
// call attitude controller
|
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
|
|
|
// pause 4 seconds before beginning land descent
|
|
float cmb_rate;
|
|
if(land_pause && millis()-land_start_time < 4000) {
|
|
cmb_rate = 0;
|
|
} else {
|
|
land_pause = false;
|
|
cmb_rate = get_land_descent_speed();
|
|
}
|
|
|
|
// 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(cmb_rate, G_Dt, true);
|
|
pos_control.update_z_controller();
|
|
}
|
|
|
|
// land_nogps_run - runs the land controller
|
|
// pilot controls roll and pitch angles
|
|
// should be called at 100hz or more
|
|
void Copter::land_nogps_run()
|
|
{
|
|
float target_roll = 0.0f, target_pitch = 0.0f;
|
|
float target_yaw_rate = 0;
|
|
|
|
// process pilot inputs
|
|
if (!failsafe.radio) {
|
|
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
|
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
|
|
// exit land if throttle is high
|
|
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
|
|
}
|
|
|
|
if (g.land_repositioning) {
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// get pilot desired lean angles
|
|
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());
|
|
}
|
|
|
|
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
|
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
|
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
|
// call attitude controller
|
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
|
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
|
#else
|
|
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
|
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
|
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
#endif
|
|
|
|
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
|
// disarm when the landing detector says we've landed and throttle is at minimum
|
|
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
|
|
init_disarm_motors();
|
|
}
|
|
#else
|
|
// disarm when the landing detector says we've landed
|
|
if (ap.land_complete) {
|
|
init_disarm_motors();
|
|
}
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
// set motors to full range
|
|
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
|
|
|
// call attitude controller
|
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
|
|
|
// pause 4 seconds before beginning land descent
|
|
float cmb_rate;
|
|
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
|
|
cmb_rate = 0;
|
|
} else {
|
|
land_pause = false;
|
|
cmb_rate = get_land_descent_speed();
|
|
}
|
|
|
|
// record desired climb rate for logging
|
|
desired_climb_rate = cmb_rate;
|
|
|
|
// call position controller
|
|
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
|
|
pos_control.update_z_controller();
|
|
}
|
|
|
|
// get_land_descent_speed - high level landing logic
|
|
// returns climb rate (in cm/s) which should be passed to the position controller
|
|
// should be called at 100hz or higher
|
|
float Copter::get_land_descent_speed()
|
|
{
|
|
#if CONFIG_SONAR == ENABLED
|
|
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
|
|
#else
|
|
bool sonar_ok = false;
|
|
#endif
|
|
|
|
// get position controller's target altitude above terrain
|
|
Location_Class target_loc = pos_control.get_pos_target();
|
|
int32_t target_alt_cm;
|
|
|
|
// get altitude target above home by default
|
|
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
|
|
|
|
// try to use terrain if enabled
|
|
if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) {
|
|
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
|
}
|
|
|
|
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
|
|
if ((target_alt_cm >= LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
|
|
if (g.land_speed_high > 0) {
|
|
// user has asked for a different landing speed than normal descent rate
|
|
return -abs(g.land_speed_high);
|
|
}
|
|
return pos_control.get_speed_down();
|
|
}else{
|
|
return -abs(g.land_speed);
|
|
}
|
|
}
|
|
|
|
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
|
|
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
|
|
// has no effect if we are not already in LAND mode
|
|
void Copter::land_do_not_use_GPS()
|
|
{
|
|
land_with_gps = false;
|
|
}
|
|
|
|
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
|
|
// this is always called from a failsafe so we trigger notification to pilot
|
|
void Copter::set_mode_land_with_pause(mode_reason_t reason)
|
|
{
|
|
set_mode(LAND, reason);
|
|
land_pause = true;
|
|
|
|
// alert pilot to mode change
|
|
AP_Notify::events.failsafe_mode_change = 1;
|
|
}
|
|
|
|
// landing_with_GPS - returns true if vehicle is landing using GPS
|
|
bool Copter::landing_with_GPS() {
|
|
return (control_mode == LAND && land_with_gps);
|
|
}
|