From 5ba3a6c5364bde705cee6bc737a7953dd87e5d12 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Feb 2016 20:55:55 +0900 Subject: [PATCH] AP_Motors: add set_radio_passthrough --- libraries/AP_Motors/AP_Motors_Class.cpp | 9 +++++++++ libraries/AP_Motors/AP_Motors_Class.h | 9 +++++++++ 2 files changed, 18 insertions(+) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index b0c4a763b7..6e65e75b2a 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -61,6 +61,15 @@ void AP_Motors::armed(bool arm) AP_Notify::flags.armed = arm; }; +// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle +void AP_Motors::set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input) +{ + _roll_radio_passthrough = roll_input; + _pitch_radio_passthrough = pitch_input; + _throttle_radio_passthrough = throttle_input; + _yaw_radio_passthrough = yaw_input; +} + /* write to an output channel */ diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index a01e665c1c..6f07609012 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -132,6 +132,9 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask() = 0; + // pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle + void set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input); + protected: // output functions that should be overloaded by child classes virtual void output_armed_stabilizing()=0; @@ -175,4 +178,10 @@ protected: // mapping to output channels uint8_t _motor_map[AP_MOTORS_MAX_NUM_MOTORS]; uint16_t _motor_map_mask; + + // pass through variables + float _roll_radio_passthrough = 0.0f; // roll input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed + float _pitch_radio_passthrough = 0.0f; // pitch input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed + float _throttle_radio_passthrough = 0.0f; // throttle/collective input from pilot in 0 ~ 1 range. used for setup and providing servo feedback while landed + float _yaw_radio_passthrough = 0.0f; // yaw input from pilot in -1 ~ +1 range. used for setup and providing servo feedback while landed };