From edc0e466143f5cb2817c96ad8dcd7ac3463124eb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 23 Sep 2024 19:43:12 +1000 Subject: [PATCH] AP_Mount: add and use global NaNf float value --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 16 +++++++--------- libraries/AP_Mount/AP_Mount_Topotek.cpp | 6 ++---- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 4 +--- libraries/AP_Mount/AP_Mount_Xacti.cpp | 11 ++++------- 4 files changed, 14 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 1c3c8cf831..33e3534d59 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -1107,7 +1107,6 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const { const uint8_t mode_id = (_config_info.record_status == RecordingStatus::ON) ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE; - const float NaN = nanf("0x4152"); const float zoom_mult_max = get_zoom_mult_max(); float zoom_pct = 0.0; if (is_positive(zoom_mult_max)) { @@ -1120,14 +1119,13 @@ void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const AP_HAL::millis(), // time_boot_ms mode_id, // camera mode (0:image, 1:video, 2:image survey) zoom_pct, // zoomLevel float, percentage from 0 to 100, NaN if unknown - NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown } #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED // send camera thermal range message to GCS void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const { - const float NaN = nanf("0x4152"); const uint32_t now_ms = AP_HAL::millis(); bool timeout = now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS; @@ -1137,12 +1135,12 @@ void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const now_ms, // time_boot_ms _instance + 1, // video stream id (assume one-to-one mapping with camera id) _instance + 1, // camera device id - timeout ? NaN : _thermal.max_C, // max in degC - timeout ? NaN : _thermal.max_pos.x, // max x position - timeout ? NaN : _thermal.max_pos.y, // max y position - timeout ? NaN : _thermal.min_C, // min in degC - timeout ? NaN : _thermal.min_pos.x, // min x position - timeout ? NaN : _thermal.min_pos.y);// min y position + timeout ? NaNf : _thermal.max_C, // max in degC + timeout ? NaNf : _thermal.max_pos.x, // max x position + timeout ? NaNf : _thermal.max_pos.y, // max y position + timeout ? NaNf : _thermal.min_C, // min in degC + timeout ? NaNf : _thermal.min_pos.x, // min x position + timeout ? NaNf : _thermal.min_pos.y);// min y position } #endif diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 56a20297c4..d8992b4876 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -566,15 +566,13 @@ void AP_Mount_Topotek::send_camera_settings(mavlink_channel_t chan) const return; } - const float NaN = nanf("0x4152"); - // send CAMERA_SETTINGS message mavlink_msg_camera_settings_send( chan, AP_HAL::millis(), // time_boot_ms _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) - NaN, // zoomLevel float, percentage from 0 to 100, NaN if unknown - NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown } // get rangefinder distance. Returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index e04510962e..c95abd03b9 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -940,8 +940,6 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const return; } - const float NaN = nanf("0x4152"); - // convert zoom times (e.g. 1x ~ 20x) to target zoom level (e.g. 0 to 100) const float zoom_level = linear_interpolate(0, 100, _zoom_times, 1, AP_MOUNT_VIEWPRO_ZOOM_MAX); @@ -951,7 +949,7 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const AP_HAL::millis(), // time_boot_ms _recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) zoom_level, // zoomLevel float, percentage from 0 to 100, NaN if unknown - NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown } // get rangefinder distance. Returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index a4c21bfea7..6353ce41d0 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -361,7 +361,6 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const static const uint8_t vendor_name[32] = "Xacti"; static uint8_t model_name[32] = "CX-GB100"; const char cam_definition_uri[140] {}; - const float NaN = nanf("0x4152"); // capability flags const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | @@ -375,9 +374,9 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const vendor_name, // vendor_name uint8_t[32] model_name, // model_name uint8_t[32] _firmware_version.received ? _firmware_version.mav_ver : 0, // firmware version uint32_t - NaN, // focal_length float (mm) - NaN, // sensor_size_h float (mm) - NaN, // sensor_size_v float (mm) + NaNf, // focal_length float (mm) + NaNf, // sensor_size_h float (mm) + NaNf, // sensor_size_v float (mm) 0, // resolution_h uint16_t (pix) 0, // resolution_v uint16_t (pix) 0, // lens_id uint8_t @@ -390,15 +389,13 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const // send camera settings message to GCS void AP_Mount_Xacti::send_camera_settings(mavlink_channel_t chan) const { - const float NaN = nanf("0x4152"); - // send CAMERA_SETTINGS message mavlink_msg_camera_settings_send( chan, AP_HAL::millis(), // time_boot_ms _recording_video ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) 0, // zoomLevel float, percentage from 0 to 100, NaN if unknown - NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown } // get attitude as a quaternion. returns true on success