Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: cope with H vs X frame in HeliQuad

Browse Source
This commit is contained in:
Andrew Tridgell 2017-09-30 16:30:02 +10:00
parent e2f710523e
commit e1f8e7bc58
4 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -181,6 +181,9 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// init
void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// remember frame type
_frame_type = frame_type;
// set update rate
set_update_rate(_speed_hz);

2
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -213,4 +213,6 @@ protected:
// internal variables
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
motor_frame_type _frame_type;
};

9
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -165,13 +165,18 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors()
// assume X quad layout, with motors at 45, 135, 225 and 315 degrees
// order FrontRight, RearLeft, FrontLeft, RearLeft
const float angles[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { 45, 225, 315, 135 };
const bool clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true };
const bool x_clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true };
const float cos45 = cosf(radians(45));
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
bool clockwise = x_clockwise[i];
if (_frame_type == MOTOR_FRAME_TYPE_H) {
// reverse yaw for H frame
clockwise = !clockwise;
}
_rollFactor[CH_1+i] = -0.5*sinf(radians(angles[i]))/cos45;
_pitchFactor[CH_1+i] = 0.5*cosf(radians(angles[i]))/cos45;
_yawFactor[CH_1+i] = clockwise[i]?-0.5:0.5;
_yawFactor[CH_1+i] = clockwise?-0.5:0.5;
_collectiveFactor[CH_1+i] = 1;
}
}

1
libraries/AP_Motors/AP_MotorsHeli_Quad.h
View File

@ -30,7 +30,6 @@ public:
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;