Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: added GPSAlt to CAM and TRIG log messages

Browse Source 
useful for people with RTK GPS
This commit is contained in:
Andrew Tridgell 2016-01-29 09:51:52 +11:00
parent 8dba91658c
commit 6f59c4ae53
3 changed files with 24 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/DataFlash/DataFlash.h
View File

@ -109,6 +109,7 @@ public:
bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
void Log_Write_Radio(const mavlink_radio_t &packet);
void Log_Write_Message(const char *message);
void Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
void Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
void Log_Write_Trigger(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc);
void Log_Write_ESC(void);
@ -179,4 +180,4 @@ private:
const char *_firmware_string;
};
#endif
#endif

44
libraries/DataFlash/LogFile.cpp
View File

@ -1538,9 +1538,9 @@ void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet)
}
// Write a Camera packet
void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc)
void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc)
{
int32_t altitude, altitude_rel;
int32_t altitude, altitude_rel, altitude_gps;
if (current_loc.flags.relative_alt) {
altitude = current_loc.alt+ahrs.get_home().alt;
altitude_rel = current_loc.alt;
@ -1548,9 +1548,14 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
altitude = current_loc.alt;
altitude_rel = current_loc.alt - ahrs.get_home().alt;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
altitude_gps = gps.location().alt;
} else {
altitude_gps = 0;
}
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
LOG_PACKET_HEADER_INIT(msg),
time_us : AP_HAL::micros64(),
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),
@ -1558,6 +1563,7 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
longitude : current_loc.lng,
altitude : altitude,
altitude_rel: altitude_rel,
altitude_gps: altitude_gps,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor
@ -1565,32 +1571,16 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c
WriteCriticalBlock(&pkt, sizeof(pkt));
}
// Write a Camera packet
void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc)
{
Log_Write_CameraInfo(LOG_CAMERA_MSG, ahrs, gps, current_loc);
}
// Write a Trigger packet
void DataFlash_Class::Log_Write_Trigger(const AP_AHRS &ahrs, const AP_GPS &gps, const Location &current_loc)
{
int32_t altitude, altitude_rel;
if (current_loc.flags.relative_alt) {
altitude = current_loc.alt+ahrs.get_home().alt;
altitude_rel = current_loc.alt;
} else {
altitude = current_loc.alt;
altitude_rel = current_loc.alt - ahrs.get_home().alt;
}
struct log_Trigger pkt = {
LOG_PACKET_HEADER_INIT(LOG_TRIGGER_MSG),
time_us : AP_HAL::micros64(),
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),
latitude : current_loc.lat,
longitude : current_loc.lng,
altitude : altitude,
altitude_rel: altitude_rel,
roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor,
yaw : (uint16_t)ahrs.yaw_sensor
};
WriteBlock(&pkt, sizeof(pkt));
Log_Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, gps, current_loc);
}
// Write an attitude packet
@ -1804,4 +1794,4 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
rpm2 : rpm_sensor.get_rpm(1)
};
WriteBlock(&pkt, sizeof(pkt));
}
}

23
libraries/DataFlash/LogStructure.h
View File

@ -385,20 +385,7 @@ struct PACKED log_Camera {
int32_t longitude;
int32_t altitude;
int32_t altitude_rel;
int16_t roll;
int16_t pitch;
uint16_t yaw;
};
struct PACKED log_Trigger {
LOG_PACKET_HEADER;
uint64_t time_us;
uint32_t gps_time;
uint16_t gps_week;
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t altitude_rel;
int32_t altitude_gps;
int16_t roll;
int16_t pitch;
uint16_t yaw;
@ -735,7 +722,9 @@ Format characters in the format string for binary log messages
{ LOG_RADIO_MSG, sizeof(log_Radio), \
"RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed" }, \
{ LOG_CAMERA_MSG, sizeof(log_Camera), \
"CAM", "QIHLLeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }, \
"CAM", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw" }, \
{ LOG_TRIGGER_MSG, sizeof(log_Camera), \
"TRIG", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw" }, \
{ LOG_ARSP_MSG, sizeof(log_AIRSPEED), \
"ARSP", "Qffcff", "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset" }, \
{ LOG_CURRENT_MSG, sizeof(log_Current), \
@ -749,9 +738,7 @@ Format characters in the format string for binary log messages
{ LOG_RFND_MSG, sizeof(log_RFND), \
"RFND", "QCC", "TimeUS,Dist1,Dist2" }, \
{ LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \
"DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }, \
{ LOG_TRIGGER_MSG, sizeof(log_Trigger), \
"TRIG", "QIHLLeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }
"DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \