From be87aa5191d45c3af0d8c3a441e2f5aa24db3923 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 7 Mar 2022 20:43:09 -0300 Subject: [PATCH 1/5] Sub: set default streamrates --- ArduSub/GCS_Mavlink.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index dbea46d35ce44..8d13f63153ceb 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -269,7 +269,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RAW_SENSORS], 0), + AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RAW_SENSORS], 2), // @Param: EXT_STAT // @DisplayName: Extended status stream rate to ground station @@ -279,7 +279,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTENDED_STATUS], 0), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTENDED_STATUS], 2), // @Param: RC_CHAN // @DisplayName: RC Channel stream rate to ground station @@ -289,7 +289,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RC_CHANNELS], 0), + AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RC_CHANNELS], 2), // @Param: POSITION // @DisplayName: Position stream rate to ground station @@ -299,7 +299,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_POSITION], 0), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_POSITION], 3), // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station @@ -309,7 +309,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA1], 0), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA1], 10), // @Param: EXTRA2 // @DisplayName: Extra data type 2 stream rate to ground station @@ -319,7 +319,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA2], 0), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA2], 10), // @Param: EXTRA3 // @DisplayName: Extra data type 3 stream rate to ground station @@ -329,7 +329,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA3], 0), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA3], 3), // @Param: PARAMS // @DisplayName: Parameter stream rate to ground station From 5538f6735f4fdae6ac4e1660dd2ff5819fc8297b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 May 2024 17:32:06 +1000 Subject: [PATCH 2/5] AP_Camera: move switch for sending camera messages into AP_Camera neatens GCS_Common a bit, reduces repetitive code --- libraries/AP_Camera/AP_Camera.cpp | 39 +++++++++++++++++++++++++++++++ libraries/AP_Camera/AP_Camera.h | 37 ++++++++++++++++------------- 2 files changed, 60 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 9a2deb8a18606..57302c8bde6ca 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -2,6 +2,7 @@ #if AP_CAMERA_ENABLED +#include #include #include #include @@ -432,6 +433,44 @@ MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet) } } +// send a mavlink message; returns false if there was not space to +// send the message, true otherwise +bool AP_Camera::send_mavlink_message(GCS_MAVLINK &link, const enum ap_message msg_id) +{ + const auto chan = link.get_chan(); + + switch (msg_id) { + case MSG_CAMERA_FEEDBACK: + CHECK_PAYLOAD_SIZE2(CAMERA_FEEDBACK); + send_feedback(chan); + break; + case MSG_CAMERA_INFORMATION: + CHECK_PAYLOAD_SIZE2(CAMERA_INFORMATION); + send_camera_information(chan); + break; + case MSG_CAMERA_SETTINGS: + CHECK_PAYLOAD_SIZE2(CAMERA_SETTINGS); + send_camera_settings(chan); + break; +#if AP_CAMERA_SEND_FOV_STATUS_ENABLED + case MSG_CAMERA_FOV_STATUS: + CHECK_PAYLOAD_SIZE2(CAMERA_FOV_STATUS); + send_camera_fov_status(chan); + break; +#endif + case MSG_CAMERA_CAPTURE_STATUS: + CHECK_PAYLOAD_SIZE2(CAMERA_CAPTURE_STATUS); + send_camera_capture_status(chan); + break; + + default: + // should not reach this; should only be called for specific IDs + break; + } + + return true; +} + // set camera trigger distance in a mission void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index ee9f142cecbb9..dd9f4ddfde565 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -8,6 +8,7 @@ #include #include +#include #include "AP_Camera_Params.h" #include "AP_Camera_shareddefs.h" @@ -86,22 +87,9 @@ class AP_Camera { // handle MAVLink command from GCS to control the camera MAV_RESULT handle_command(const mavlink_command_int_t &packet); - // send camera feedback message to GCS - void send_feedback(mavlink_channel_t chan); - - // send camera information message to GCS - void send_camera_information(mavlink_channel_t chan); - - // send camera settings message to GCS - void send_camera_settings(mavlink_channel_t chan); - -#if AP_CAMERA_SEND_FOV_STATUS_ENABLED - // send camera field of view status - void send_camera_fov_status(mavlink_channel_t chan); -#endif - - // send camera capture status message to GCS - void send_camera_capture_status(mavlink_channel_t chan); + // send a mavlink message; returns false if there was not space to + // send the message, true otherwise + bool send_mavlink_message(class GCS_MAVLINK &link, const enum ap_message id); // configure camera void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time); @@ -232,6 +220,23 @@ class AP_Camera { // perform any required parameter conversion void convert_params(); + // send camera feedback message to GCS + void send_feedback(mavlink_channel_t chan); + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan); + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan); + +#if AP_CAMERA_SEND_FOV_STATUS_ENABLED + // send camera field of view status + void send_camera_fov_status(mavlink_channel_t chan); +#endif + + // send camera capture status message to GCS + void send_camera_capture_status(mavlink_channel_t chan); + HAL_Semaphore _rsem; // semaphore for multi-thread access AP_Camera_Backend *primary; // primary camera backed bool _is_in_auto_mode; // true if in AUTO mode From 78fcf7057c6c23eb312a14b5d6f6599f87208827 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 May 2024 17:32:06 +1000 Subject: [PATCH 3/5] GCS_MAVLink: move switch for sending camera messages into AP_Camera neatens GCS_Common a bit, reduces repetitive code --- libraries/GCS_MAVLink/GCS_Common.cpp | 42 ++-------------------------- 1 file changed, 2 insertions(+), 40 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ad782c46cad96..2c9dc7cc306a5 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6014,46 +6014,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if AP_CAMERA_ENABLED case MSG_CAMERA_FEEDBACK: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); - camera->send_feedback(chan); - } - break; case MSG_CAMERA_INFORMATION: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_INFORMATION); - camera->send_camera_information(chan); - } - break; case MSG_CAMERA_SETTINGS: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_SETTINGS); - camera->send_camera_settings(chan); - } - break; #if AP_CAMERA_SEND_FOV_STATUS_ENABLED case MSG_CAMERA_FOV_STATUS: - { - AP_Camera *camera = AP::camera(); - if (camera == nullptr) { - break; - } - CHECK_PAYLOAD_SIZE(CAMERA_FOV_STATUS); - camera->send_camera_fov_status(chan); - } - break; #endif case MSG_CAMERA_CAPTURE_STATUS: { @@ -6061,11 +6025,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) if (camera == nullptr) { break; } - CHECK_PAYLOAD_SIZE(CAMERA_CAPTURE_STATUS); - camera->send_camera_capture_status(chan); + return camera->send_mavlink_message(*this, id); } - break; -#endif +#endif // AP_CAMERA_ENABLED case MSG_SYSTEM_TIME: CHECK_PAYLOAD_SIZE(SYSTEM_TIME); From d2901dc9c9b433b27277fd822bcd3da8e6a2ab8d Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 21 May 2024 19:22:45 +0200 Subject: [PATCH 4/5] SITL: Removed trailing underscore --- libraries/SITL/SITL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 98a5afa034c0b..c9633291ce300 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -208,7 +208,7 @@ const AP_Param::GroupInfo SIM::var_info2[] = { AP_GROUPINFO("TEMP_BFACTOR", 4, SIM, temp_baro_factor, 0), AP_GROUPINFO("WIND_DIR_Z", 10, SIM, wind_dir_z, 0), - // @Param: WIND_T_ + // @Param: WIND_T // @DisplayName: Wind Profile Type // @Description: Selects how wind varies from surface to WIND_T_ALT // @Values: 0:square law,1: none, 2:linear-see WIND_T_COEF From 396865223f78b57708635cdee6d6d9104dd12dbd Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Wed, 29 May 2024 13:29:37 +0200 Subject: [PATCH 5/5] autotest: Removed unnecessary parameter whitelist item --- Tools/autotest/vehicle_test_suite.py | 1 - 1 file changed, 1 deletion(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 8a65a60e9254e..d057def1a881d 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2627,7 +2627,6 @@ def get_sim_parameter_documentation_get_whitelist(self): "SIM_WAVE_LENGTH", "SIM_WAVE_SPEED", "SIM_WIND_DIR_Z", - "SIM_WIND_T", ]) vinfo_key = self.vehicleinfo_key()