Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: Added altitude offset based on alt_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
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
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
Vector3f vel; // the vehicle's velocity in m/s
} vehicle;

3
AntennaTracker/defines.h
View File

@ -24,10 +24,9 @@ enum ServoType {
SERVO_TYPE_CR=2
};
enum AltSource{
enum AltSource {
ALT_SOURCE_BARO=0,
ALT_SOURCE_GPS=1
};
// Logging parameters

11
AntennaTracker/tracking.cpp
View File

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