From a9ce08f6dadf129484125dc4a0225391aa90e7fa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Jul 2022 18:45:02 +1000 Subject: [PATCH] AP_Terrain: move terrain handling up into GCS_MAVLink common --- libraries/AP_Terrain/AP_Terrain.h | 21 ++++---- libraries/AP_Terrain/TerrainGCS.cpp | 84 +++++++++++++++++------------ 2 files changed, 63 insertions(+), 42 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index a0dda4d772..ecb372f20e 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -105,16 +105,15 @@ public: #if HAL_GCS_ENABLED // send any pending terrain request message - bool send_cache_request(mavlink_channel_t chan); - void send_request(mavlink_channel_t chan); + bool send_cache_request(class GCS_MAVLINK &link); + void send_request(GCS_MAVLINK &link); // handle terrain data and reports from GCS // send a terrain report for the current location, extrapolating height as we do for navigation: - void send_report(mavlink_channel_t chan); + void send_report(GCS_MAVLINK &link); // send a terrain report or Location loc - void send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate); - void handle_data(mavlink_channel_t chan, const mavlink_message_t &msg); - void handle_terrain_check(mavlink_channel_t chan, const mavlink_message_t &msg); + void send_terrain_report(GCS_MAVLINK &link, const Location &loc, bool extrapolate); + void handle_terrain_check(GCS_MAVLINK &link, const mavlink_message_t &msg); void handle_terrain_data(const mavlink_message_t &msg); #endif @@ -203,6 +202,10 @@ public: */ void set_reference_location(void); +#if HAL_GCS_ENABLED + void handle_message(GCS_MAVLINK &link, const mavlink_message_t &msg); +#endif // HAL_GCS_ENABLED + private: // allocate the terrain subsystem data bool allocate(void); @@ -322,9 +325,9 @@ private: /* request any missing 4x4 grids from a block */ - bool request_missing(mavlink_channel_t chan, struct grid_cache &gcache); - bool request_missing(mavlink_channel_t chan, const struct grid_info &info); -#endif + bool request_missing(class GCS_MAVLINK &link, struct grid_cache &gcache); + bool request_missing(GCS_MAVLINK &link, const struct grid_info &info); +#endif // HAL_GCS_ENABLED /* look for blocks that need to be read/written to disk diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index 22f1c791ea..ad638053c0 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -36,8 +36,12 @@ extern const AP_HAL::HAL& hal; /* request any missing 4x4 grids from a block, given a grid_cache */ -bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcache) +bool AP_Terrain::request_missing(GCS_MAVLINK &link, struct grid_cache &gcache) { + if (!HAVE_PAYLOAD_SPACE(link.get_chan(), TERRAIN_REQUEST)) { + return false; + } + struct grid_block &grid = gcache.grid; if (options.get() & uint16_t(Options::DisableDownload)) { @@ -61,16 +65,17 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac return false; } - if (!HAVE_PAYLOAD_SPACE(chan, TERRAIN_REQUEST)) { - // not enough buffer space - return false; - } - /* ask the GCS to send a set of 4x4 grids */ - mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap); - last_request_time_ms[chan] = AP_HAL::millis(); + const mavlink_terrain_request_t packet { + bitmap_mask & ~grid.bitmap, + grid.lat, + grid.lon, + (uint16_t)grid_spacing + }; + mavlink_msg_terrain_request_send_struct(link.get_chan(), &packet); + last_request_time_ms[link.get_chan()] = AP_HAL::millis(); return true; } @@ -78,21 +83,21 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac /* request any missing 4x4 grids from a block */ -bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info) +bool AP_Terrain::request_missing(GCS_MAVLINK &link, const struct grid_info &info) { // find the grid struct grid_cache &gcache = find_grid_cache(info); - return request_missing(chan, gcache); + return request_missing(link, gcache); } /* send any pending cache requests */ -bool AP_Terrain::send_cache_request(mavlink_channel_t chan) +bool AP_Terrain::send_cache_request(GCS_MAVLINK &link) { for (uint16_t i=0; i= GRID_CACHE_VALID) { - if (request_missing(chan, cache[i])) { + if (request_missing(link, cache[i])) { return true; } } @@ -103,7 +108,7 @@ bool AP_Terrain::send_cache_request(mavlink_channel_t chan) /* send any pending terrain request to the GCS */ -void AP_Terrain::send_request(mavlink_channel_t chan) +void AP_Terrain::send_request(GCS_MAVLINK &link) { if (!allocate()) { // not enabled @@ -117,12 +122,12 @@ void AP_Terrain::send_request(mavlink_channel_t chan) if (!AP::ahrs().get_location(loc)) { // we don't know where we are. Request any cached blocks. // this allows for download of mission items when we have no GPS lock - send_cache_request(chan); + send_cache_request(link); return; } // did we request recently? - if (AP_HAL::millis() - last_request_time_ms[chan] < 2000) { + if (AP_HAL::millis() - last_request_time_ms[link.get_chan()] < 2000) { // too soon to request again return; } @@ -131,7 +136,7 @@ void AP_Terrain::send_request(mavlink_channel_t chan) struct grid_info info; calculate_grid_info(loc, info); - if (request_missing(chan, info)) { + if (request_missing(link, info)) { return; } @@ -139,13 +144,13 @@ void AP_Terrain::send_request(mavlink_channel_t chan) // mission items, rally items, squares surrounding our current // location, favourite holiday destination, scripting, height // reference location, .... - if (send_cache_request(chan)) { + if (send_cache_request(link)) { return; } // request the current loc last to ensure it has highest last // access time - if (request_missing(chan, info)) { + if (request_missing(link, info)) { return; } } @@ -193,34 +198,41 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded) const /* handle terrain messages from GCS */ -void AP_Terrain::handle_data(mavlink_channel_t chan, const mavlink_message_t &msg) +void AP_Terrain::handle_message(GCS_MAVLINK &link, const mavlink_message_t &msg) { - if (msg.msgid == MAVLINK_MSG_ID_TERRAIN_DATA) { - handle_terrain_data(msg); - } else if (msg.msgid == MAVLINK_MSG_ID_TERRAIN_CHECK) { - handle_terrain_check(chan, msg); + switch (msg.msgid) { + case MAVLINK_MSG_ID_TERRAIN_DATA: + return handle_terrain_data(msg); + case MAVLINK_MSG_ID_TERRAIN_CHECK: + return handle_terrain_check(link, msg); + // default: + // shouldn't have been called } } /* send a TERRAIN_REPORT for the current location */ -void AP_Terrain::send_report(mavlink_channel_t chan) +void AP_Terrain::send_report(GCS_MAVLINK &link) { Location loc; if (!AP::ahrs().get_location(loc)) { loc = {}; } - send_terrain_report(chan, loc, true); + send_terrain_report(link, loc, true); } /* send a TERRAIN_REPORT for a location */ -void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate) +void AP_Terrain::send_terrain_report(GCS_MAVLINK &link, const Location &loc, bool extrapolate) { #if HAL_GCS_ENABLED + if (!HAVE_PAYLOAD_SPACE(link.get_chan(), TERRAIN_REPORT)) { + return; + } + float terrain_height = 0; uint16_t spacing = 0; if (height_amsl(loc, terrain_height)) { @@ -237,25 +249,31 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc float current_height = 0.0f; height_above_terrain(current_height, extrapolate); - if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) { - mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing, - terrain_height, current_height, - pending, loaded); - } + const mavlink_terrain_report_t packet { + loc.lat, + loc.lng, + terrain_height, + current_height, + spacing, + pending, + loaded + }; + + mavlink_msg_terrain_report_send_struct(link.get_chan(), &packet); #endif } /* handle TERRAIN_CHECK messages from GCS */ -void AP_Terrain::handle_terrain_check(mavlink_channel_t chan, const mavlink_message_t &msg) +void AP_Terrain::handle_terrain_check(GCS_MAVLINK &link, const mavlink_message_t &msg) { mavlink_terrain_check_t packet; mavlink_msg_terrain_check_decode(&msg, &packet); Location loc; loc.lat = packet.lat; loc.lng = packet.lon; - send_terrain_report(chan, loc, false); + send_terrain_report(link, loc, false); } /*