From f571b34fd743b283ad4687b58bc3f89af0f78e70 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Apr 2017 14:51:00 +1000 Subject: [PATCH] AP_Motors: setup RC speed for tailsitters --- libraries/AP_Motors/AP_MotorsTailsitter.cpp | 9 +++++++++ libraries/AP_Motors/AP_MotorsTailsitter.h | 5 +---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index d6d0067c58..bd24a60125 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -35,6 +35,15 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f _flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER); } + +/// Constructor +AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) : + AP_MotorsMulticopter(loop_rate, speed_hz) +{ + SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz); + SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz); +} + void AP_MotorsTailsitter::output_to_motors() { if (!_flags.initialised_ok) { diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.h b/libraries/AP_Motors/AP_MotorsTailsitter.h index d66805f5c4..cbd3c9e263 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.h +++ b/libraries/AP_Motors/AP_MotorsTailsitter.h @@ -12,10 +12,7 @@ class AP_MotorsTailsitter : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) - { - }; + AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init void init(motor_frame_class frame_class, motor_frame_type frame_type);