ardupilot/libraries/AP_Motors/AP_MotorsHeli.h

277 lines
13 KiB
C++

/// @file AP_MotorsHeli.h
/// @brief Motor control class for Traditional Heli
#pragma once
#include <inttypes.h>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_Motors_Class.h"
#include "AP_MotorsHeli_RSC.h"
// servo output rates
#define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters
// default swash min and max angles and positions
#define AP_MOTORS_HELI_SWASH_CYCLIC_MAX 2500
#define AP_MOTORS_HELI_COLLECTIVE_MIN 1250
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -90.0f // minimum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 90.0f // maximum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN -1.0f // minimum landed collective blade pitch angle in deg for modes using althold
// flybar types
#define AP_MOTORS_HELI_NOFLYBAR 0
// rsc function output channels.
#define AP_MOTORS_HELI_RSC CH_8
class AP_HeliControls;
/// @class AP_MotorsHeli
class AP_MotorsHeli : public AP_Motors {
public:
/// Constructor
AP_MotorsHeli( uint16_t loop_rate,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz),
_main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC)
{
AP_Param::setup_object_defaults(this, var_info);
};
// init
void init(motor_frame_class frame_class, motor_frame_type frame_type) override;
// 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) override;
// set update rate to motors - a value in hertz
virtual void set_update_rate( uint16_t speed_hz ) override = 0;
// output_min - sets servos to neutral point with motors stopped
void output_min() override;
// output_test_seq - spin a motor at the pwm value specified
// 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
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override = 0;
//
// heli specific methods
//
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
virtual bool parameter_check(bool display_msg) const;
// has_flybar - returns true if we have a mechical flybar
virtual bool has_flybar() const { return AP_MOTORS_HELI_NOFLYBAR; }
// 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; }
// set_inverted_flight - enables/disables inverted flight
void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; }
// get_rsc_mode - gets the current rotor speed control method
uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); }
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; }
// set_rpm - for rotor speed governor
virtual void set_rpm(float rotor_rpm) = 0;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
virtual void set_desired_rotor_speed(float desired_speed) = 0;
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
virtual float get_desired_rotor_speed() const = 0;
// get_main_rotor_speed - estimated rotor speed when no governor or speed sensor used
virtual float get_main_rotor_speed() const = 0;
// return true if the main rotor is up to speed
bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
virtual bool rotor_speed_above_critical() const = 0;
//get rotor governor output
virtual float get_governor_output() const = 0;
//get engine throttle output
virtual float get_control_output() const = 0;
// 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
virtual uint16_t get_motor_mask() override = 0;
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) {}
// output - sends commands to the motors
void output() override;
// supports_yaw_passthrough
virtual bool supports_yaw_passthrough() const { return false; }
// update estimated throttle required to hover
void update_throttle_hover(float dt);
float get_throttle_hover() const override { return constrain_float(_collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX); }
// accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff
bool get_takeoff_collective() const { return _heliflags.takeoff_collective; }
// accessor to get the land min collective flag signifying that current collective is lower than collective required for landing
bool get_below_land_min_coll() const { return _heliflags.below_land_min_coll; }
// support passing init_targets_on_arming flag to greater code
bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }
// set_in_autorotation - allows main code to set when aircraft is in autorotation.
void set_in_autorotation(bool autorotation) { _heliflags.in_autorotation = autorotation; }
// set_enable_bailout - allows main code to set when RSC can immediately ramp engine instantly
void set_enable_bailout(bool bailout) { _heliflags.enable_bailout = bailout; }
// return true if the servo test is still running/pending
bool servo_test_running() const { return _heliflags.servo_test_running; }
// set land complete flag
void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
// enum for heli optional features
enum class HeliOption {
USE_LEAKY_I = (1<<0), // 1
};
// use leaking integrator management scheme
bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); }
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
const char* get_frame_string() const override { return "HELI"; }
protected:
// manual servo modes (used for setup)
enum ServoControlModes {
SERVO_CONTROL_MODE_AUTOMATED = 0,
SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH,
SERVO_CONTROL_MODE_MANUAL_MAX,
SERVO_CONTROL_MODE_MANUAL_CENTER,
SERVO_CONTROL_MODE_MANUAL_MIN,
SERVO_CONTROL_MODE_MANUAL_OSCILLATE,
};
// output - sends commands to the motors
void output_armed_stabilizing() override;
void output_armed_zero_throttle();
void output_disarmed();
// external objects we depend upon
AP_MotorsHeli_RSC _main_rotor; // main rotor
// update_motor_controls - sends commands to motor controllers
virtual void update_motor_control(RotorControlState state) = 0;
// run spool logic
void output_logic();
// output_to_motors - sends commands to the motors
virtual void output_to_motors() = 0;
// reset_flight_controls - resets all controls and scalars to flight status
void reset_flight_controls();
// update the throttle input filter
void update_throttle_filter() override;
// move_actuators - moves swash plate and tail rotor
virtual void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) = 0;
// reset_swash_servo - free up swash servo for maximum movement
void reset_swash_servo(SRV_Channel::Aux_servo_function_t function);
// init_outputs - initialise Servo/PWM ranges and endpoints
virtual bool init_outputs() = 0;
// 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;
// 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;
// write to a swash servo. output value is pwm
void rc_write_swash(uint8_t chan, float swash_in);
// save parameters as part of disarming
void save_params_on_disarm() override;
// Determines if _heli_options bit is set
bool heli_option(HeliOption opt) const;
// updates the takeoff collective flag indicating that current collective is greater than collective required to indicate takeoff.
void update_takeoff_collective_flag(float coll_out);
// enum values for HOVER_LEARN parameter
enum HoverLearn {
HOVER_LEARN_DISABLED = 0,
HOVER_LEARN_ONLY = 1,
HOVER_LEARN_AND_SAVE = 2
};
// flags bitmask
struct heliflags_type {
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
uint8_t inverted_flight : 1; // true for inverted flight
uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming
uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed
uint8_t in_autorotation : 1; // true if aircraft is in autorotation
uint8_t enable_bailout : 1; // true if allowing RSC to quickly ramp up engine
uint8_t servo_test_running : 1; // true if servo_test is running
uint8_t land_complete : 1; // true if aircraft is landed
uint8_t takeoff_collective : 1; // true if collective is above 30% between H_COL_MID and H_COL_MAX
uint8_t below_land_min_coll : 1; // true if collective is below H_COL_LAND_MIN
} _heliflags;
// parameters
AP_Int16 _cyclic_max; // Maximum cyclic angle of the swash plate in centi-degrees
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
AP_Float _collective_hover; // estimated collective required to hover throttle in the range 0 ~ 1
AP_Int8 _collective_hover_learn; // enable/disabled hover collective learning
AP_Int8 _heli_options; // bitmask for optional features
AP_Float _collective_zero_thrust_deg;// Zero thrust blade collective pitch in degrees
AP_Float _collective_land_min_deg; // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
AP_Float _collective_max_deg; // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX)
AP_Float _collective_min_deg; // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
// internal variables
float _collective_zero_thrust_pct; // collective zero thrutst parameter value converted to 0 ~ 1 range
float _collective_land_min_pct; // collective land min parameter value converted to 0 ~ 1 range
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
motor_frame_type _frame_type;
motor_frame_class _frame_class;
};