Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: althold uses 0 to 1 throttle range and spool logic

Browse Source 
stabilizes even while landed
sets desired motor spool state
check for spool up before takeoff
This commit is contained in:
Leonard Hall 2016-01-18 12:52:36 +09:00 committed by Randy Mackay
parent 2180514dde
commit 70433d25e3
1 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopter/control_althold.cpp
View File

@ -59,14 +59,14 @@ void Copter::althold_run()
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete());
#else
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.spool_up_complete());
#endif
// Alt Hold State Machine Determination
if(!ap.auto_armed) {
althold_state = AltHold_Disarmed;
} else if (!motors.get_interlock()){
if (!motors.armed() || !motors.get_interlock()) {
althold_state = AltHold_MotorStop;
} else if (!ap.auto_armed){
althold_state = AltHold_Disarmed;
} else if (takeoff_state.running || takeoff_triggered){
althold_state = AltHold_Takeoff;
} else if (ap.land_complete){
@ -85,7 +85,9 @@ void Copter::althold_run()
// 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 // Multicopter do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif // HELI_FRAME
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
@ -102,6 +104,7 @@ void Copter::althold_run()
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SHUT_DOWN);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
#endif // HELI_FRAME
@ -121,6 +124,22 @@ void Copter::althold_run()
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
// slow start if landed
if (ap.land_complete) {
motors.slow_start(true);
}
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::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());
@ -137,13 +156,20 @@ void Copter::althold_run()
// 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(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
#else // Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
#else // Multicopter stabilize roll/pitch/yaw when landed
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
}
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_Flying:
motors.set_desired_spool_state(AP_MotorsMulticopter::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());