From fb2362eceec278da15c0bffd2f66d5e93493f483 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:24 +1000 Subject: [PATCH 1/6] AP_Terrain: split sending terrain report from terrain request --- libraries/AP_Terrain/AP_Terrain.h | 3 +++ libraries/AP_Terrain/TerrainGCS.cpp | 19 +++++++++++++------ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 9d473a6b308c5..b0cbe826ab761 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -109,6 +109,9 @@ class AP_Terrain { void send_request(mavlink_channel_t chan); // 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); + // 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); diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index e65b87404130b..e5e9f2ec25a1e 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -115,17 +115,12 @@ void AP_Terrain::send_request(mavlink_channel_t chan) Location loc; if (!AP::ahrs().get_location(loc)) { - // we don't know where we are. Send a report and request any cached blocks. + // we don't know where we are. Request any cached blocks. // this allows for download of mission items when we have no GPS lock - loc = {}; - send_terrain_report(chan, loc, true); send_cache_request(chan); return; } - // always send a terrain report - send_terrain_report(chan, loc, true); - // did we request recently? if (AP_HAL::millis() - last_request_time_ms[chan] < 2000) { // too soon to request again @@ -207,6 +202,18 @@ void AP_Terrain::handle_data(mavlink_channel_t chan, const mavlink_message_t &ms } } +/* + send a TERRAIN_REPORT for the current location + */ +void AP_Terrain::send_report(mavlink_channel_t chan) +{ + Location loc; + if (!AP::ahrs().get_location(loc)) { + loc = {}; + } + + send_terrain_report(chan, loc, true); +} /* send a TERRAIN_REPORT for a location From 343e7fe95cf36a79a2f87ce106f37061725aed5f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:24 +1000 Subject: [PATCH 2/6] GCS_MAVLink: split sending terrain report from terrain request --- libraries/GCS_MAVLink/GCS_Common.cpp | 6 ++++-- libraries/GCS_MAVLink/ap_message.h | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 05c2d8f85c81e..586b50dde6c54 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1049,8 +1049,10 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER}, #endif { MAVLINK_MSG_ID_DISTANCE_SENSOR, MSG_DISTANCE_SENSOR}, - // request also does report: - { MAVLINK_MSG_ID_TERRAIN_REQUEST, MSG_TERRAIN}, +#if AP_TERRAIN_AVAILABLE + { MAVLINK_MSG_ID_TERRAIN_REQUEST, MSG_TERRAIN_REQUEST}, + { MAVLINK_MSG_ID_TERRAIN_REPORT, MSG_TERRAIN_REPORT}, +#endif #if AP_MAVLINK_BATTERY2_ENABLED { MAVLINK_MSG_ID_BATTERY2, MSG_BATTERY2}, #endif diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 347e85d797dc0..b7db82874aaf4 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -52,7 +52,8 @@ enum ap_message : uint8_t { MSG_WIND, MSG_RANGEFINDER, MSG_DISTANCE_SENSOR, - MSG_TERRAIN, + MSG_TERRAIN_REQUEST, + MSG_TERRAIN_REPORT, MSG_BATTERY2, MSG_CAMERA_FEEDBACK, MSG_CAMERA_INFORMATION, From 1fcb09d226c66dcee922f22c288621c7b44b706d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:25 +1000 Subject: [PATCH 3/6] ArduCopter: split sending terrain report from terrain request --- ArduCopter/GCS_Mavlink.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 314836448b10b..4a54063bd9a3a 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -336,12 +336,16 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) { switch(id) { - case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE + case MSG_TERRAIN_REQUEST: CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); copter.terrain.send_request(chan); -#endif break; + case MSG_TERRAIN_REPORT: + CHECK_PAYLOAD_SIZE(TERRAIN_REPORT); + copter.terrain.send_report(chan); + break; +#endif case MSG_WIND: CHECK_PAYLOAD_SIZE(WIND); @@ -451,7 +455,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate - // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY,GENERATOR_STATUS, and WINCH_STATUS + // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, TERRAIN_REPORT, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY,GENERATOR_STATUS, and WINCH_STATUS // @Units: Hz // @Range: 0 50 @@ -544,7 +548,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif MSG_DISTANCE_SENSOR, #if AP_TERRAIN_AVAILABLE - MSG_TERRAIN, + MSG_TERRAIN_REQUEST, + MSG_TERRAIN_REPORT, #endif #if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, From 56b5b4074ec89af164504712d771ec57e7ddc5be Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:25 +1000 Subject: [PATCH 4/6] ArduPlane: split sending terrain report from terrain request --- ArduPlane/GCS_Mavlink.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 527f8dd68c285..45a596bc6331a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -423,12 +423,16 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) // unused break; - case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE + case MSG_TERRAIN_REQUEST: CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); plane.terrain.send_request(chan); -#endif break; + case MSG_TERRAIN_REPORT: + CHECK_PAYLOAD_SIZE(TERRAIN_REPORT); + plane.terrain.send_report(chan); + break; +#endif case MSG_WIND: CHECK_PAYLOAD_SIZE(WIND); @@ -577,7 +581,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate - // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY2, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, and BATTERY_STATUS + // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, TERRAIN_REPORT, BATTERY2, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, and BATTERY_STATUS // @Units: Hz // @Range: 0 50 // @Increment: 1 @@ -686,7 +690,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, #if AP_TERRAIN_AVAILABLE - MSG_TERRAIN, + MSG_TERRAIN_REPORT, + MSG_TERRAIN_REQUEST, #endif #if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, From 2acb96d3788968c043b09e5ed57c337ec34d4bd3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:25 +1000 Subject: [PATCH 5/6] ArduSub: split sending terrain report from terrain request --- ArduSub/GCS_Mavlink.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index fc2b5c0227945..66411bb577976 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -248,12 +248,16 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) send_info(); break; - case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE + case MSG_TERRAIN_REQUEST: CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); sub.terrain.send_request(chan); -#endif break; + case MSG_TERRAIN_REPORT: + CHECK_PAYLOAD_SIZE(TERRAIN_REPORT); + sub.terrain.send_report(chan); + break; +#endif default: return GCS_MAVLINK::try_send_message(id); @@ -407,7 +411,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif MSG_DISTANCE_SENSOR, #if AP_TERRAIN_AVAILABLE - MSG_TERRAIN, + MSG_TERRAIN_REQUEST, + MSG_TERRAIN_REPORT, #endif #if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, From 3e5039d71b7ce892a834691243fa035a67a26ac6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 27 Sep 2024 11:01:25 +1000 Subject: [PATCH 6/6] Blimp: correct parameter documentation Blimp doesn't send this stuff --- Blimp/GCS_Mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 763133f76eef1..b132d80160a5c 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -288,7 +288,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station - // @Description: Stream rate of AHRS, SYSTEM_TIME, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION and RPM to ground station + // @Description: Stream rate of AHRS, SYSTEM_TIME, RANGEFINDER, DISTANCE_SENSOR, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION and RPM to ground station // @Units: Hz // @Range: 0 10 // @Increment: 1