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

Copter: drift uses 0 to 1 throttle and sets motor spool state

This commit is contained in:
Leonard Hall 2016-01-18 14:37:05 +09:00 committed by Randy Mackay
parent 100fcf799e
commit 263052da21
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -762,7 +762,7 @@ private:
void circle_run();
bool drift_init(bool ignore_checks);
void drift_run();
int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled);
float get_throttle_assist(float velz, float pilot_throttle_scaled);
bool flip_init(bool ignore_checks);
void flip_run();
bool guided_init(bool ignore_checks);

30
ArduCopter/control_drift.cpp
View File

@ -18,14 +18,14 @@
#endif
#ifndef DRIFT_THR_ASSIST_MAX
# define DRIFT_THR_ASSIST_MAX 300.0f // maximum assistance throttle assist will provide
# define DRIFT_THR_ASSIST_MAX 0.3f // maximum assistance throttle assist will provide
#endif
#ifndef DRIFT_THR_MIN
# define DRIFT_THR_MIN 213 // throttle assist will be active when pilot's throttle is above this value
# define DRIFT_THR_MIN 0.213f // throttle assist will be active when pilot's throttle is above this value
#endif
#ifndef DRIFT_THR_MAX
# define DRIFT_THR_MAX 787 // throttle assist will be active when pilot's throttle is below this value
# define DRIFT_THR_MAX 0.787f // throttle assist will be active when pilot's throttle is below this value
#endif
// drift_init - initialise drift controller
@ -46,10 +46,11 @@ void Copter::drift_run()
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
float pilot_throttle_scaled;
// if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
// if landed and throttle at zero, set throttle to zero and exit immediately
if (!motors.armed() || !motors.get_interlock() || (ap.land_complete && 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);
return;
}
@ -93,6 +94,9 @@ void Copter::drift_run()
breaker = 0.0f;
}
// 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());
@ -100,25 +104,21 @@ void Copter::drift_run()
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
}
// get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity
int16_t Copter::get_throttle_assist(float velz, int16_t pilot_throttle_scaled)
// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity
float Copter::get_throttle_assist(float velz, float pilot_throttle_scaled)
{
// throttle assist - adjusts throttle to slow the vehicle's vertical velocity
// Only active when pilot's throttle is between 213 ~ 787
// Assistance is strongest when throttle is at mid, drops linearly to no assistance at 213 and 787
float thr_assist = 0.0f;
if (pilot_throttle_scaled > g.throttle_min && pilot_throttle_scaled < THR_MAX &&
pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
if (pilot_throttle_scaled > DRIFT_THR_MIN && pilot_throttle_scaled < DRIFT_THR_MAX) {
// calculate throttle assist gain
thr_assist = 1.2f - ((float)abs(pilot_throttle_scaled - 500) / 240.0f);
thr_assist = 1.2f - ((float)fabsf(pilot_throttle_scaled - 0.5f) / 0.24f);
thr_assist = constrain_float(thr_assist, 0.0f, 1.0f) * -DRIFT_THR_ASSIST_GAIN * velz;
// ensure throttle assist never adjusts the throttle by more than 300 pwm
thr_assist = constrain_float(thr_assist, -DRIFT_THR_ASSIST_MAX, DRIFT_THR_ASSIST_MAX);
// ensure throttle assist never pushes throttle below throttle_min or above throttle_max
thr_assist = constrain_float(thr_assist, g.throttle_min - pilot_throttle_scaled, THR_MAX - pilot_throttle_scaled);
}
return pilot_throttle_scaled + thr_assist;
return constrain_float(pilot_throttle_scaled + thr_assist, 0.0f, 1.0f);
}