mirror of https://github.com/ArduPilot/ardupilot
93 lines
3.7 KiB
C++
93 lines
3.7 KiB
C++
/// @file AP_MotorsHeli_Swash.h
|
|
/// @brief Swashplate Library for traditional heli
|
|
#pragma once
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
// swashplate types
|
|
enum SwashPlateType {
|
|
SWASHPLATE_TYPE_H3 = 0,
|
|
SWASHPLATE_TYPE_H1,
|
|
SWASHPLATE_TYPE_H3_140,
|
|
SWASHPLATE_TYPE_H3_120,
|
|
SWASHPLATE_TYPE_H4_90,
|
|
SWASHPLATE_TYPE_H4_45
|
|
};
|
|
|
|
class AP_MotorsHeli_Swash {
|
|
public:
|
|
|
|
AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3);
|
|
|
|
// configure - configure the swashplate settings for any updated parameters
|
|
void configure();
|
|
|
|
// get_swash_type - gets swashplate type
|
|
SwashPlateType get_swash_type() const { return _swash_type; }
|
|
|
|
// calculates servo output
|
|
void calculate(float roll, float pitch, float collective);
|
|
|
|
// Output calculated values to servos
|
|
void output();
|
|
|
|
// get_phase_angle - returns the rotor phase angle
|
|
int16_t get_phase_angle() const { return _phase_angle; }
|
|
|
|
// Get function output mask
|
|
uint32_t get_output_mask() const;
|
|
|
|
// var_info
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
private:
|
|
|
|
// linearize mechanical output of swashplate servo
|
|
float get_linear_servo_output(float input) const;
|
|
|
|
// CCPM Mixers - calculate mixing scale factors by swashplate type
|
|
void calculate_roll_pitch_collective_factors();
|
|
|
|
// Setup a servo
|
|
void add_servo_angle(uint8_t num, float angle, float collective);
|
|
void add_servo_raw(uint8_t num, float roll, float pitch, float collective);
|
|
|
|
// write to a swash servo. output value is pwm
|
|
void rc_write(uint8_t chan, float swash_in);
|
|
|
|
enum CollectiveDirection {
|
|
COLLECTIVE_DIRECTION_NORMAL = 0,
|
|
COLLECTIVE_DIRECTION_REVERSED
|
|
};
|
|
|
|
static const uint8_t _max_num_servos {4};
|
|
|
|
// Currently configured setup
|
|
SwashPlateType _swash_type; // Swashplate type
|
|
CollectiveDirection _collective_direction; // Collective control direction, normal or reversed
|
|
bool _make_servo_linear; // Sets servo output to be linearized
|
|
|
|
// Internal variables
|
|
bool _enabled[_max_num_servos]; // True if this output servo is enabled
|
|
float _rollFactor[_max_num_servos]; // Roll axis scaling of servo output based on servo position
|
|
float _pitchFactor[_max_num_servos]; // Pitch axis scaling of servo output based on servo position
|
|
float _collectiveFactor[_max_num_servos]; // Collective axis scaling of servo output based on servo position
|
|
float _output[_max_num_servos]; // Servo output value
|
|
uint8_t _motor_num[_max_num_servos]; // Motor function to use for output
|
|
|
|
// parameters
|
|
AP_Int8 _swashplate_type; // Swash Type Setting
|
|
AP_Int8 _swash_coll_dir; // Collective control direction, normal or reversed
|
|
AP_Int8 _linear_swash_servo; // linearize swashplate output
|
|
AP_Int8 enable;
|
|
AP_Int16 _servo1_pos; // servo1 azimuth position on swashplate with front of heli being 0 deg
|
|
AP_Int16 _servo2_pos; // servo2 azimuth position on swashplate with front of heli being 0 deg
|
|
AP_Int16 _servo3_pos; // servo3 azimuth position on swashplate with front of heli being 0 deg
|
|
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces
|
|
// a roll, this can be negative depending on mechanics.
|
|
|
|
};
|
|
|