2012-04-02 05:26:37 -03:00
|
|
|
/// @file AP_MotorsHeli.h
|
|
|
|
/// @brief Motor control class for Traditional Heli
|
2016-02-17 21:25:38 -04:00
|
|
|
#pragma once
|
2012-04-02 05:26:37 -03:00
|
|
|
|
|
|
|
#include <inttypes.h>
|
2015-07-22 10:46:53 -03:00
|
|
|
|
2015-08-11 03:28:44 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
2015-08-15 13:42:46 -03:00
|
|
|
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
2017-01-03 05:56:57 -04:00
|
|
|
#include <RC_Channel/RC_Channel.h>
|
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2015-08-15 13:42:46 -03:00
|
|
|
#include "AP_Motors_Class.h"
|
2015-07-21 07:07:54 -03:00
|
|
|
#include "AP_MotorsHeli_RSC.h"
|
2012-06-02 04:12:00 -03:00
|
|
|
|
2013-11-04 00:13:54 -04:00
|
|
|
// servo output rates
|
|
|
|
#define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters
|
2012-04-02 05:26:37 -03:00
|
|
|
|
2013-11-04 00:13:54 -04:00
|
|
|
// default swash min and max angles and positions
|
2014-09-18 22:54:26 -03:00
|
|
|
#define AP_MOTORS_HELI_SWASH_CYCLIC_MAX 2500
|
2013-11-04 00:13:54 -04:00
|
|
|
#define AP_MOTORS_HELI_COLLECTIVE_MIN 1250
|
|
|
|
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
|
|
|
|
#define AP_MOTORS_HELI_COLLECTIVE_MID 1500
|
2012-08-21 17:54:01 -03:00
|
|
|
|
2013-11-06 08:39:10 -04:00
|
|
|
// swash min while landed or landing (as a number from 0 ~ 1000
|
|
|
|
#define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0
|
|
|
|
|
2013-11-07 03:07:53 -04:00
|
|
|
// default main rotor speed (ch8 out) as a number from 0 ~ 1000
|
2015-05-28 18:37:13 -03:00
|
|
|
#define AP_MOTORS_HELI_RSC_SETPOINT 700
|
2013-11-07 03:07:53 -04:00
|
|
|
|
2015-06-18 20:38:37 -03:00
|
|
|
// default main rotor critical speed
|
|
|
|
#define AP_MOTORS_HELI_RSC_CRITICAL 500
|
|
|
|
|
2015-08-11 21:20:28 -03:00
|
|
|
// RSC output defaults
|
2015-08-07 20:58:49 -03:00
|
|
|
#define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0
|
2018-03-23 01:09:14 -03:00
|
|
|
#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 250
|
|
|
|
#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 320
|
|
|
|
#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 380
|
|
|
|
#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 500
|
|
|
|
#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 1000
|
2015-08-07 20:58:49 -03:00
|
|
|
|
2013-11-07 03:07:53 -04:00
|
|
|
// default main rotor ramp up time in seconds
|
2018-03-23 01:09:14 -03:00
|
|
|
#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to setpoint
|
2013-11-09 00:38:33 -04:00
|
|
|
#define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed
|
2013-11-04 00:13:54 -04:00
|
|
|
|
|
|
|
// flybar types
|
|
|
|
#define AP_MOTORS_HELI_NOFLYBAR 0
|
2012-05-30 22:50:25 -03:00
|
|
|
|
|
|
|
class AP_HeliControls;
|
|
|
|
|
2012-08-21 23:19:52 -03:00
|
|
|
/// @class AP_MotorsHeli
|
2012-04-02 05:26:37 -03:00
|
|
|
class AP_MotorsHeli : public AP_Motors {
|
2012-08-21 23:19:52 -03:00
|
|
|
public:
|
|
|
|
|
|
|
|
/// Constructor
|
2015-07-22 10:46:53 -03:00
|
|
|
AP_MotorsHeli( uint16_t loop_rate,
|
2012-08-21 23:19:52 -03:00
|
|
|
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
|
2015-07-22 10:46:53 -03:00
|
|
|
AP_Motors(loop_rate, speed_hz)
|
2012-08-21 23:19:52 -03:00
|
|
|
{
|
2015-06-16 08:56:00 -03:00
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// initialise flags
|
|
|
|
_heliflags.landing_collective = 0;
|
2015-05-28 18:27:16 -03:00
|
|
|
_heliflags.rotor_runup_complete = 0;
|
2013-11-04 07:53:27 -04:00
|
|
|
};
|
2012-08-21 23:19:52 -03:00
|
|
|
|
|
|
|
// init
|
2016-12-14 01:46:42 -04:00
|
|
|
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);
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2012-09-13 09:31:13 -03:00
|
|
|
// set update rate to motors - a value in hertz
|
2015-07-22 10:46:53 -03:00
|
|
|
virtual void set_update_rate( uint16_t speed_hz ) = 0;
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2015-08-12 14:22:39 -03:00
|
|
|
// output_min - sets servos to neutral point with motors stopped
|
|
|
|
void output_min();
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2018-04-27 13:22:07 -03:00
|
|
|
// output_test_seq - spin a motor at the pwm value specified
|
2014-04-28 04:29:49 -03:00
|
|
|
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
|
|
|
|
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
|
2018-04-27 13:22:07 -03:00
|
|
|
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override = 0;
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
//
|
|
|
|
// heli specific methods
|
|
|
|
//
|
|
|
|
|
2015-07-08 14:14:49 -03:00
|
|
|
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
|
2015-11-30 16:16:04 -04:00
|
|
|
virtual bool parameter_check(bool display_msg) const;
|
2015-07-08 14:14:49 -03:00
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// has_flybar - returns true if we have a mechical flybar
|
2015-07-22 10:46:53 -03:00
|
|
|
virtual bool has_flybar() const { return AP_MOTORS_HELI_NOFLYBAR; }
|
2013-11-04 07:53:27 -04:00
|
|
|
|
|
|
|
// set_collective_for_landing - limits collective from going too low if we know we are landed
|
|
|
|
void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
|
|
|
|
|
2017-08-27 20:51:33 -03:00
|
|
|
// set_inverted_flight - enables/disables inverted flight
|
|
|
|
void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; }
|
2018-03-08 13:16:31 -04:00
|
|
|
|
2015-08-11 15:31:20 -03:00
|
|
|
// get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
|
2014-01-30 03:30:04 -04:00
|
|
|
uint8_t get_rsc_mode() const { return _rsc_mode; }
|
2013-11-08 04:29:54 -04:00
|
|
|
|
2016-02-03 04:58:44 -04:00
|
|
|
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
|
2016-05-24 15:12:26 -03:00
|
|
|
float get_rsc_setpoint() const { return _rsc_setpoint / 1000.0f; }
|
2013-11-08 04:29:54 -04:00
|
|
|
|
2016-02-03 04:58:44 -04:00
|
|
|
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
|
|
|
|
virtual void set_desired_rotor_speed(float desired_speed) = 0;
|
2013-11-08 04:29:54 -04:00
|
|
|
|
2016-02-03 04:58:44 -04:00
|
|
|
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
|
|
|
|
virtual float get_desired_rotor_speed() const = 0;
|
2015-06-18 16:50:28 -03:00
|
|
|
|
2015-08-11 15:31:20 -03:00
|
|
|
// get_main_rotor_speed - gets estimated or measured main rotor speed
|
2016-02-03 04:58:44 -04:00
|
|
|
virtual float get_main_rotor_speed() const = 0;
|
2015-06-18 16:50:28 -03:00
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// return true if the main rotor is up to speed
|
2015-08-10 21:08:27 -03:00
|
|
|
bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }
|
2012-08-21 23:19:52 -03:00
|
|
|
|
2015-07-08 16:09:54 -03:00
|
|
|
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
2015-07-22 10:46:53 -03:00
|
|
|
virtual bool rotor_speed_above_critical() const = 0;
|
2015-07-08 16:09:54 -03:00
|
|
|
|
2014-07-26 04:29:12 -03:00
|
|
|
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
|
|
|
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
2015-07-22 10:46:53 -03:00
|
|
|
virtual uint16_t get_motor_mask() = 0;
|
2014-07-26 04:29:12 -03:00
|
|
|
|
2017-03-14 06:46:48 -03:00
|
|
|
virtual void set_acro_tail(bool set) {}
|
|
|
|
|
|
|
|
// ext_gyro_gain - set external gyro gain in range 0 ~ 1
|
|
|
|
virtual void ext_gyro_gain(float gain) {}
|
2018-03-08 13:16:31 -04:00
|
|
|
|
2015-05-21 15:33:18 -03:00
|
|
|
// output - sends commands to the motors
|
2016-12-12 06:23:35 -04:00
|
|
|
void output();
|
2015-05-21 15:33:18 -03:00
|
|
|
|
2016-02-17 22:28:54 -04:00
|
|
|
// supports_yaw_passthrough
|
|
|
|
virtual bool supports_yaw_passthrough() const { return false; }
|
|
|
|
|
2016-07-01 02:35:29 -03:00
|
|
|
float get_throttle_hover() const { return 0.5f; }
|
2016-03-22 10:57:26 -03:00
|
|
|
|
2016-02-17 22:28:54 -04:00
|
|
|
// var_info for holding Parameter information
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
protected:
|
|
|
|
|
2015-10-14 15:57:51 -03:00
|
|
|
// manual servo modes (used for setup)
|
|
|
|
enum ServoControlModes {
|
2016-02-02 08:35:58 -04:00
|
|
|
SERVO_CONTROL_MODE_AUTOMATED = 0,
|
2015-10-14 15:57:51 -03:00
|
|
|
SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH,
|
2015-10-13 14:39:58 -03:00
|
|
|
SERVO_CONTROL_MODE_MANUAL_MAX,
|
2015-10-14 15:57:51 -03:00
|
|
|
SERVO_CONTROL_MODE_MANUAL_CENTER,
|
2015-10-13 14:39:58 -03:00
|
|
|
SERVO_CONTROL_MODE_MANUAL_MIN,
|
2015-10-21 17:00:36 -03:00
|
|
|
SERVO_CONTROL_MODE_MANUAL_OSCILLATE,
|
2015-10-14 15:57:51 -03:00
|
|
|
};
|
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// output - sends commands to the motors
|
2016-12-12 06:23:35 -04:00
|
|
|
void output_armed_stabilizing();
|
|
|
|
void output_armed_zero_throttle();
|
|
|
|
void output_disarmed();
|
2015-08-12 14:22:39 -03:00
|
|
|
|
|
|
|
// update_motor_controls - sends commands to motor controllers
|
2015-08-28 03:23:26 -03:00
|
|
|
virtual void update_motor_control(RotorControlState state) = 0;
|
2015-08-12 14:22:39 -03:00
|
|
|
|
|
|
|
// reset_flight_controls - resets all controls and scalars to flight status
|
|
|
|
void reset_flight_controls();
|
2013-11-04 07:53:27 -04:00
|
|
|
|
2015-05-21 15:19:25 -03:00
|
|
|
// update the throttle input filter
|
2016-12-12 06:23:35 -04:00
|
|
|
void update_throttle_filter();
|
2015-05-21 15:19:25 -03:00
|
|
|
|
2015-08-12 12:41:40 -03:00
|
|
|
// move_actuators - moves swash plate and tail rotor
|
2016-02-02 08:30:16 -04:00
|
|
|
virtual void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) = 0;
|
2015-07-22 05:14:05 -03:00
|
|
|
|
|
|
|
// reset_swash_servo - free up swash servo for maximum movement
|
2017-01-03 05:56:57 -04:00
|
|
|
void reset_swash_servo(SRV_Channel *servo);
|
2015-07-22 05:14:05 -03:00
|
|
|
|
2015-08-12 12:41:40 -03:00
|
|
|
// init_outputs - initialise Servo/PWM ranges and endpoints
|
2017-01-03 05:56:57 -04:00
|
|
|
virtual bool init_outputs() = 0;
|
2015-07-22 05:14:05 -03:00
|
|
|
|
2016-02-17 22:28:54 -04:00
|
|
|
// calculate_armed_scalars - must be implemented by child classes
|
|
|
|
virtual void calculate_armed_scalars() = 0;
|
|
|
|
|
|
|
|
// calculate_scalars - must be implemented by child classes
|
|
|
|
virtual void calculate_scalars() = 0;
|
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
2015-07-22 10:46:53 -03:00
|
|
|
virtual void calculate_roll_pitch_collective_factors() = 0;
|
2015-07-21 07:07:54 -03:00
|
|
|
|
2016-02-17 22:28:54 -04:00
|
|
|
// servo_test - move servos through full range of movement
|
|
|
|
// to be overloaded by child classes, different vehicle types would have different movement patterns
|
|
|
|
virtual void servo_test() = 0;
|
|
|
|
|
2018-04-21 00:17:50 -03:00
|
|
|
// convert input in -1 to +1 range to pwm output for swashplate servos. . Special handling of trim is required
|
|
|
|
// to keep travel between the swashplate servos consistent.
|
|
|
|
int16_t calc_pwm_output_1to1_swash_servo(float input, const SRV_Channel *servo);
|
|
|
|
|
2013-11-04 07:53:27 -04:00
|
|
|
// flags bitmask
|
|
|
|
struct heliflags_type {
|
|
|
|
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
|
2015-05-28 18:27:16 -03:00
|
|
|
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
|
2017-08-27 20:51:33 -03:00
|
|
|
uint8_t inverted_flight : 1; // true for inverted flight
|
2013-11-04 07:53:27 -04:00
|
|
|
} _heliflags;
|
|
|
|
|
|
|
|
// parameters
|
2014-09-18 22:54:26 -03:00
|
|
|
AP_Int16 _cyclic_max; // Maximum cyclic angle of the swash plate in centi-degrees
|
2013-11-04 07:53:27 -04:00
|
|
|
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
|
|
|
|
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
|
2016-05-12 13:59:30 -03:00
|
|
|
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
|
2015-10-14 15:57:51 -03:00
|
|
|
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
|
2013-11-07 03:07:53 -04:00
|
|
|
AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv
|
|
|
|
AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active
|
2018-03-08 13:16:31 -04:00
|
|
|
AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach setpoint
|
2013-11-09 00:38:33 -04:00
|
|
|
AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
|
2013-11-06 08:39:10 -04:00
|
|
|
AP_Int16 _land_collective_min; // Minimum collective when landed or landing
|
2015-06-18 20:38:37 -03:00
|
|
|
AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible
|
2015-08-11 21:20:28 -03:00
|
|
|
AP_Int16 _rsc_idle_output; // Rotor control output while at idle
|
2018-03-23 01:09:14 -03:00
|
|
|
AP_Int16 _rsc_thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective
|
2016-06-29 23:28:34 -03:00
|
|
|
AP_Int16 _rsc_slewrate; // throttle slew rate (percentage per second)
|
2015-10-21 20:47:24 -03:00
|
|
|
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
|
2013-11-04 07:53:27 -04:00
|
|
|
|
|
|
|
// internal variables
|
2016-02-05 21:39:20 -04:00
|
|
|
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
|
2015-10-21 20:47:24 -03:00
|
|
|
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
|
2017-09-30 03:30:02 -03:00
|
|
|
|
|
|
|
motor_frame_type _frame_type;
|
2012-08-21 23:19:52 -03:00
|
|
|
};
|