mirror of https://github.com/ArduPilot/ardupilot
76 lines
3.0 KiB
C++
76 lines
3.0 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
|
|
};
|
|
|
|
// collective direction
|
|
enum CollectiveDirection {
|
|
COLLECTIVE_DIRECTION_NORMAL = 0,
|
|
COLLECTIVE_DIRECTION_REVERSED
|
|
};
|
|
|
|
class AP_MotorsHeli_Swash {
|
|
public:
|
|
|
|
AP_MotorsHeli_Swash()
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
};
|
|
|
|
// configure - configure the swashplate settings for any updated parameters
|
|
void configure();
|
|
|
|
// CCPM Mixers - calculate mixing scale factors by swashplate type
|
|
void calculate_roll_pitch_collective_factors();
|
|
|
|
// get_swash_type - gets swashplate type
|
|
SwashPlateType get_swash_type() const { return _swash_type; }
|
|
|
|
// get_servo_out - calculates servo output
|
|
float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const;
|
|
|
|
// linearize mechanical output of swashplate servo
|
|
float get_linear_servo_output(float input) const;
|
|
|
|
// get_phase_angle - returns the rotor phase angle
|
|
int16_t get_phase_angle() const { return _phase_angle; }
|
|
|
|
// var_info
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
private:
|
|
// internal variables
|
|
SwashPlateType _swash_type; // Swashplate type
|
|
CollectiveDirection _collective_direction; // Collective control direction, normal or reversed
|
|
float _rollFactor[4]; // Roll axis scaling of servo output based on servo position
|
|
float _pitchFactor[4]; // Pitch axis scaling of servo output based on servo position
|
|
float _collectiveFactor[4]; // Collective axis scaling of servo output based on servo position
|
|
int8_t _make_servo_linear; // Sets servo output to be linearized
|
|
|
|
// 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.
|
|
|
|
};
|
|
|