ardupilot/ArduCopter/control_land.pde

254 lines
8.3 KiB
Plaintext
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// counter to verify landings
2014-09-19 03:54:36 -03:00
static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed
static bool land_with_gps;
static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
static bool land_init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = GPS_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);
}
2014-02-15 04:37:24 -04:00
// 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;
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
static void 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
static void land_gps_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
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 (land_complete_maybe()) {
wp_nav.loiter_soften_for_landing();
}
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = g.rc_1.control_in;
pitch_control = g.rc_2.control_in;
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
}
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_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_throttle_land();
}
// 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
static void land_nogps_run()
{
int16_t target_roll = 0, target_pitch = 0;
float target_yaw_rate = 0;
// if not auto armed or landed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
#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;
}
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
}
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_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_throttle_land();
}
// call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// get_throttle_land - 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
static float get_throttle_land()
{
2014-07-08 02:41:39 -03:00
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && sonar.healthy();
#else
bool sonar_ok = false;
#endif
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
2014-07-08 02:41:39 -03:00
if (current_loc.alt >= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);
}
}
// land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing)
static bool land_complete_maybe()
{
2014-09-19 03:54:36 -03:00
return (ap.land_complete || ap.land_complete_maybe);
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at 50hz
static void update_land_detector()
{
// detect whether we have landed by watching for low climb rate, motors hitting their lower limit, overall low throttle and low rotation rate
if ((abs(climb_rate) < LAND_DETECTOR_CLIMBRATE_MAX) &&
(abs(baro_climbrate) < LAND_DETECTOR_BARO_CLIMBRATE_MAX) &&
motors.limit.throttle_lower &&
#if FRAME_CONFIG != HELI_FRAME
(motors.get_throttle_out() < get_non_takeoff_throttle()) &&
#endif
(ahrs.get_gyro().length() < LAND_DETECTOR_ROTATION_MAX)) {
if (!ap.land_complete) {
// increase counter until we hit the trigger then set land complete flag
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
2014-09-19 03:54:36 -03:00
land_detector = LAND_DETECTOR_TRIGGER;
}
}
} else {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}
2014-09-19 03:54:36 -03:00
// set land maybe flag
set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER);
}
// 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
static void 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
static void set_mode_land_with_pause()
{
set_mode(LAND);
land_pause = true;
}
// landing_with_GPS - returns true if vehicle is landing using GPS
static bool landing_with_GPS() {
return (control_mode == LAND && land_with_gps);
}