|
|
|
@ -60,7 +60,7 @@ void Tracker::update_bearing_and_distance()
@@ -60,7 +60,7 @@ void Tracker::update_bearing_and_distance()
|
|
|
|
|
// calculate pitch to vehicle
|
|
|
|
|
// To-Do: remove need for check of control_mode
|
|
|
|
|
if (control_mode != SCAN && !nav_status.manual_control_pitch) { |
|
|
|
|
nav_status.pitch = degrees(atan2f(nav_status.altitude_difference, nav_status.distance)); |
|
|
|
|
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance)); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -140,14 +140,14 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
@@ -140,14 +140,14 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
|
|
|
|
|
// calculate altitude difference based on difference in barometric pressure
|
|
|
|
|
float alt_diff = barometer.get_altitude_difference(local_pressure, aircraft_pressure); |
|
|
|
|
if (!isnan(alt_diff)) { |
|
|
|
|
nav_status.altitude_difference = alt_diff + nav_status.altitude_offset; |
|
|
|
|
nav_status.alt_difference_baro = alt_diff + nav_status.altitude_offset; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (nav_status.need_altitude_calibration) { |
|
|
|
|
// we have done a baro calibration - zero the altitude
|
|
|
|
|
// difference to the aircraft
|
|
|
|
|
nav_status.altitude_offset = -nav_status.altitude_difference; |
|
|
|
|
nav_status.altitude_difference = 0; |
|
|
|
|
nav_status.altitude_offset = -nav_status.alt_difference_baro; |
|
|
|
|
nav_status.alt_difference_baro = 0; |
|
|
|
|
nav_status.need_altitude_calibration = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|