AP_Motors: add set_radio_passthrough

This commit is contained in:
Randy Mackay 2016-02-03 20:55:55 +09:00
parent f0575de776
commit 5ba3a6c536
2 changed files with 18 additions and 0 deletions

View File

@ -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
*/

View File

@ -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
};