Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: AP_Motor's throttle_hover replaces throttle_average

This commit is contained in:
Leonard Hall 2016-06-09 21:42:15 +09:00 committed by Randy Mackay
parent dec9323127
commit e9d8a28ec0
11 changed files with 35 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.cpp
View File

@ -97,7 +97,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_thr_average, 100, 90),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
@ -476,8 +476,6 @@ void Copter::one_hz_loop()
motors.set_frame_orientation(g.frame_orientation);
// set all throttle channel settings
// set hover throttle
motors.set_hover_throttle(g.throttle_mid);
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
}

36
ArduCopter/Attitude.cpp
View File

@ -94,31 +94,35 @@ float Copter::get_look_ahead_yaw()
* throttle control
****************************************************************/
// update_thr_average - update estimated throttle required to hover (if necessary)
// should be called at 100hz
void Copter::update_thr_average()
// update estimated throttle required to hover (if necessary)
// called at 100hz
void Copter::update_throttle_hover()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
throttle_average = 0.5f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
#if FRAME_CONFIG != HELI_FRAME
// if not armed or landed exit
if (!motors.armed() || ap.land_complete) {
return;
}
// do not update in manual throttle modes or Drift
if (mode_has_manual_throttle(control_mode) || (control_mode == DRIFT)) {
return;
}
// do not update while climbing or descending
if (!is_zero(pos_control.get_desired_velocity().z)) {
return;
}
// get throttle output
float throttle = motors.get_throttle();
// calc average throttle if we are in a level hover
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_average = throttle_average * 0.99f + throttle * 0.01f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
// Can we set the time constant automatically
motors.update_throttle_hover(0.01f);
}
#endif
}
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
@ -141,7 +145,7 @@ float Copter::get_pilot_desired_throttle(int16_t throttle_control)
throttle_control = constrain_int16(throttle_control,0,1000);
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float thr_mid = MAX(0,g.throttle_mid-g.throttle_min) / (float)(1000-g.throttle_min);
float thr_mid = constrain_float(motors.get_throttle_hover(), 0.1f, 0.9f);
// check throttle is above, below or in the deadband
if (throttle_control < mid_stick) {
@ -201,7 +205,7 @@ float Copter::get_non_takeoff_throttle()
{
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
return MAX(0,g.throttle_mid-g.throttle_min) / ((float)(1000-g.throttle_min) * 2.0f);
return MAX(0,motors.get_throttle_hover()/2.0f);
}
float Copter::get_takeoff_trigger_throttle()
@ -285,7 +289,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
void Copter::set_accel_throttle_I_from_pilot_throttle(float pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator((pilot_throttle-throttle_average) * 1000.0f);
g.pid_accel_z.set_integrator((pilot_throttle-motors.get_throttle_hover()) * 1000.0f);
}
// updates position controller's maximum altitude using fence and EKF limits

1
ArduCopter/Copter.cpp
View File

@ -51,7 +51,6 @@ Copter::Copter(void) :
super_simple_cos_yaw(1.0),
super_simple_sin_yaw(0.0f),
initial_armed_bearing(0),
throttle_average(0.0f),
desired_climb_rate(0),
loiter_time_max(0),
loiter_time(0),

3
ArduCopter/Copter.h
View File

@ -373,7 +373,6 @@ private:
int32_t initial_armed_bearing;
// Throttle variables
float throttle_average; // estimated throttle required to hover
int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only
// Loiter control
@ -620,7 +619,7 @@ private:
void check_ekf_yaw_reset();
float get_roi_yaw();
float get_look_ahead_yaw();
void update_thr_average();
void update_throttle_hover();
void set_throttle_takeoff();
float get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_climb_rate(float throttle_control);

6
ArduCopter/control_althold.cpp
View File

@ -92,7 +92,7 @@ void Copter::althold_run()
#else
// Multicopters do not stabilize roll/pitch/yaw when motor are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
#endif
break;
@ -108,7 +108,7 @@ void Copter::althold_run()
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case AltHold_Takeoff:
@ -151,7 +151,7 @@ void Copter::althold_run()
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case AltHold_Flying:

4
ArduCopter/control_autotune.cpp
View File

@ -271,7 +271,7 @@ void Copter::autotune_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::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->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -304,7 +304,7 @@ void Copter::autotune_run()
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
}else{
// check if pilot is overriding the controls
if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || target_climb_rate != 0) {

2
ArduCopter/control_brake.cpp
View File

@ -49,7 +49,7 @@ void Copter::brake_run()
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-motors.get_throttle_hover());
return;
}

6
ArduCopter/control_loiter.cpp
View File

@ -106,7 +106,7 @@ void Copter::loiter_run()
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
#endif
break;
@ -122,7 +122,7 @@ void Copter::loiter_run()
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case Loiter_Takeoff:
@ -166,7 +166,7 @@ void Copter::loiter_run()
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case Loiter_Flying:

4
ArduCopter/control_poshold.cpp
View File

@ -145,7 +145,7 @@ void Copter::poshold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -193,7 +193,7 @@ void Copter::poshold_run()
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}else{
// convert pilot input to lean angles

4
ArduCopter/control_sport.cpp
View File

@ -36,7 +36,7 @@ void Copter::sport_run()
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::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->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -99,7 +99,7 @@ void Copter::sport_run()
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
}else{
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

1
ArduCopter/radio.cpp
View File

@ -59,7 +59,6 @@ void Copter::init_rc_out()
motors.set_loop_rate(scheduler.get_loop_rate_hz());
motors.Init(); // motor initialisation
#if FRAME_CONFIG != HELI_FRAME
motors.set_hover_throttle(g.throttle_mid);
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif