Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: log rally points into dataflash on upload

Browse Source
This commit is contained in:
Peter Barker 2018-12-29 15:02:29 +11:00 committed by Andrew Tridgell
parent 3bba981a00
commit 9108eeb8f5
6 changed files with 43 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Logger/AP_Logger.cpp
View File

@ -667,6 +667,13 @@ void AP_Logger::Write_Mission_Cmd(const AP_Mission &mission,
FOR_EACH_BACKEND(Write_Mission_Cmd(mission, cmd));
}
void AP_Logger::Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point)
{
FOR_EACH_BACKEND(Write_RallyPoint(total, sequence, rally_point));
}
uint32_t AP_Logger::num_dropped() const
{
if (_next_backend == 0) {

4
libraries/AP_Logger/AP_Logger.h
View File

@ -207,7 +207,9 @@ public:
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
void Write_Rally();
void Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point);
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_AOA_SSA(AP_AHRS &ahrs);
void Write_Beacon(AP_Beacon &beacon);

25
libraries/AP_Logger/AP_Logger_Backend.cpp
View File

@ -416,3 +416,28 @@ bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
return Write_Message(msg);
}
// Write rally points
bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point)
{
struct log_Rally pkt_rally = {
LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
time_us : AP_HAL::micros64(),
total : total,
sequence : sequence,
latitude : rally_point.lat,
longitude : rally_point.lng,
altitude : rally_point.alt
};
return WriteBlock(&pkt_rally, sizeof(pkt_rally));
}
// Write rally points
void AP_Logger_Backend::Write_Rally()
{
LoggerMessageWriter_WriteAllRallyPoints writer;
writer.set_dataflash_backend(this);
writer.process();
}

25
libraries/AP_Logger/LogFile.cpp
View File

@ -1608,31 +1608,6 @@ void AP_Logger::Write_Rate(const AP_AHRS_View *ahrs,
WriteBlock(&pkt_rate, sizeof(pkt_rate));
}
// Write rally points
bool AP_Logger_Backend::Log_Write_RallyPoint(uint8_t total,
uint8_t sequence,
const RallyLocation &rally_point)
{
struct log_Rally pkt_rally = {
LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
time_us : AP_HAL::micros64(),
total : total,
sequence : sequence,
latitude : rally_point.lat,
longitude : rally_point.lng,
altitude : rally_point.alt
};
return WriteBlock(&pkt_rally, sizeof(pkt_rally));
}
// Write rally points
void AP_Logger::Log_Write_Rally()
{
AP_Logger_WriteAllRallyPoints writer;
writer.set_dataflash_backend(this);
writer.process();
}
// Write visual odometry sensor data
void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
{

11
libraries/AP_Logger/LoggerMessageWriter.cpp
View File

@ -190,7 +190,8 @@ void LoggerMessageWriter_WriteSysInfo::process() {
_finished = true; // all done!
}
void LoggerMessageWriter_WriteAllRallyPoints::process() {
void LoggerMessageWriter_WriteAllRallyPoints::process()
{
const AP_Rally *_rally = AP::rally();
if (_rally == nullptr) {
_finished = true;
@ -208,7 +209,7 @@ void LoggerMessageWriter_WriteAllRallyPoints::process() {
FALLTHROUGH;
case ar_blockwriter_stage_write_new_rally_message:
if (! _dataflash_backend->Log_Write_Message("New rally")) {
if (! _dataflash_backend->Write_Message("New rally")) {
return; // call me again
}
stage = ar_blockwriter_stage_write_all_rally_points;
@ -218,7 +219,7 @@ void LoggerMessageWriter_WriteAllRallyPoints::process() {
while (_rally_number_to_send < _rally->get_rally_total()) {
RallyLocation rallypoint;
if (_rally->get_rally_point_with_index(_rally_number_to_send, rallypoint)) {
if (!_dataflash_backend->Log_Write_RallyPoint(
if (!_dataflash_backend->Write_RallyPoint(
_rally->get_rally_total(),
_rally_number_to_send,
rallypoint)) {
@ -237,9 +238,9 @@ void LoggerMessageWriter_WriteAllRallyPoints::process() {
_finished = true;
}
void DFMessageWriter_WriteAllRallyPoints::reset()
void LoggerMessageWriter_WriteAllRallyPoints::reset()
{
DFMessageWriter::reset();
LoggerMessageWriter::reset();
stage = ar_blockwriter_stage_init;
_rally_number_to_send = 0;
}

3
libraries/AP_Logger/LoggerMessageWriter.h
View File

@ -78,7 +78,8 @@ class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter {
public:
LoggerMessageWriter_DFLogStart() :
_writesysinfo(),
_writeentiremission()
_writeentiremission(),
_writeallrallypoints()
{
}