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

Plane: revert AP_Math class change

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-05 12:34:27 +10:00
parent d8146ff3f6
commit 8395b92309
4 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Attitude.pde
View File

@ -286,7 +286,7 @@ static void stabilize_acro(float speed_scaler)
/* /*
check for special roll handling near the pitch poles check for special roll handling near the pitch poles
*/ */
if (g.acro_locking && AP_Math::is_zero(roll_rate)) { if (g.acro_locking && is_zero(roll_rate)) {
/* /*
we have no roll stick input, so we will enter "roll locked" we have no roll stick input, so we will enter "roll locked"
mode, and hold the roll we had when the stick was released mode, and hold the roll we had when the stick was released
@ -313,7 +313,7 @@ static void stabilize_acro(float speed_scaler)
channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler); channel_roll->servo_out = rollController.get_rate_out(roll_rate, speed_scaler);
} }
if (g.acro_locking && AP_Math::is_zero(pitch_rate)) { if (g.acro_locking && is_zero(pitch_rate)) {
/* /*
user has zero pitch stick input, so we lock pitch at the user has zero pitch stick input, so we lock pitch at the
point they release the stick point they release the stick
@ -459,7 +459,7 @@ static void calc_nav_yaw_ground(void)
if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) { if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
steer_rate = 0; steer_rate = 0;
} }
if (!AP_Math::is_zero(steer_rate)) { if (!is_zero(steer_rate)) {
// pilot is giving rudder input // pilot is giving rudder input
steer_state.locked_course = false; steer_state.locked_course = false;
} else if (!steer_state.locked_course) { } else if (!steer_state.locked_course) {

26
ArduPlane/GCS_Mavlink.pde
View File

@ -1049,7 +1049,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_PREFLIGHT_CALIBRATION: case MAV_CMD_PREFLIGHT_CALIBRATION:
in_calibration = true; in_calibration = true;
if (AP_Math::is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
ins.init_gyro(); ins.init_gyro();
if (ins.gyro_calibrated_ok_all()) { if (ins.gyro_calibrated_ok_all()) {
ahrs.reset_gyro_drift(); ahrs.reset_gyro_drift();
@ -1057,16 +1057,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else { } else {
result = MAV_RESULT_FAILED; result = MAV_RESULT_FAILED;
} }
} else if (AP_Math::is_equal(packet.param3,1.0f)) { } else if (is_equal(packet.param3,1.0f)) {
init_barometer(); init_barometer();
if (airspeed.enabled()) { if (airspeed.enabled()) {
zero_airspeed(false); zero_airspeed(false);
} }
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else if (AP_Math::is_equal(packet.param4,1.0f)) { } else if (is_equal(packet.param4,1.0f)) {
trim_radio(); trim_radio();
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else if (AP_Math::is_equal(packet.param5,1.0f)) { } else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch; float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this); AP_InertialSensor_UserInteract_MAVLink interact(this);
if (g.skip_gyro_cal) { if (g.skip_gyro_cal) {
@ -1089,12 +1089,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
if (AP_Math::is_equal(packet.param1,2.0f)) { if (is_equal(packet.param1,2.0f)) {
// save first compass's offsets // save first compass's offsets
compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} }
if (AP_Math::is_equal(packet.param1,5.0f)) { if (is_equal(packet.param1,5.0f)) {
// save secondary compass's offsets // save secondary compass's offsets
compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
@ -1102,14 +1102,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
if (AP_Math::is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
// run pre_arm_checks and arm_checks and display failures // run pre_arm_checks and arm_checks and display failures
if (arm_motors(AP_Arming::MAVLINK)) { if (arm_motors(AP_Arming::MAVLINK)) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_FAILED; result = MAV_RESULT_FAILED;
} }
} else if (AP_Math::is_zero(packet.param1)) { } else if (is_zero(packet.param1)) {
if (disarm_motors()) { if (disarm_motors()) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} else { } else {
@ -1170,9 +1170,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (AP_Math::is_equal(packet.param1,1.0f) || AP_Math::is_equal(packet.param1,3.0f)) { if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader // when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(AP_Math::is_equal(packet.param1,3.0f)); hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} }
break; break;
@ -1227,7 +1227,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (AP_Math::is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} }
@ -1239,10 +1239,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// param6 : longitude // param6 : longitude
// param7 : altitude (absolute) // param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure result = MAV_RESULT_FAILED; // assume failure
if (AP_Math::is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
init_home(); init_home();
} else { } else {
if (AP_Math::is_zero(packet.param5) && AP_Math::is_zero(packet.param6) && AP_Math::is_zero(packet.param7)) { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
// don't allow the 0,0 position // don't allow the 0,0 position
break; break;
} }

2
ArduPlane/navigation.pde
View File

@ -182,7 +182,7 @@ static void update_fbwb_speed_height(void)
change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f); change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f);
if (AP_Math::is_zero(elevator_input) && !AP_Math::is_zero(last_elevator_input)) { if (is_zero(elevator_input) && !is_zero(last_elevator_input)) {
// the user has just released the elevator, lock in // the user has just released the elevator, lock in
// the current altitude // the current altitude
set_target_altitude_current(); set_target_altitude_current();

4
ArduPlane/takeoff.pde
View File

@ -37,7 +37,7 @@ static bool auto_takeoff_check(void)
// Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing // Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
if (!launchTimerStarted && if (!launchTimerStarted &&
!AP_Math::is_zero(g.takeoff_throttle_min_accel) && !is_zero(g.takeoff_throttle_min_accel) &&
SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) { SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
goto no_launch; goto no_launch;
} }
@ -65,7 +65,7 @@ static bool auto_takeoff_check(void)
} }
// Check ground speed and time delay // Check ground speed and time delay
if (((gps.ground_speed() > g.takeoff_throttle_min_speed || AP_Math::is_zero(g.takeoff_throttle_min_speed))) && if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) &&
((now - last_tkoff_arm_time) >= wait_time_ms)) { ((now - last_tkoff_arm_time) >= wait_time_ms)) {
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), (double)gps.ground_speed()); gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), (double)gps.ground_speed());
launchTimerStarted = false; launchTimerStarted = false;