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

Copter: AP_Math: Move rc_input_to_roll_pitch conversion to AP_Math

This commit is contained in:
Leonard Hall 2022-04-23 11:59:38 +09:30 committed by Randy Mackay
parent 1a21c6da1e
commit 0c008fe8a9
1 changed files with 6 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduCopter/mode.cpp
View File

@ -406,27 +406,14 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd
return;
}
float rc_2_rad = radians(angle_max_cd * 0.01) / (float)ROLL_PITCH_YAW_INPUT_MAX;
// fetch roll and pitch stick positions and convert them to normalised horizontal thrust
Vector2f thrust;
thrust.x = - tanf(rc_2_rad * channel_pitch->get_control_in());
thrust.y = tanf(rc_2_rad * channel_roll->get_control_in());
// calculate the horizontal thrust limit based on the angle limit
angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd);
float thrust_limit = tanf(radians(angle_limit_cd * 0.01));
// apply horizontal thrust limit
thrust.limit_length(thrust_limit);
// Conversion from angular thrust vector to euler angles.
float pitch_rad = - atanf(thrust.x);
float roll_rad = atanf(cosf(pitch_rad) * thrust.y);
//transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
float roll_out_deg;
float pitch_out_deg;
rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01, angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg);
// Convert to centi-degrees
roll_out_cd = degrees(roll_rad) * 100.0;
pitch_out_cd = degrees(pitch_rad) * 100.0;
roll_out_cd = roll_out_deg * 100.0;
pitch_out_cd = pitch_out_deg * 100.0;
}
// transform pilot's roll or pitch input into a desired velocity