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

Tracker: Change vehicle info used to calculate offset

Browse Source
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_lat;
int32_t vehicle_lng; int32_t vehicle_lng;
int32_t vehicle_alt; int32_t vehicle_alt;
float vehicle_heading; float vehicle_vel_x;
float vehicle_speed; float vehicle_vel_y;
float vehicle_vel_z;
}; };
// Write a vehicle pos packet // 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 = { struct log_Vehicle_Pos pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG), 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_lat : lat,
vehicle_lng : lng, vehicle_lng : lng,
vehicle_alt : alt, vehicle_alt : alt,
vehicle_heading : heading, vehicle_vel_x : vel.x,
vehicle_speed : ground_speed, vehicle_vel_y : vel.y,
vehicle_vel_z : vel.z,
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -75,7 +77,7 @@ const struct LogStructure Tracker::log_structure[] = {
{LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro), {LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
"VBAR", "Qff", "TimeUS,Press,AltDiff" }, "VBAR", "Qff", "TimeUS,Press,AltDiff" },
{LOG_V_POS_MSG, sizeof(log_Vehicle_Pos), {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() 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 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 micxroseconds
uint32_t last_update_ms; // last position update in milliseconds uint32_t last_update_ms; // last position update in milliseconds
float heading; // last known direction vehicle is moving Vector3f vel; // the vehicle's velocity in m/s
float ground_speed; // vehicle's last known ground speed in m/s
} vehicle; } vehicle;
// Navigation controller state // Navigation controller state
@ -255,7 +254,7 @@ private:
void compass_cal_update(); void compass_cal_update();
void Log_Write_Attitude(); void Log_Write_Attitude();
void Log_Write_Baro(void); 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_Baro(float pressure, float altitude);
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();
void start_logging(); void start_logging();

10
AntennaTracker/tracking.cpp
View File

@ -15,7 +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;
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 // set valid_location flag
vehicle.location_valid = true; vehicle.location_valid = true;
} else { } else {
@ -124,14 +127,13 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.location.lat = msg.lat; vehicle.location.lat = msg.lat;
vehicle.location.lng = msg.lon; vehicle.location.lng = msg.lon;
vehicle.location.alt = msg.alt/10; vehicle.location.alt = msg.alt/10;
vehicle.heading = msg.hdg * 0.01f; vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f;
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.heading, vehicle.ground_speed); Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
} }
} }