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

Tracker: Added altitude offset based on alt_source

Browse Source
This commit is contained in:
stefanlynka 2016-06-13 17:50:41 +09:00 committed by Randy Mackay
parent 6f88da07aa
commit ef60c202b4
3 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
AntennaTracker/Tracker.h
View File

@ -147,7 +147,7 @@ private:
bool location_valid; // true if we have a valid location for the vehicle bool location_valid; // true if we have a valid location for the vehicle
Location location; // lat, long in degrees * 10^7; alt in meters * 100 Location location; // lat, long in degrees * 10^7; alt in meters * 100
Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100 Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
uint32_t last_update_us; // last position update in micxroseconds uint32_t last_update_us; // last position update in microseconds
uint32_t last_update_ms; // last position update in milliseconds uint32_t last_update_ms; // last position update in milliseconds
Vector3f vel; // the vehicle's velocity in m/s Vector3f vel; // the vehicle's velocity in m/s
} vehicle; } vehicle;

1
AntennaTracker/defines.h
View File

@ -27,7 +27,6 @@ enum ServoType {
enum AltSource { enum AltSource {
ALT_SOURCE_BARO=0, ALT_SOURCE_BARO=0,
ALT_SOURCE_GPS=1 ALT_SOURCE_GPS=1
}; };
// Logging parameters // Logging parameters

9
AntennaTracker/tracking.cpp
View File

@ -15,10 +15,10 @@ void Tracker::update_vehicle_pos_estimate()
if (dt < TRACKING_TIMEOUT_SEC) { if (dt < TRACKING_TIMEOUT_SEC) {
// project the vehicle position to take account of lost radio packets // project the vehicle position to take account of lost radio packets
vehicle.location_estimate = vehicle.location; vehicle.location_estimate = vehicle.location;
float east_offset = vehicle.vel.x * dt; float north_offset = vehicle.vel.x * dt;
float north_offset = vehicle.vel.y * dt; float east_offset = vehicle.vel.y * dt;
//location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
location_offset(vehicle.location_estimate, north_offset, east_offset); location_offset(vehicle.location_estimate, north_offset, east_offset);
vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt;
// set valid_location flag // set valid_location flag
vehicle.location_valid = true; vehicle.location_valid = true;
} else { } else {
@ -61,7 +61,7 @@ void Tracker::update_bearing_and_distance()
nav_status.distance = get_distance(current_loc, vehicle.location_estimate); nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
// calculate altitude difference to vehicle using gps // calculate altitude difference to vehicle using gps
nav_status.alt_difference_gps = (vehicle.location.alt - current_loc.alt) / 100.0f; nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
// calculate pitch to vehicle // calculate pitch to vehicle
// To-Do: remove need for check of control_mode // To-Do: remove need for check of control_mode
@ -130,7 +130,6 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f); 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_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis(); vehicle.last_update_ms = AP_HAL::millis();
// log vehicle as GPS2 // log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) { if (should_log(MASK_LOG_GPS)) {
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel); Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);