Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[rotorcraft] rc euler read in fixed point

Browse Source 
closes #462
This commit is contained in:
Ewoud
2013-06-14 15:04:56 +02:00
committed by Felix Ruess
parent b97d8c2236
commit dab1d0ef9e
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
+7 -7
View File
@@ -84,16 +84,16 @@ float stabilization_attitude_get_heading_f(void) {
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
const float max_phi_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) / MAX_PPRZ;
const float max_theta_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) / MAX_PPRZ;
const float max_r_scale = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) / MAX_PPRZ;
sp->phi = (int32_t) (radio_control.values[RADIO_ROLL] * max_phi_scale);
sp->theta = (int32_t) (radio_control.values[RADIO_PITCH] * max_theta_scale);
const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += (int32_t) (radio_control.values[RADIO_YAW] * max_r_scale / RC_UPDATE_FREQ);
sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (autopilot_mode == AP_MODE_FORWARD) {