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

Tracker: Change vehicle info used to calculate offset

This commit is contained in:
stefanlynka 2016-06-13 12:52:03 +09:00 committed by Randy Mackay
parent 17355baa83
commit ce3f3012c5
3 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
AntennaTracker/Log.cpp
View File

@ -51,12 +51,13 @@ struct PACKED log_Vehicle_Pos {
int32_t vehicle_lat;
int32_t vehicle_lng;
int32_t vehicle_alt;
float vehicle_heading;
float vehicle_speed;
float vehicle_vel_x;
float vehicle_vel_y;
float vehicle_vel_z;
};
// Write a vehicle pos packet
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, float heading, float ground_speed)
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel)
{
struct log_Vehicle_Pos pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
@ -64,8 +65,9 @@ void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, float
vehicle_lat : lat,
vehicle_lng : lng,
vehicle_alt : alt,
vehicle_heading : heading,
vehicle_speed : ground_speed,
vehicle_vel_x : vel.x,
vehicle_vel_y : vel.y,
vehicle_vel_z : vel.z,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -75,7 +77,7 @@ const struct LogStructure Tracker::log_structure[] = {
{LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
"VBAR", "Qff", "TimeUS,Press,AltDiff" },
{LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
"VPOS", "QLLeff", "TimeUS,Lat,Lng,Alt,Heading,Speed" }
"VPOS", "QLLefff", "TimeUS,Lat,Lng,Alt,VelX,VelY,VelZ" }
};
void Tracker::Log_Write_Vehicle_Startup_Messages()

5
AntennaTracker/Tracker.h
View File

@ -149,8 +149,7 @@ private:
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_ms; // last position update in milliseconds
float heading; // last known direction vehicle is moving
float ground_speed; // vehicle's last known ground speed in m/s
Vector3f vel; // the vehicle's velocity in m/s
} vehicle;
// Navigation controller state
@ -255,7 +254,7 @@ private:
void compass_cal_update();
void Log_Write_Attitude();
void Log_Write_Baro(void);
void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt,float heading,float ground_speed);
void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt, const Vector3f& vel);
void Log_Write_Vehicle_Baro(float pressure, float altitude);
void Log_Write_Vehicle_Startup_Messages();
void start_logging();

10
AntennaTracker/tracking.cpp
View File

@ -15,7 +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;
location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
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);
location_offset(vehicle.location_estimate, north_offset, east_offset);
// set valid_location flag
vehicle.location_valid = true;
} else {
@ -124,14 +127,13 @@ 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.heading = msg.hdg * 0.01f;
vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f;
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();
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.heading, vehicle.ground_speed);
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
}
}