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

Copter: get_pilot_desired_throttle outputs in 0 to 1 range

This commit is contained in:
Leonard Hall 2015-12-20 21:33:27 +10:30 committed by Randy Mackay
parent e132ea34d9
commit fd2509f6ed
2 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/Attitude.cpp
View File

@ -133,27 +133,29 @@ void Copter::set_throttle_takeoff()
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick // get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes // used only for manual throttle modes
// returns throttle output 0 to 1000 // returns throttle output 0 to 1
int16_t Copter::get_pilot_desired_throttle(int16_t throttle_control) float Copter::get_pilot_desired_throttle(int16_t throttle_control)
{ {
int16_t throttle_out; float throttle_out;
int16_t mid_stick = channel_throttle->get_control_mid(); int16_t mid_stick = channel_throttle->get_control_mid();
// ensure reasonable throttle values // ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000); 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); 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);
// check throttle is above, below or in the deadband // check throttle is above, below or in the deadband
if (throttle_control < mid_stick) { if (throttle_control < mid_stick) {
// below the deadband // below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min)); throttle_out = ((float)throttle_control)*thr_mid/(float)mid_stick;
}else if(throttle_control > mid_stick) { }else if(throttle_control > mid_stick) {
// above the deadband // above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick)) * (float)(1000-g.throttle_mid) / (float)(1000-mid_stick); throttle_out = (thr_mid) + ((float)(throttle_control-mid_stick)) * (1.0f - thr_mid) / (float)(1000-mid_stick);
}else{ }else{
// must be in the deadband // must be in the deadband
throttle_out = g.throttle_mid; throttle_out = thr_mid;
} }
return throttle_out; return throttle_out;

2
ArduCopter/Copter.h
View File

@ -610,7 +610,7 @@ private:
float get_look_ahead_yaw(); float get_look_ahead_yaw();
void update_thr_average(); void update_thr_average();
void set_throttle_takeoff(); void set_throttle_takeoff();
int16_t get_pilot_desired_throttle(int16_t throttle_control); float get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_climb_rate(float throttle_control); float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle(); float get_non_takeoff_throttle();
float get_takeoff_trigger_throttle(); float get_takeoff_trigger_throttle();