mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
dec8c5de77
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
148 lines
7.1 KiB
C++
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
|