From 1a9d3125ef0f60a79e89fc8ca9874de73fb8c46d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 23 Feb 2015 11:27:51 +0900 Subject: [PATCH] Motors: _hover_out to pct * 10 instead of pwm --- libraries/AP_Motors/AP_Motors_Class.cpp | 7 +++---- libraries/AP_Motors/AP_Motors_Class.h | 10 +++++++--- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index dcc4b03b1b..3987d19edf 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -144,11 +144,10 @@ void AP_Motors::set_min_throttle(uint16_t min_throttle) _min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f; } -// set_mid_throttle - sets the mid throttle which is close to the hover throttle of the copter -// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control -void AP_Motors::set_mid_throttle(uint16_t mid_throttle) +// get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000) +int16_t AP_Motors::get_hover_throttle_as_pwm() const { - _hover_out = _rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * mid_throttle / 1000.0f; + return (_rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * _hover_out / 1000.0f); } // throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 511911f86c..4589f2ffbd 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -104,9 +104,13 @@ public: // set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle) void set_min_throttle(uint16_t min_throttle); - // set_mid_throttle - sets the mid throttle which is close to the hover throttle of the copter + + // set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control - void set_mid_throttle(uint16_t mid_throttle); + void set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; } + + // get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000) + int16_t get_hover_throttle_as_pwm() const; int16_t throttle_min() const { return _min_throttle;} int16_t throttle_max() const { return _max_throttle;} @@ -212,7 +216,7 @@ protected: uint16_t _speed_hz; // speed in hz to send updates to motors int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) - int16_t _hover_out; // the estimated hover throttle in pwm (i.e. 1000 ~ 2000). calculated from the THR_MID parameter + int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero // battery voltage compensation variables