Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli: Create Delta Phase Angle variable and method to set. Will be used by CCComp code.

This commit is contained in:
Robert Lefebvre 2014-05-11 19:35:26 -04:00 committed by Randy Mackay
parent 4b5167e529
commit fd542e99d8
2 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -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
// roll factors
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
_rollFactor[CH_3] = cosf(radians(_servo3_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 + _delta_phase_angle)));
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle)));
// pitch factors
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle)));
// collective factors
_collectiveFactor[CH_1] = 1;

9
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -116,7 +116,8 @@ public:
_rsc_runup_increment(0.0f),
_rotor_speed_estimate(0.0f),
_tail_direct_drive_out(0),
_dt(0.01f)
_dt(0.01f),
_delta_phase_angle(0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -191,8 +192,11 @@ public:
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
//
// set_dt for setting main loop rate time
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:
@ -288,6 +292,7 @@ private:
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
float _dt; // main loop time
int16_t _delta_phase_angle; // phase angle dynamic compensation
};
#endif // AP_MOTORSHELI