Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: fixed manual control, and added baro calibration

Browse Source 
this allows baro offsets to be compensated for before takeoff
This commit is contained in:
Andrew Tridgell 2014-04-09 14:29:23 +10:00
parent a15e4633b7
commit 8debcba4b1
3 changed files with 23 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AntennaTracker/AntennaTracker.pde
View File

@ -105,6 +105,10 @@ static struct {
float distance; float distance;
float pitch; float pitch;
float altitude_difference; float altitude_difference;
float altitude_offset;
bool manual_control_yaw:1;
bool manual_control_pitch:1;
bool need_altitude_calibration:1;
} nav_status; } nav_status;
static uint32_t start_time_ms; static uint32_t start_time_ms;

2
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -666,6 +666,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
calibrate_ins(); calibrate_ins();
} else if (packet.param3 == 1) { } else if (packet.param3 == 1) {
init_barometer(); init_barometer();
// zero the altitude difference on next baro update
nav_status.need_altitude_calibration = true;
} }
if (packet.param4 == 1) { if (packet.param4 == 1) {
// Cant trim radio // Cant trim radio

20
Tools/AntennaTracker/tracking.pde
View File

@ -230,8 +230,12 @@ static void update_tracking(void)
float pitch = degrees(atan2f(nav_status.altitude_difference, distance)); float pitch = degrees(atan2f(nav_status.altitude_difference, distance));
// update nav_status for NAV_CONTROLLER_OUTPUT // update nav_status for NAV_CONTROLLER_OUTPUT
nav_status.bearing = bearing; if (!nav_status.manual_control_yaw) {
nav_status.pitch = pitch; nav_status.bearing = bearing;
}
if (!nav_status.manual_control_pitch) {
nav_status.pitch = pitch;
}
nav_status.distance = distance; nav_status.distance = distance;
switch (control_mode) { 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 // calculate altitude difference based on difference in barometric pressure
float alt_diff = logf(scaling) * (ground_temp+273.15f) * 29271.267 * 0.001f; float alt_diff = logf(scaling) * (ground_temp+273.15f) * 29271.267 * 0.001f;
if (!isnan(alt_diff)) { 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.bearing = msg.x;
nav_status.pitch = msg.y; nav_status.pitch = msg.y;
nav_status.distance = 0.0; 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 // z, r and buttons are not used
} }