diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index d1da5dfe01..231519876e 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -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; diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index eb00361442..dbcb4c9e0d 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -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 diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 7b302b3710..b27507c2f7 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -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