From d5cc3f34055287e356cde3b6815a79ee1b9fb126 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Wed, 28 Oct 2020 19:15:38 +0100 Subject: [PATCH 1/4] Save CAM_MODE with type defined by camera definition (if existing) --- src/core/mavlink_parameters.cpp | 20 +++------------- src/plugins/camera/camera_impl.cpp | 37 +++++++++++++++++++++++++++++- 2 files changed, 39 insertions(+), 18 deletions(-) diff --git a/src/core/mavlink_parameters.cpp b/src/core/mavlink_parameters.cpp index d93282a693..da26649da6 100644 --- a/src/core/mavlink_parameters.cpp +++ b/src/core/mavlink_parameters.cpp @@ -451,33 +451,19 @@ void MAVLinkParameters::process_param_ext_value(const mavlink_message_t& message case WorkItem::Type::Get: { ParamValue value; value.set_from_mavlink_param_ext_value(param_ext_value); + if (value.is_same_type(work->param_value)) { if (work->get_param_callback) { work->get_param_callback(MAVLinkParameters::Result::Success, value); } - } else if (value.is_uint8() && work->param_value.is_uint16()) { - // FIXME: workaround for mismatching type uint8_t which should be uint16_t. - ParamValue correct_type_value; - correct_type_value.set_uint16(static_cast(value.get_uint8())); - if (work->get_param_callback) { - work->get_param_callback( - MAVLinkParameters::Result::Success, correct_type_value); - } - } else if (value.is_uint8() && work->param_value.is_uint32()) { - // FIXME: workaround for mismatching type uint8_t which should be uint32_t. - ParamValue correct_type_value; - correct_type_value.set_uint32(static_cast(value.get_uint8())); - if (work->get_param_callback) { - work->get_param_callback( - MAVLinkParameters::Result::Success, correct_type_value); - } } else { - LogErr() << "Param types don't match"; + LogErr() << "Param types don't match for " << work->param_name; ParamValue no_value; if (work->get_param_callback) { work->get_param_callback(MAVLinkParameters::Result::WrongType, no_value); } } + _parent.unregister_timeout_handler(_timeout_cookie); // LogDebug() << "time taken: " << // _parent.get_time().elapsed_since_s(_last_request_time); diff --git a/src/plugins/camera/camera_impl.cpp b/src/plugins/camera/camera_impl.cpp index ef54742532..1bb4c34a6f 100644 --- a/src/plugins/camera/camera_impl.cpp +++ b/src/plugins/camera/camera_impl.cpp @@ -581,8 +581,43 @@ void CameraImpl::save_camera_mode(const float mavlink_camera_mode) return; } + // If there is a camera definition (which is the case if we are + // in this function, and if CAM_MODE is defined there, then + // we reuse that type. Otherwise, we hardcode it to `uint32_t`. + // Note that it could be that the camera definition defines options + // different than {PHOTO, VIDEO}, in which case the mode received + // from CAMERA_SETTINGS will be wrong. Not sure if it means that + // it should be ignored in that case, but that may be tricky to + // maintain (what if the MAVLink CAMERA_MODE enum evolves?), so + // I am assuming here that in such a case, CAMERA_SETTINGS is + // never sent by the camera. MAVLinkParameters::ParamValue value; - value.set_uint32(uint32_t(mavlink_camera_mode)); + if (_camera_definition->get_setting("CAM_MODE", value)) { + if (value.is_uint8()) { + value.set_uint8(uint8_t(mavlink_camera_mode)); + } else if (value.is_int8()) { + value.set_int8(int8_t(mavlink_camera_mode)); + } else if (value.is_uint16()) { + value.set_uint16(uint16_t(mavlink_camera_mode)); + } else if (value.is_int16()) { + value.set_int16(int16_t(mavlink_camera_mode)); + } else if (value.is_uint32()) { + value.set_uint32(uint32_t(mavlink_camera_mode)); + } else if (value.is_int32()) { + value.set_int32(int32_t(mavlink_camera_mode)); + } else if (value.is_uint64()) { + value.set_uint64(uint64_t(mavlink_camera_mode)); + } else if (value.is_int64()) { + value.set_int64(int64_t(mavlink_camera_mode)); + } else if (value.is_float()) { + value.set_float(float(mavlink_camera_mode)); + } else if (value.is_double()) { + value.set_double(double(mavlink_camera_mode)); + } + } else { + value.set_uint32(uint32_t(mavlink_camera_mode)); + } + _camera_definition->set_setting("CAM_MODE", value); refresh_params(); } From d4498b7814087794b441a76bf27226ef416aebed Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Thu, 29 Oct 2020 11:41:15 +0100 Subject: [PATCH 2/4] The shared_ptr should not be passed as a reference, otherwise it does not behave like a shared_ptr anymore --- src/core/mavlink_commands.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 66923af0c4..d604177899 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -34,7 +34,7 @@ MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& comma auto prom = std::make_shared>(); auto res = prom->get_future(); - queue_command_async(command, [&prom](Result result, float progress) { + queue_command_async(command, [prom](Result result, float progress) { UNUSED(progress); // We can only fulfill the promise once in C++11. // Therefore we have to ignore the IN_PROGRESS state and wait @@ -55,7 +55,7 @@ MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& comm auto prom = std::make_shared>(); auto res = prom->get_future(); - queue_command_async(command, [&prom](Result result, float progress) { + queue_command_async(command, [prom](Result result, float progress) { UNUSED(progress); // We can only fulfill the promise once in C++11. // Therefore we have to ignore the IN_PROGRESS state and wait From 36cabaa7d386495cc6301e90b75f5e5f1d25b3c0 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Thu, 29 Oct 2020 11:42:11 +0100 Subject: [PATCH 3/4] Remove confusing comment and set the result for InProgress commands, otherwise promise will be resolved twice --- src/core/mavlink_commands.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index d604177899..a1c7a74403 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -205,10 +205,8 @@ void MavlinkCommandSender::receive_command_ack(mavlink_message_t message) std::bind(&MavlinkCommandSender::receive_timeout, this), work->retries_to_do * work->timeout_s, &_timeout_cookie); - // FIXME: We can only call callbacks with promises once, so let's not do it - // on IN_PROGRESS. - // call_callback(work->callback, Result::IN_PROGRESS, command_ack.progress / - // 100.0f); + + temp_result = {Result::InProgress, command_ack.progress / 100.0f}; break; default: From 405c819c2dde01090d62bb1079e19434301a3d11 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Thu, 29 Oct 2020 11:42:42 +0100 Subject: [PATCH 4/4] Fix comment such that it compiles when commented out --- src/core/mavlink_commands.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index a1c7a74403..cfe96606e5 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -151,9 +151,8 @@ void MavlinkCommandSender::receive_command_ack(mavlink_message_t message) return; } - // LogDebug() << "We got an ack: " << command_ack.command - // << " after: " << _parent.get_time().elapsed_since_s(work->time_started) << " - // s"; + // LogDebug() << "We got an ack: " << command_ack.command << " after: " + // << _parent.get_time().elapsed_since_s(work->time_started) << " s"; temp_callback = work->callback; switch (command_ack.result) {