ardupilot/libraries/AP_Motors/AP_MotorsHeli_Dual.h
bnsgeyer dec8c5de77 AP_Motors: convert heli code to use SRV_Channels
this converts the heli code to use the SRV_Channels output
functions. It does not change behaviour, but removes the last vehicle
type that did its own servo output calculations.  This change also
fixed servo initialization conflicts.

Note that this also allows helis to be setup with more than one
channel for a particular output (eg. two separate channels for tail
servo if they are wanted). This isn't likely to be used much, but does
make heli consistent with other vehicle types
2018-07-16 12:41:16 +10:00

148 lines
7.1 KiB
C++

/// @file AP_MotorsHeli_Dual.h
/// @brief Motor control class for dual heli (tandem or transverse)
/// @author Fredrik Hedberg
#ifndef __AP_MOTORS_HELI_DUAL_H__
#define __AP_MOTORS_HELI_DUAL_H__
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include "AP_MotorsHeli.h"
#include "AP_MotorsHeli_RSC.h"
// servo position defaults
#define AP_MOTORS_HELI_DUAL_SERVO1_POS -60
#define AP_MOTORS_HELI_DUAL_SERVO2_POS 60
#define AP_MOTORS_HELI_DUAL_SERVO3_POS 180
#define AP_MOTORS_HELI_DUAL_SERVO4_POS -60
#define AP_MOTORS_HELI_DUAL_SERVO5_POS 60
#define AP_MOTORS_HELI_DUAL_SERVO6_POS 180
// collective control direction definitions
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_NORMAL 0
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_REVERSED 1
// rsc function output channel
#define AP_MOTORS_HELI_DUAL_RSC CH_8
// tandem modes
#define AP_MOTORS_HELI_DUAL_MODE_TANDEM 0 // tandem mode (rotors front and aft)
#define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE 1 // transverse mode (rotors side by side)
// default differential-collective-pitch scaler
#define AP_MOTORS_HELI_DUAL_DCP_SCALER 0.25f
// maximum number of swashplate servos
#define AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS 6
// default collective min, max and midpoints for the rear swashplate
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN 1250
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX 1750
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID 1500
/// @class AP_MotorsHeli_Dual
class AP_MotorsHeli_Dual : public AP_MotorsHeli {
public:
// constructor
AP_MotorsHeli_Dual(uint16_t loop_rate,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_MotorsHeli(loop_rate, speed_hz),
_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_DUAL_RSC)
{
AP_Param::setup_object_defaults(this, var_info);
};
// set_update_rate - set update rate to motors
void set_update_rate( uint16_t speed_hz ) override;
// output_test_seq - spin a motor at the pwm value specified
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(float desired_speed) override;
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
float get_main_rotor_speed() const override { return _rotor.get_rotor_speed(); }
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
float get_desired_rotor_speed() const override { return _rotor.get_rotor_speed(); }
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const override { return _rotor.get_rotor_speed() > _rotor.get_critical_speed(); }
// calculate_scalars - recalculates various scalars used
void calculate_scalars() override;
// calculate_armed_scalars - recalculates scalars that can change while armed
void calculate_armed_scalars() override;
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
uint16_t get_motor_mask() override;
// has_flybar - returns true if we have a mechical flybar
bool has_flybar() const override { return AP_MOTORS_HELI_NOFLYBAR; }
// supports_yaw_passthrought - returns true if we support yaw passthrough
bool supports_yaw_passthrough() const override { return false; }
// servo_test - move servos through full range of movement
void servo_test() override;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// init_outputs
bool init_outputs () override;
// update_motor_controls - sends commands to motor controllers
void update_motor_control(RotorControlState state) override;
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors () override;
// move_actuators - moves swash plate to attitude of parameters passed in
void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override;
// objects we depend upon
AP_MotorsHeli_RSC _rotor; // main rotor controller
// internal variables
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function
float _servo_test_cycle_time = 0.0f; // cycle time tracker, used by servo_test function
float _collective_test = 0.0f; // over-ride for collective output, used by servo_test function
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
// parameters
AP_Int16 _collective2_min; // Lowest possible servo position for the rear swashplate
AP_Int16 _collective2_max; // Highest possible servo position for the rear swashplate
AP_Int16 _collective2_mid; // Swash servo position corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
AP_Int16 _servo1_pos; // angular location of swash servo #1
AP_Int16 _servo2_pos; // angular location of swash servo #2
AP_Int16 _servo3_pos; // angular location of swash servo #3
AP_Int16 _servo4_pos; // angular location of swash servo #4
AP_Int16 _servo5_pos; // angular location of swash servo #5
AP_Int16 _servo6_pos; // angular location of swash servo #6
AP_Int8 _collective_direction; // Collective control direction, normal or reversed
AP_Int16 _swash1_phase_angle; // phase angle correction for 1st swash.
AP_Int16 _swash2_phase_angle; // phase angle correction for 2nd swash.
AP_Int8 _dual_mode; // which dual mode the heli is
AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
AP_Float _yaw_scaler; // scaling factor applied to the yaw mixing
// internal variables
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
float _rollFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
float _pitchFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
float _collectiveFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
float _yawFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
};
#endif // AP_MotorsHeli_Dual