From ae4e49569823e6d224621d13233f847f1ffe40f1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 22 Jan 2016 11:08:55 +0900 Subject: [PATCH] AP_Motors: roll, pitch, yaw input in -1 to +1 range --- libraries/AP_Motors/AP_Motors_Class.cpp | 6 +++--- libraries/AP_Motors/AP_Motors_Class.h | 23 +++++++++-------------- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 1157dc7b1f..679978c1d0 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -26,11 +26,11 @@ extern const AP_HAL::HAL& hal; // Constructor AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : - _roll_control_input(0.0f), - _pitch_control_input(0.0f), - _yaw_control_input(0.0f), _loop_rate(loop_rate), _speed_hz(speed_hz), + _roll_in(0.0f), + _pitch_in(0.0f), + _yaw_in(0.0f), _throttle_in(0.0f), _throttle_filter(), _batt_voltage(0.0f), diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 8a128b2c27..208f7c7f0c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -59,25 +59,20 @@ public: bool get_interlock() const { return _flags.interlock; }; // set_roll, set_pitch, set_yaw, set_throttle - void set_roll(int16_t roll_in) { _roll_control_input = roll_in; }; // range -4500 ~ 4500 - void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500 - void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500 + void set_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1 + void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1 + void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1 void set_throttle(float throttle_in) { _throttle_in = constrain_float(throttle_in,0.0f,1.0f); }; // range 0 ~ 1 void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; } // accessors for roll, pitch, yaw and throttle inputs to motors - float get_roll() const { return _roll_control_input; } - float get_pitch() const { return _pitch_control_input; } - float get_yaw() const { return _yaw_control_input; } + float get_roll() const { return _roll_in; } + float get_pitch() const { return _pitch_in; } + float get_yaw() const { return _yaw_in; } float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); } - // accessors for roll, pitch, yaw to motors within the range -1 ~ +1 - float get_roll_thrust() const { return _roll_control_input / 4500.0f; } - float get_pitch_thrust() const { return _pitch_control_input / 4500.0f; } - float get_yaw_thrust() const { return _yaw_control_input / 4500.0f; } - // // voltage, current and air pressure compensation or limiting features - multicopters only // @@ -151,11 +146,11 @@ protected: } _flags; // internal variables - float _roll_control_input; // desired roll control from attitude controllers, +/- 4500 - float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500 - float _yaw_control_input; // desired yaw control from attitude controller, +/- 4500 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 + float _roll_in; // desired roll control from attitude controllers, -1 ~ +1 + float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1 + float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1 float _throttle_in; // last throttle input from set_throttle caller LowPassFilterFloat _throttle_filter; // throttle input filter