From 1a74b7fc0c03ec709aaf6279a9444839d608ca2f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 10 Feb 2017 16:26:12 +1100 Subject: [PATCH] AP_Motors: started on tailsitter support --- libraries/AP_Motors/AP_Motors.h | 1 + libraries/AP_Motors/AP_MotorsTailsitter.cpp | 89 +++++++++++++++++++++ libraries/AP_Motors/AP_MotorsTailsitter.h | 45 +++++++++++ libraries/AP_Motors/AP_Motors_Class.h | 3 +- 4 files changed, 137 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Motors/AP_MotorsTailsitter.cpp create mode 100644 libraries/AP_Motors/AP_MotorsTailsitter.h diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index 707c2b4538..5b8d697d6b 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -7,3 +7,4 @@ #include "AP_MotorsHeli_Single.h" #include "AP_MotorsSingle.h" #include "AP_MotorsCoax.h" +#include "AP_MotorsTailsitter.h" diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp new file mode 100644 index 0000000000..e0c6bd5bf8 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -0,0 +1,89 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + * AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters + * + */ + +#include +#include +#include "AP_MotorsTailsitter.h" +#include + +extern const AP_HAL::HAL& hal; + +#define SERVO_OUTPUT_RANGE 4500 +#define THROTTLE_RANGE 100 + +// init +void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type) +{ + // record successful initialisation if what we setup was the desired frame_class + _flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER); +} + +void AP_MotorsTailsitter::output_to_motors() +{ + if (!_flags.initialised_ok) { + return; + } + switch (_spool_mode) { + case SHUT_DOWN: + _aileron = 0; + _elevator = 0; + _rudder = 0; + _throttle = 0; + break; + case SPIN_WHEN_ARMED: + // sends output to motors when armed but not flying + _aileron = 0; + _elevator = 0; + _rudder = 0; + _throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; + break; + case SPOOL_UP: + case THROTTLE_UNLIMITED: + case SPOOL_DOWN: + break; + } + // outputs are setup here, and written to the HAL by the plane servos loop + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, _aileron*SERVO_OUTPUT_RANGE); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _elevator*SERVO_OUTPUT_RANGE); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _rudder*SERVO_OUTPUT_RANGE); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, _throttle*THROTTLE_RANGE); +} + +// calculate outputs to the motors +void AP_MotorsTailsitter::output_armed_stabilizing() +{ + _aileron = -_yaw_in; + _elevator = _pitch_in; + _rudder = _roll_in; + _throttle = get_throttle(); + + // sanity check throttle is above zero and below current limited throttle + if (_throttle <= 0.0f) { + _throttle = 0.0f; + limit.throttle_lower = true; + } + if (_throttle >= _throttle_thrust_max) { + _throttle = _throttle_thrust_max; + limit.throttle_upper = true; + } + + _throttle = constrain_float(_throttle, 0.1, 1); +} + diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.h b/libraries/AP_Motors/AP_MotorsTailsitter.h new file mode 100644 index 0000000000..d66805f5c4 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsTailsitter.h @@ -0,0 +1,45 @@ +/// @file AP_MotorsTailsitter.h +/// @brief Motor control class for tailsitters +#pragma once + +#include +#include +#include +#include "AP_MotorsMulticopter.h" + +/// @class AP_MotorsTailsitter +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) + { + }; + + // init + void init(motor_frame_class frame_class, motor_frame_type frame_type); + + // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) + void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) {} + void set_update_rate( uint16_t speed_hz ) {} + void enable() {} + + void output_test(uint8_t motor_seq, int16_t pwm) {} + + // output_to_motors - sends output to named servos + void output_to_motors(); + + // return 0 motor mask + uint16_t get_motor_mask() { return 0; } + +protected: + // calculate motor outputs + void output_armed_stabilizing(); + + // calculated outputs + float _aileron; // -1..1 + float _elevator; // -1..1 + float _rudder; // -1..1 + float _throttle; // 0..1 +}; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index fe20a70452..3284415b9c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -35,7 +35,8 @@ public: MOTOR_FRAME_HELI = 6, MOTOR_FRAME_TRI = 7, MOTOR_FRAME_SINGLE = 8, - MOTOR_FRAME_COAX = 9 + MOTOR_FRAME_COAX = 9, + MOTOR_FRAME_TAILSITTER = 10, }; enum motor_frame_type { MOTOR_FRAME_TYPE_PLUS = 0,