aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Change M_PI to match Copter.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-02-27 11:35:34 -08:00 committed by Andrew Tridgell
parent 584171cf3c
commit de669c1d41
3 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Attitude.cpp
View File

@ -33,7 +33,7 @@ void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &ro
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI_F) * atanf(cosf(pitch_in*(M_PI_F/18000))*tanf(roll_in*(M_PI_F/18000)));
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
// return
roll_out = roll_in;

2
ArduSub/commands_logic.cpp
View File

@ -666,7 +666,7 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/M_2PI_F) >= LOWBYTE(cmd.p1);
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
// verify_RTL - handles any state changes required to implement RTL

4
ArduSub/control_poshold.cpp
View File

@ -625,8 +625,8 @@ void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pit
poshold.wind_comp_timer = 0;
// convert earth frame desired accelerations to body frame roll and pitch lean angles
roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI_F);
pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI_F);
roll_angle = atanf((-poshold.wind_comp_ef.x*ahrs.sin_yaw() + poshold.wind_comp_ef.y*ahrs.cos_yaw())/981)*(18000/M_PI);
pitch_angle = atanf(-(poshold.wind_comp_ef.x*ahrs.cos_yaw() + poshold.wind_comp_ef.y*ahrs.sin_yaw())/981)*(18000/M_PI);
}
// poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis