aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: integrate init take-off

Browse Source
This commit is contained in:
Randy Mackay 2013-12-30 22:13:27 +09:00 committed by Andrew Tridgell
parent 8988b48ad8
commit 77c38f4de4
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/Attitude.pde
View File

@ -853,13 +853,9 @@ void throttle_accel_deactivate()
static void
set_throttle_takeoff()
{
// set alt target
controller_desired_alt = current_loc.alt + ALT_HOLD_TAKEOFF_JUMP;
// tell position controller to reset alt target and reset I terms
pos_control.init_takeoff();
// clear i term from acceleration controller
if (g.pid_throttle_accel.get_integrator() < 0) {
g.pid_throttle_accel.reset_I();
}
// tell motors to do a slow start
motors.slow_start(true);
}

11
ArduCopter/control_stabilize.pde
View File

@ -126,8 +126,8 @@ static void stabilize_run()
// althold_init - initialise althold controller
static bool althold_init()
{
// project stopping point
float alt_target = pos_control.get_stopping_point_z();
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}
@ -141,8 +141,9 @@ static void althold_run()
int16_t target_climb_rate;
// if not auto armed set throttle to zero and exit immediately
if(ap.auto_armed) {
if(!ap.auto_armed) {
// To-Do: set target angles to zero or current attitude?
// To-Do: reset altitude target if we're somehow not landed?
attitude_control.set_throttle_out(0, false);
set_target_alt_for_reporting(0);
return;
@ -215,10 +216,10 @@ static void althold_run()
if (!ap.land_complete) {
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(target_climb_rate); // this function calls set_target_alt_for_reporting for us
get_throttle_surface_tracking(target_climb_rate);
}else{
// if no sonar fall back stabilize rate controller
pos_control.climb_at_rate(target_climb_rate); // this function calls set_target_alt_for_reporting for us
pos_control.climb_at_rate(target_climb_rate);
}
}
}