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

AP_Terrain: added dataflash logging

Browse Source
This commit is contained in:
Andrew Tridgell 2014-08-06 12:16:29 +10:00
parent cd8fca40fb
commit 9ca65602c6
2 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -19,6 +19,7 @@
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#include <GCS.h>
#include <DataFlash.h>
#include "AP_Terrain.h"
#if AP_TERRAIN_AVAILABLE
@ -281,4 +282,40 @@ enum AP_Terrain::TerrainStatus AP_Terrain::status(void)
return TerrainStatusOK;
}
/*
log terrain data to dataflash log
*/
void AP_Terrain::log_terrain_data(DataFlash_Class &dataflash)
{
if (!enable) {
return;
}
Location loc;
if (!ahrs.get_position(loc)) {
// we don't know where we are
return;
}
float terrain_height = 0;
float current_height = 0;
uint16_t pending, loaded;
height_amsl(loc, terrain_height);
height_above_terrain(current_height, true);
get_statistics(pending, loaded);
struct log_TERRAIN pkt = {
LOG_PACKET_HEADER_INIT(LOG_TERRAIN_MSG),
time_ms : hal.scheduler->millis(),
status : (uint8_t)status(),
lat : loc.lat,
lng : loc.lng,
spacing : grid_spacing,
terrain_height : terrain_height,
current_height : current_height,
pending : pending,
loaded : loaded
};
dataflash.WriteBlock(&pkt, sizeof(pkt));
}
#endif // AP_TERRAIN_AVAILABLE

6
libraries/AP_Terrain/AP_Terrain.h
View File

@ -19,6 +19,7 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#include <DataFlash.h>
#if HAL_OS_POSIX_IO && defined(HAL_BOARD_TERRAIN_DIRECTORY)
#define AP_TERRAIN_AVAILABLE 1
@ -151,6 +152,11 @@ public:
*/
bool height_above_terrain(float &terrain_altitude, bool extrapolate = false);
/*
log terrain status to DataFlash
*/
void log_terrain_data(DataFlash_Class &dataflash);
private:
// allocate the terrain subsystem data
void allocate(void);