mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 07:58:28 -04:00
33a274c928
Also balances pilot and takeoff climb rates Also removes takeoff's direct use of poscontrol target Combined effort of Randy and Leonard
86 lines
3.3 KiB
Plaintext
86 lines
3.3 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
* control_althold.pde - init and run calls for althold, flight mode
|
|
*/
|
|
|
|
// althold_init - initialise althold controller
|
|
static bool althold_init(bool ignore_checks)
|
|
{
|
|
// initialize vertical speeds and leash lengths
|
|
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
|
pos_control.set_accel_z(g.pilot_accel_z);
|
|
|
|
// initialise altitude target to stopping point
|
|
pos_control.set_target_to_stopping_point_z();
|
|
|
|
return true;
|
|
}
|
|
|
|
// althold_run - runs the althold controller
|
|
// should be called at 100hz or more
|
|
static void althold_run()
|
|
{
|
|
float target_roll, target_pitch;
|
|
float target_yaw_rate;
|
|
float target_climb_rate;
|
|
float takeoff_climb_rate = 0.0f;
|
|
|
|
// if not auto armed set throttle to zero and exit immediately
|
|
if(!ap.auto_armed) {
|
|
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average);
|
|
return;
|
|
}
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
update_simple_mode();
|
|
|
|
// get pilot desired lean angles
|
|
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
|
|
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);
|
|
|
|
// get pilot desired climb rate
|
|
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
|
|
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
|
|
|
// get takeoff adjusted pilot and takeoff climb rates
|
|
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
|
|
|
|
// check for take-off
|
|
if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
|
|
if (!takeoff_state.running) {
|
|
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
|
|
}
|
|
|
|
// indicate we are taking off
|
|
set_land_complete(false);
|
|
// clear i term when we're taking off
|
|
set_throttle_takeoff();
|
|
}
|
|
|
|
// reset target lean angles and heading while landed
|
|
if (ap.land_complete) {
|
|
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(g.rc_3.control_in),true,g.throttle_filt);
|
|
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average);
|
|
}else{
|
|
// call attitude controller
|
|
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
|
// body-frame rate controller is run directly from 100hz loop
|
|
|
|
// call throttle controller
|
|
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
|
|
// if sonar is ok, use surface tracking
|
|
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
|
}
|
|
|
|
// call position controller
|
|
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
|
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
|
pos_control.update_z_controller();
|
|
}
|
|
}
|