mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_Terrain: use Rally singleton
This commit is contained in:
parent
6bb11b5cc8
commit
2207f52c8e
@ -56,9 +56,8 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
AP_Terrain::AP_Terrain(const AP_Mission &_mission, const AP_Rally &_rally) :
|
AP_Terrain::AP_Terrain(const AP_Mission &_mission) :
|
||||||
mission(_mission),
|
mission(_mission),
|
||||||
rally(_rally),
|
|
||||||
disk_io_state(DiskIoIdle),
|
disk_io_state(DiskIoIdle),
|
||||||
fd(-1)
|
fd(-1)
|
||||||
{
|
{
|
||||||
|
@ -29,7 +29,6 @@
|
|||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
#include <AP_AHRS/AP_AHRS.h>
|
#include <AP_AHRS/AP_AHRS.h>
|
||||||
#include <AP_Mission/AP_Mission.h>
|
#include <AP_Mission/AP_Mission.h>
|
||||||
#include <AP_Rally/AP_Rally.h>
|
|
||||||
|
|
||||||
#define TERRAIN_DEBUG 0
|
#define TERRAIN_DEBUG 0
|
||||||
|
|
||||||
@ -76,7 +75,7 @@
|
|||||||
|
|
||||||
class AP_Terrain {
|
class AP_Terrain {
|
||||||
public:
|
public:
|
||||||
AP_Terrain(const AP_Mission &_mission, const AP_Rally &_rally);
|
AP_Terrain(const AP_Mission &_mission);
|
||||||
|
|
||||||
/* Do not allow copies */
|
/* Do not allow copies */
|
||||||
AP_Terrain(const AP_Terrain &other) = delete;
|
AP_Terrain(const AP_Terrain &other) = delete;
|
||||||
@ -340,10 +339,6 @@ private:
|
|||||||
// all waypoints
|
// all waypoints
|
||||||
const AP_Mission &mission;
|
const AP_Mission &mission;
|
||||||
|
|
||||||
// reference to AP_Rally, so we can ask preload terrain data for
|
|
||||||
// all rally points
|
|
||||||
const AP_Rally &rally;
|
|
||||||
|
|
||||||
// cache of grids in memory, LRU
|
// cache of grids in memory, LRU
|
||||||
uint8_t cache_size = 0;
|
uint8_t cache_size = 0;
|
||||||
struct grid_cache *cache = nullptr;
|
struct grid_cache *cache = nullptr;
|
||||||
|
@ -19,6 +19,7 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_Rally/AP_Rally.h>
|
||||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
#include "AP_Terrain.h"
|
#include "AP_Terrain.h"
|
||||||
@ -110,11 +111,16 @@ void AP_Terrain::update_mission_data(void)
|
|||||||
*/
|
*/
|
||||||
void AP_Terrain::update_rally_data(void)
|
void AP_Terrain::update_rally_data(void)
|
||||||
{
|
{
|
||||||
if (last_rally_change_ms != rally.last_change_time_ms() ||
|
const AP_Rally *rally = AP::rally();
|
||||||
|
if (rally == nullptr) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (last_rally_change_ms != rally->last_change_time_ms() ||
|
||||||
last_rally_spacing != grid_spacing) {
|
last_rally_spacing != grid_spacing) {
|
||||||
// a rally point has changed - start again
|
// a rally point has changed - start again
|
||||||
next_rally_index = 1;
|
next_rally_index = 1;
|
||||||
last_rally_change_ms = rally.last_change_time_ms();
|
last_rally_change_ms = rally->last_change_time_ms();
|
||||||
last_rally_spacing = grid_spacing;
|
last_rally_spacing = grid_spacing;
|
||||||
}
|
}
|
||||||
if (next_rally_index == 0) {
|
if (next_rally_index == 0) {
|
||||||
@ -132,7 +138,7 @@ void AP_Terrain::update_rally_data(void)
|
|||||||
while (true) {
|
while (true) {
|
||||||
// get next rally point
|
// get next rally point
|
||||||
struct RallyLocation rp;
|
struct RallyLocation rp;
|
||||||
if (!rally.get_rally_point_with_index(next_rally_index, rp)) {
|
if (!rally->get_rally_point_with_index(next_rally_index, rp)) {
|
||||||
// nothing more to do
|
// nothing more to do
|
||||||
next_rally_index = 0;
|
next_rally_index = 0;
|
||||||
return;
|
return;
|
||||||
|
Loading…
Reference in New Issue
Block a user