Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 17:34:01 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: roll, pitch, yaw input in -1 to +1 range

Browse Source
This commit is contained in:
Leonard Hall 2016-01-22 11:08:55 +09:00 committed by Randy Mackay
parent 90b3d7ca39
commit ae4e495698
2 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -26,11 +26,11 @@ extern const AP_HAL::HAL& hal;
// Constructor // Constructor
AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : 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), _loop_rate(loop_rate),
_speed_hz(speed_hz), _speed_hz(speed_hz),
_roll_in(0.0f),
_pitch_in(0.0f),
_yaw_in(0.0f),
_throttle_in(0.0f), _throttle_in(0.0f),
_throttle_filter(), _throttle_filter(),
_batt_voltage(0.0f), _batt_voltage(0.0f),

23
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -59,25 +59,20 @@ public:
bool get_interlock() const { return _flags.interlock; }; bool get_interlock() const { return _flags.interlock; };
// set_roll, set_pitch, set_yaw, set_throttle // 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_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1
void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500 void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500 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(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_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; } void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; }
// accessors for roll, pitch, yaw and throttle inputs to motors // accessors for roll, pitch, yaw and throttle inputs to motors
float get_roll() const { return _roll_control_input; } float get_roll() const { return _roll_in; }
float get_pitch() const { return _pitch_control_input; } float get_pitch() const { return _pitch_in; }
float get_yaw() const { return _yaw_control_input; } float get_yaw() const { return _yaw_in; }
float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); } 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 // voltage, current and air pressure compensation or limiting features - multicopters only
// //
@ -151,11 +146,11 @@ protected:
} _flags; } _flags;
// internal variables // 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 _loop_rate; // rate at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors 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 float _throttle_in; // last throttle input from set_throttle caller
LowPassFilterFloat _throttle_filter; // throttle input filter LowPassFilterFloat _throttle_filter; // throttle input filter