From 8debcba4b140425119481c7bd1487f834c74d9e0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 9 Apr 2014 14:29:23 +1000
Subject: [PATCH] AntennaTracker: fixed manual control, and added baro
calibration
this allows baro offsets to be compensated for before takeoff
---
Tools/AntennaTracker/AntennaTracker.pde | 4 ++++
Tools/AntennaTracker/GCS_Mavlink.pde | 2 ++
Tools/AntennaTracker/tracking.pde | 20 +++++++++++++++++---
3 files changed, 23 insertions(+), 3 deletions(-)
diff --git a/Tools/AntennaTracker/AntennaTracker.pde b/Tools/AntennaTracker/AntennaTracker.pde
index c71867efc..0820342d6 100644
--- a/Tools/AntennaTracker/AntennaTracker.pde
+++ b/Tools/AntennaTracker/AntennaTracker.pde
@@ -105,6 +105,10 @@ static struct {
float distance;
float pitch;
float altitude_difference;
+ float altitude_offset;
+ bool manual_control_yaw:1;
+ bool manual_control_pitch:1;
+ bool need_altitude_calibration:1;
} nav_status;
static uint32_t start_time_ms;
diff --git a/Tools/AntennaTracker/GCS_Mavlink.pde b/Tools/AntennaTracker/GCS_Mavlink.pde
index 3c7b8c8ce..5b34d1a7e 100644
--- a/Tools/AntennaTracker/GCS_Mavlink.pde
+++ b/Tools/AntennaTracker/GCS_Mavlink.pde
@@ -666,6 +666,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
calibrate_ins();
} else if (packet.param3 == 1) {
init_barometer();
+ // zero the altitude difference on next baro update
+ nav_status.need_altitude_calibration = true;
}
if (packet.param4 == 1) {
// Cant trim radio
diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde
index 1f34fbea1..63e736b59 100644
--- a/Tools/AntennaTracker/tracking.pde
+++ b/Tools/AntennaTracker/tracking.pde
@@ -230,8 +230,12 @@ static void update_tracking(void)
float pitch = degrees(atan2f(nav_status.altitude_difference, distance));
// update nav_status for NAV_CONTROLLER_OUTPUT
- nav_status.bearing = bearing;
- nav_status.pitch = pitch;
+ if (!nav_status.manual_control_yaw) {
+ nav_status.bearing = bearing;
+ }
+ if (!nav_status.manual_control_pitch) {
+ nav_status.pitch = pitch;
+ }
nav_status.distance = distance;
switch (control_mode) {
@@ -277,7 +281,15 @@ static void tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
// calculate altitude difference based on difference in barometric pressure
float alt_diff = logf(scaling) * (ground_temp+273.15f) * 29271.267 * 0.001f;
if (!isnan(alt_diff)) {
- nav_status.altitude_difference = alt_diff;
+ nav_status.altitude_difference = 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.need_altitude_calibration = false;
}
}
@@ -289,5 +301,7 @@ static void tracking_manual_control(const mavlink_manual_control_t &msg)
nav_status.bearing = msg.x;
nav_status.pitch = msg.y;
nav_status.distance = 0.0;
+ nav_status.manual_control_yaw = (msg.x != 0x7FFF);
+ nav_status.manual_control_pitch = (msg.y != 0x7FFF);
// z, r and buttons are not used
}
--
GitLab