From 263052da215f3841851c4edefa53a1b308191856 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 18 Jan 2016 14:37:05 +0900 Subject: [PATCH] Copter: drift uses 0 to 1 throttle and sets motor spool state --- ArduCopter/Copter.h | 2 +- ArduCopter/control_drift.cpp | 30 +++++++++++++++--------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 08a96d4c51..c3ce1f5196 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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); diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index d3c21280ea..c79ead3500 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -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); }