zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: refactor landing to reduce duplication, use vertical vel ff

master
Jonathan Challinger 9 years ago committed by Randy Mackay
parent
e311139a21
commit
e22220ab62
4 changed files with 106 additions and 220 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ArduCopter/Copter.h
  2. 71
      ArduCopter/control_auto.cpp
  3. 199
      ArduCopter/control_land.cpp
  4. 53
      ArduCopter/control_rtl.cpp

3
ArduCopter/Copter.h
Unescape View File

@ -813,7 +813,8 @@ private: @@ -813,7 +813,8 @@ private:
void land_run();
void land_gps_run();
void land_nogps_run();
float get_land_descent_speed();
void land_run_vertical_control(bool pause_descent = false);
void land_run_horizontal_control();
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();

71
ArduCopter/control_auto.cpp
Unescape View File

@ -360,8 +360,9 @@ void Copter::auto_land_start(const Vector3f& destination) @@ -360,8 +360,9 @@ void Copter::auto_land_start(const Vector3f& destination)
// initialise loiter target destination
wp_nav.init_loiter_target(destination);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_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());
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -371,11 +372,8 @@ void Copter::auto_land_start(const Vector3f& destination) @@ -371,11 +372,8 @@ void Copter::auto_land_start(const Vector3f& destination)
// called by auto_run at 100hz or more
void Copter::auto_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
// 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(0, 0, 0, get_smoothing_gain());
@ -390,64 +388,11 @@ void Copter::auto_land_run() @@ -390,64 +388,11 @@ void Copter::auto_land_run()
return;
}
// relax loiter targets if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
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();
}
// 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 && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
land_run_horizontal_control();
land_run_vertical_control();
}
// auto_rtl_start - initialises RTL in AUTO flight mode

199
ArduCopter/control_land.cpp
Unescape View File

@ -23,9 +23,10 @@ bool Copter::land_init(bool ignore_checks) @@ -23,9 +23,10 @@ bool Copter::land_init(bool ignore_checks)
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();
// 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());
land_start_time = millis();
land_pause = false;
@ -52,9 +53,6 @@ void Copter::land_run() @@ -52,9 +53,6 @@ void Copter::land_run()
// 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
@ -81,78 +79,17 @@ void Copter::land_gps_run() @@ -81,78 +79,17 @@ void Copter::land_gps_run()
#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 && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#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, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < 4000) {
cmb_rate = 0;
} else {
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
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_run_horizontal_control();
land_run_vertical_control(land_pause);
}
// land_nogps_run - runs the land controller
@ -215,56 +152,102 @@ void Copter::land_nogps_run() @@ -215,56 +152,102 @@ void Copter::land_nogps_run()
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(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 {
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
land_run_vertical_control(land_pause);
}
void Copter::land_run_vertical_control(bool pause_descent)
{
bool navigating = pos_control.is_active_xy();
// compute desired velocity
int32_t alt_above_ground;
if (rangefinder_alt_ok()) {
alt_above_ground = rangefinder_state.alt_cm_filt.get();
} else {
if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_ground);
}
}
float cmb_rate = 0;
if (!pause_descent) {
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z());
cmb_rate = constrain_float(cmb_rate, pos_control.get_speed_down(), -abs(g.land_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);
// 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();
}
// 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()
void Copter::land_run_horizontal_control()
{
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
#else
bool rangefinder_ok = false;
#endif
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// 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);
}
}
// get position controller's target altitude above terrain
Location_Class target_loc = pos_control.get_pos_target();
int32_t target_alt_cm;
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();
// get altitude target above home by default
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
// record if pilot has overriden roll or pitch
if (roll_control != 0 || pitch_control != 0) {
ap.land_repo_active = true;
}
}
// 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);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) {
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);
#if PRECISION_LANDING == ENABLED
bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();
// run precision landing
if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#else
bool doing_precision_landing = false;
#endif
// 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.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch

53
ArduCopter/control_rtl.cpp
Unescape View File

@ -349,8 +349,9 @@ void Copter::rtl_land_start() @@ -349,8 +349,9 @@ void Copter::rtl_land_start()
// Set wp navigation target to above home
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_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());
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -360,8 +361,6 @@ void Copter::rtl_land_start() @@ -360,8 +361,6 @@ void Copter::rtl_land_start()
// called by rtl_run at 100hz or more
void Copter::rtl_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landing completed 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
@ -393,53 +392,11 @@ void Copter::rtl_land_run() @@ -393,53 +392,11 @@ void Copter::rtl_land_run()
return;
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
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();
}
// 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 pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
land_run_horizontal_control();
land_run_vertical_control();
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;

Write Preview
Loading…
Cancel
Save