mirror of https://github.com/ArduPilot/ardupilot
AP_MotorsHeli: Create Delta Phase Angle variable and method to set. Will be used by CCComp code.
This commit is contained in:
parent
4b5167e529
commit
fd542e99d8
|
@ -470,14 +470,14 @@ void AP_MotorsHeli::calculate_roll_pitch_collective_factors()
|
||||||
if (_swash_type == AP_MOTORS_HELI_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
|
if (_swash_type == AP_MOTORS_HELI_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
|
||||||
|
|
||||||
// roll factors
|
// roll factors
|
||||||
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
|
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle)));
|
||||||
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
|
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle)));
|
||||||
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
|
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle)));
|
||||||
|
|
||||||
// pitch factors
|
// pitch factors
|
||||||
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
|
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle)));
|
||||||
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
|
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle)));
|
||||||
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
|
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle)));
|
||||||
|
|
||||||
// collective factors
|
// collective factors
|
||||||
_collectiveFactor[CH_1] = 1;
|
_collectiveFactor[CH_1] = 1;
|
||||||
|
|
|
@ -116,7 +116,8 @@ public:
|
||||||
_rsc_runup_increment(0.0f),
|
_rsc_runup_increment(0.0f),
|
||||||
_rotor_speed_estimate(0.0f),
|
_rotor_speed_estimate(0.0f),
|
||||||
_tail_direct_drive_out(0),
|
_tail_direct_drive_out(0),
|
||||||
_dt(0.01f)
|
_dt(0.01f),
|
||||||
|
_delta_phase_angle(0)
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
|
@ -191,8 +192,11 @@ public:
|
||||||
// var_info for holding Parameter information
|
// var_info for holding Parameter information
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
//
|
// set_dt for setting main loop rate time
|
||||||
void set_dt(float dt) { _dt = dt; }
|
void set_dt(float dt) { _dt = dt; }
|
||||||
|
|
||||||
|
// set_dt for setting main loop rate time
|
||||||
|
void set_delta_phase_angle(int16_t angle) { _delta_phase_angle = angle; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
@ -288,6 +292,7 @@ private:
|
||||||
float _rotor_speed_estimate; // estimated speed of the main rotor (0~1000)
|
float _rotor_speed_estimate; // estimated speed of the main rotor (0~1000)
|
||||||
int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
|
int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
|
||||||
float _dt; // main loop time
|
float _dt; // main loop time
|
||||||
|
int16_t _delta_phase_angle; // phase angle dynamic compensation
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // AP_MOTORSHELI
|
#endif // AP_MOTORSHELI
|
||||||
|
|
Loading…
Reference in New Issue