From fd2eb8bb314e168248c1d02f4e149624e5e2c484 Mon Sep 17 00:00:00 2001 From: stefanlynka Date: Wed, 6 Jul 2016 16:49:52 +0900 Subject: [PATCH] Tracker: Adding altitude difference calculation using relative altitude. --- AntennaTracker/Parameters.cpp | 4 ++-- AntennaTracker/Tracker.h | 1 + AntennaTracker/defines.h | 3 ++- AntennaTracker/servos.cpp | 2 +- AntennaTracker/tracking.cpp | 8 +++++++- 5 files changed, 13 insertions(+), 5 deletions(-) diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index fac43da451..e975e51a2d 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -207,8 +207,8 @@ const AP_Param::Info Tracker::var_info[] = { // @Param: ALT_SOURCE // @DisplayName: Altitude Source - // @Description: What provides altitude information for vehicle - // @Values: 0:Barometer,1:GPS + // @Description: What provides altitude information for vehicle. Vehicle only assumes tracker has same altitude as vehicle's home + // @Values: 0:Barometer,1:GPS,2:GPS vehicle only // @User: Standard GSCALAR(alt_source, "ALT_SOURCE", 0), diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index f2d364825a..698e176993 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -150,6 +150,7 @@ private: uint32_t last_update_us; // last position update in microseconds uint32_t last_update_ms; // last position update in milliseconds Vector3f vel; // the vehicle's velocity in m/s + int32_t relative_alt; // the vehicle's relative altitude in meters * 100 } vehicle; // Navigation controller state diff --git a/AntennaTracker/defines.h b/AntennaTracker/defines.h index a60efa4ddc..57d4f0902d 100644 --- a/AntennaTracker/defines.h +++ b/AntennaTracker/defines.h @@ -26,7 +26,8 @@ enum ServoType { enum AltSource { ALT_SOURCE_BARO=0, - ALT_SOURCE_GPS=1 + ALT_SOURCE_GPS=1, + ALT_SOURCE_GPS_VEH_ONLY=2 }; // Logging parameters diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index b9a6b3a849..363376dd91 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -187,7 +187,7 @@ void Tracker::update_yaw_position_servo(float yaw) int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); int32_t yaw_cd = wrap_180_cd(yaw*100); int32_t yaw_limit_cd = g.yaw_range*100/2; - const int16_t margin = max(500, wrap_360_cd(36000-yaw_limit_cd)/2); + const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2); // Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation // the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking. diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index bb5c7b0260..4121ffa2ce 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -61,7 +61,12 @@ void Tracker::update_bearing_and_distance() nav_status.distance = get_distance(current_loc, vehicle.location_estimate); // calculate altitude difference to vehicle using gps - nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f; + if (g.alt_source == ALT_SOURCE_GPS){ + nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f; + } else { + // g.alt_source == ALT_SOURCE_GPS_VEH_ONLY + nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f; + } // calculate pitch to vehicle // To-Do: remove need for check of control_mode @@ -127,6 +132,7 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.location.lat = msg.lat; vehicle.location.lng = msg.lon; vehicle.location.alt = msg.alt/10; + vehicle.relative_alt = msg.relative_alt/10; vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f); vehicle.last_update_us = AP_HAL::micros(); vehicle.last_update_ms = AP_HAL::millis();