From 1a583c538292d3d76d0489216acff70de7ab2c26 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 3 Aug 2015 13:36:10 +0900 Subject: [PATCH] AP_Motors: calc_roll_pwm based on throttle pwm range --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 1 + libraries/AP_Motors/AP_MotorsMulticopter.h | 6 +++--- libraries/AP_Motors/AP_Motors_Class.cpp | 1 + libraries/AP_Motors/AP_Motors_Class.h | 1 + 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 0c1de4f34e..5a2f6a4be3 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -350,6 +350,7 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad _throttle_radio_min = radio_min; _throttle_radio_max = radio_max; _throttle_pwm_scalar = (_throttle_radio_max - _throttle_radio_min) / 1000.0f; + _rpy_pwm_scalar = (_throttle_radio_max - (_throttle_radio_min + _min_throttle)) / 9000.0f; _min_throttle = (float)min_throttle * _throttle_pwm_scalar; } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index e491faa582..91614c4483 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -131,9 +131,9 @@ protected: // RPY channels typically +/-45 degrees servo travel between +/-400 PWM // Throttle channel typically 0-1000 range converts to 1100-1900 PWM for final output signal to motors // ToDo: this should all be handled as floats +/- 1.0 instead of PWM and fake angle ranges - int16_t calc_roll_pwm() { return (_roll_control_input / 11.25f);} - int16_t calc_pitch_pwm() { return (_pitch_control_input / 11.25f);} - int16_t calc_yaw_pwm() { return (_yaw_control_input / 11.25f);} + float calc_roll_pwm() { return (_roll_control_input * _rpy_pwm_scalar); } + float calc_pitch_pwm() { return (_pitch_control_input * _rpy_pwm_scalar); } + float calc_yaw_pwm() { return (_yaw_control_input * _rpy_pwm_scalar); } int16_t calc_throttle_radio_output() { return (_throttle_control_input * _throttle_pwm_scalar) + _throttle_radio_min;} // flag bitmask diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 9d7271de5e..7f5568c870 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -34,6 +34,7 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : _throttle_control_input(0.0f), _yaw_control_input(0.0f), _throttle_pwm_scalar(1.0f), + _rpy_pwm_scalar(0.074f), _loop_rate(loop_rate), _speed_hz(speed_hz), _throttle_radio_min(1100), diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 63e806ab75..6a6912f7b7 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -153,6 +153,7 @@ protected: float _throttle_control_input; // desired throttle (thrust) control from attitude controller, 0-1000 float _yaw_control_input; // desired yaw control from attitude controller, +/- 4500 float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0 + float _rpy_pwm_scalar; // scaler used to convert roll, pitch, yaw inputs to pwm range uint16_t _loop_rate; // rate at which output() function is called (normally 400hz) uint16_t _speed_hz; // speed in hz to send updates to motors int16_t _throttle_radio_min; // minimum radio channel pwm