From 399f9f6f9805582dfb7208d8f9317b7b30c0ab5c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Apr 2024 17:58:56 +0100 Subject: [PATCH] AP_Camera: RunCam camera backend RunCam parameter conversion add RunCam camera settings and control --- libraries/AP_Camera/AP_Camera.cpp | 69 +++++++++++- libraries/AP_Camera/AP_Camera.h | 10 ++ libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- libraries/AP_Camera/AP_Camera_config.h | 4 + libraries/AP_Camera/AP_RunCam.cpp | 127 +++++++++++++++++++---- libraries/AP_Camera/AP_RunCam.h | 49 +++++++-- 6 files changed, 232 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 288b0506bb..aee97439dc 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "AP_Camera_Backend.h" #include "AP_Camera_Servo.h" #include "AP_Camera_Relay.h" @@ -14,6 +15,7 @@ #include "AP_Camera_MAVLink.h" #include "AP_Camera_MAVLinkCamV2.h" #include "AP_Camera_Scripting.h" +#include "AP_RunCam.h" const AP_Param::GroupInfo AP_Camera::var_info[] = { @@ -41,10 +43,24 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Path: AP_Camera_Params.cpp AP_SUBGROUPINFO(_params[1], "2", 13, AP_Camera, AP_Camera_Params), #endif +#if AP_CAMERA_RUNCAM_ENABLED + // @Group: 1_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[0], "1_RC_", 14, AP_Camera, _backend_var_info[0]), +#if AP_CAMERA_MAX_INSTANCES > 1 + // @Group: 2_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[1], "2_RC_", 15, AP_Camera, _backend_var_info[1]), +#endif +#endif AP_GROUPEND }; +#if AP_CAMERA_RUNCAM_ENABLED +const AP_Param::GroupInfo *AP_Camera::_backend_var_info[AP_CAMERA_MAX_INSTANCES]; +#endif + extern const AP_HAL::HAL& hal; AP_Camera::AP_Camera(uint32_t _log_camera_bit) : @@ -238,6 +254,17 @@ void AP_Camera::init() case CameraType::SCRIPTING: _backends[instance] = NEW_NOTHROW AP_Camera_Scripting(*this, _params[instance], instance); break; +#endif +#if AP_CAMERA_RUNCAM_ENABLED + // check for RunCam driver + case CameraType::RUNCAM: + if (_backends[instance] == nullptr) { // may have already been created by the conversion code + _backends[instance] = NEW_NOTHROW AP_RunCam(*this, _params[instance], instance, _runcam_instances); + _backend_var_info[instance] = AP_RunCam::var_info; + AP_Param::load_object_from_eeprom(_backends[instance], _backend_var_info[instance]); + _runcam_instances++; + } + break; #endif case CameraType::NONE: break; @@ -899,7 +926,11 @@ AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const void AP_Camera::convert_params() { // exit immediately if CAM1_TYPE has already been configured - if (_params[0].type.configured()) { + if (_params[0].type.configured() +#if AP_CAMERA_RUNCAM_ENABLED + && _params[1].type.configured() +#endif + ) { return; } @@ -919,6 +950,42 @@ void AP_Camera::convert_params() } _params[0].type.set_and_save(cam1_type); +#if AP_CAMERA_RUNCAM_ENABLED + // RunCam PARAMETER_CONVERSION - Added: Nov-2024 ahead of 4.7 release + + // Since slot 1 is essentially used by the trigger type, we will use slot 2 for runcam + int8_t rc_type = 0; + // find vehicle's top level key + uint16_t k_param_vehicle_key; + if (!AP_Param::find_top_level_key_by_pointer(AP::vehicle(), k_param_vehicle_key)) { + return; + } + + // RunCam protocol configured so set cam type to RunCam + bool rc_protocol_configured = false; + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + if (serial_manager && serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0)) { + rc_protocol_configured = true; + } + + const AP_Param::ConversionInfo rc_type_info = { + k_param_vehicle_key, AP_GROUP_ELEM_IDX(1, 1), AP_PARAM_INT8, "CAM_RC_TYPE" + }; + AP_Int8 rc_type_old; + const bool found_rc_type = AP_Param::find_old_parameter(&rc_type_info, &rc_type_old); + + if (rc_protocol_configured || (found_rc_type && rc_type_old.get() > 0)) { + rc_type = int8_t(CameraType::RUNCAM); + _backends[1] = NEW_NOTHROW AP_RunCam(*this, _params[1], 1, _runcam_instances); + _backend_var_info[1] = AP_RunCam::var_info; + AP_Param::convert_class(k_param_vehicle_key, &_backends[1], _backend_var_info[1], 1, false); + AP_Param::invalidate_count(); + _runcam_instances++; + } + + _params[1].type.set_and_save(rc_type); +#endif // AP_CAMERA_RUNCAM_ENABLED + // convert CAM_DURATION (in deci-seconds) to CAM1_DURATION (in seconds) int8_t cam_duration = 0; if (AP_Param::get_param_by_index(this, 1, AP_PARAM_INT8, &cam_duration) && (cam_duration > 0)) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a7b7f602ac..e1f0a07433 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -23,6 +23,7 @@ class AP_Camera_Mount; class AP_Camera_MAVLink; class AP_Camera_MAVLinkCamV2; class AP_Camera_Scripting; +class AP_RunCam; /// @class Camera /// @brief Object managing a Photo or video camera @@ -37,6 +38,7 @@ class AP_Camera { friend class AP_Camera_MAVLink; friend class AP_Camera_MAVLinkCamV2; friend class AP_Camera_Scripting; + friend class AP_RunCam; public: @@ -72,6 +74,9 @@ public: #endif #if AP_CAMERA_SCRIPTING_ENABLED SCRIPTING = 7, // Scripting backend +#endif +#if AP_CAMERA_RUNCAM_ENABLED + RUNCAM = 8, // RunCam backend #endif }; @@ -216,6 +221,11 @@ protected: // parameters for backends AP_Camera_Params _params[AP_CAMERA_MAX_INSTANCES]; +#if AP_CAMERA_RUNCAM_ENABLED + // var info pointer for RunCam + static const struct AP_Param::GroupInfo *_backend_var_info[AP_CAMERA_MAX_INSTANCES]; + uint8_t _runcam_instances; +#endif private: diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 749d2985f0..7c4b092996 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting, 8:RunCam // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 90f69ea7da..fa1ac3c487 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -62,3 +62,7 @@ #ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED #define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED #endif + +#ifndef AP_CAMERA_RUNCAM_ENABLED +#define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_RUNCAM_ENABLED +#endif diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp index fadecee6d0..c6d461667b 100644 --- a/libraries/AP_Camera/AP_RunCam.cpp +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -23,7 +23,7 @@ */ #include "AP_RunCam.h" -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -36,7 +36,7 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam device type // @Description: RunCam device type used to determine OSD menu structure and shutter options. // @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split, 3:RunCam Split4 4k, 4:RunCam Hybrid/RunCam Thumb Pro, 5:Runcam 2 4k - AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceType::Disabled), AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceModel::SplitMicro), AP_PARAM_FLAG_ENABLE), // @Param: FEATURES // @DisplayName: RunCam features available @@ -55,13 +55,13 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam button delay before allowing further button presses // @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("BTN_DELAY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), + AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), // @Param: MDE_DELAY // @DisplayName: RunCam mode delay before allowing further button presses // @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("MDE_DELAY", 5, AP_RunCam, _mode_delay_ms, 800), + AP_GROUPINFO("MDE_DELY", 5, AP_RunCam, _mode_delay_ms, 800), // @Param: CONTROL // @DisplayName: RunCam control option @@ -118,13 +118,24 @@ AP_RunCam::Menu AP_RunCam::_menus[RUNCAM_MAX_DEVICE_TYPES] = { { 6, { 3, 10, 2, 2, 8 }}, // Runcam 2 4K }; -AP_RunCam::AP_RunCam() +const char* AP_RunCam::_models[RUNCAM_MAX_DEVICE_TYPES] = { + "SplitMicro", + "Split", + "Split4k", + "Hybrid", + "Run24k" +}; + +AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance) + : AP_Camera_Backend(frontend, params, instance), _runcam_instance(runcam_instance) { AP_Param::setup_object_defaults(this, var_info); - if (_singleton != nullptr) { - AP_HAL::panic("AP_RunCam must be singleton"); + if (_singleton != nullptr && _singleton->_instance == instance) { + AP_HAL::panic("AP_RunCam instance must be a singleton %u\n", instance); + } + if (_singleton == nullptr) { + _singleton = this; } - _singleton = this; _cam_type.set(constrain_int16(_cam_type, 0, RUNCAM_MAX_DEVICE_TYPES)); _video_recording = VideoOption(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT)); } @@ -134,19 +145,19 @@ void AP_RunCam::init() { AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); if (serial_manager) { - uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0); + uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, _runcam_instance); } if (uart != nullptr) { /* if the user has setup a serial port as a runcam then default type to the split micro (Andy's development platform!). This makes setup a bit easier for most users while still enabling parameters to be hidden for users - without a runcam + without a RunCam */ - _cam_type.set_default(int8_t(DeviceType::SplitMicro)); + _cam_type.set_default(int8_t(DeviceModel::SplitMicro)); AP_Param::invalidate_count(); } - if (_cam_type.get() == int8_t(DeviceType::Disabled)) { + if (_cam_type.get() == int8_t(DeviceModel::Disabled)) { uart = nullptr; return; } @@ -156,7 +167,7 @@ void AP_RunCam::init() } // Split and Runcam 2 4k requires two mode presses to get into the menu - if (_cam_type.get() == int8_t(DeviceType::Split) || _cam_type.get() == int8_t(DeviceType::Run24k)) { + if (_cam_type.get() == int8_t(DeviceModel::Split) || _cam_type.get() == int8_t(DeviceModel::Run24k)) { _menu_enter_level = -1; _in_menu = -1; } @@ -221,7 +232,7 @@ void AP_RunCam::osd_option() { // input update loop void AP_RunCam::update() { - if (uart == nullptr || _cam_type.get() == int8_t(DeviceType::Disabled)) { + if (uart == nullptr || _cam_type.get() == int8_t(DeviceModel::Disabled)) { return; } @@ -551,12 +562,12 @@ void AP_RunCam::handle_2_key_simulation_process(Event ev) case Event::IN_MENU_ENTER: // in a sub-menu and save-and-exit was selected - if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms); _sub_menu_pos = 0; _in_menu--; // in the top-menu and save-and-exit was selected - } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _mode_delay_ms); _in_menu--; _state = State::EXITING_MENU; @@ -712,7 +723,7 @@ void AP_RunCam::handle_5_key_simulation_response(const Request& request) // command to start recording AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING; @@ -721,7 +732,7 @@ AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { // command to stop recording AP_RunCam::ControlOperation AP_RunCam::stop_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING; @@ -1058,8 +1069,86 @@ uint8_t AP_RunCam::Request::get_expected_response_length(const Command command) return 0; } +// AP_Camera API + +// return true if healthy +bool AP_RunCam::healthy() const +{ + return camera_ready(); +} + +// momentary switch to change camera between picture and video modes +void AP_RunCam::cam_mode_toggle() { + +} + +// entry point to actually take a picture. returns true on success +bool AP_RunCam::trigger_pic() { + return false; +} + +// send camera information message to GCS +void AP_RunCam::send_camera_information(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready() || _cam_type.get() == 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) { + return; + } + + static const uint8_t vendor_name[32] = "RunCam"; + uint8_t model_name[32] {}; + strncpy((char *)model_name, _models[_cam_type.get()-1], MIN(sizeof(model_name), sizeof(_models[_cam_type.get()-1]))); + const char cam_definition_uri[140] {}; + + // capability flags + uint32_t flags = 0; + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) { + flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO; + } + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) { + flags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE; + } + + // send CAMERA_INFORMATION message + mavlink_msg_camera_information_send( + chan, + AP_HAL::millis(), // time_boot_ms + vendor_name, // vendor_name uint8_t[32] + model_name, // model_name uint8_t[32] + 0, // firmware version uint32_t + NaNf, // sensor_size_h float (mm) + NaNf, // sensor_size_v float (mm) + 0, // sensor_size_v float (mm) + 0, // resolution_h uint16_t (pix) + 0, // resolution_v uint16_t (pix) + 0, // lens_id uint8_t + flags, // flags uint32_t (CAMERA_CAP_FLAGS) + 0, // cam_definition_version uint16_t + cam_definition_uri, // cam_definition_uri char[140] + _instance + 1); // gimbal_device_id uint8_t +} + +// send camera settings message to GCS +void AP_RunCam::send_camera_settings(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready()) { + return; + } + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _video_recording == VideoOption::RECORDING ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + AP_RunCam *AP::runcam() { return AP_RunCam::get_singleton(); } -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED diff --git a/libraries/AP_Camera/AP_RunCam.h b/libraries/AP_Camera/AP_RunCam.h index 9eb6299923..27b329fb68 100644 --- a/libraries/AP_Camera/AP_RunCam.h +++ b/libraries/AP_Camera/AP_RunCam.h @@ -23,7 +23,9 @@ #include "AP_Camera_config.h" -#if HAL_RUNCAM_ENABLED +#include "AP_Camera_Backend.h" + +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -38,10 +40,10 @@ /// @class AP_RunCam /// @brief Object managing a RunCam device -class AP_RunCam +class AP_RunCam : public AP_Camera_Backend { public: - AP_RunCam(); + AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance); // do not allow copies CLASS_NO_COPY(AP_RunCam); @@ -51,7 +53,7 @@ public: return _singleton; } - enum class DeviceType { + enum class DeviceModel { Disabled = 0, SplitMicro = 1, // video support only Split = 2, // camera and video support @@ -79,22 +81,49 @@ public: VIDEO_RECORDING_AT_BOOT = (1 << 4) }; + + // return true if healthy + bool healthy() const override; + + // momentary switch to change camera between picture and video modes + void cam_mode_toggle() override; + + // entry point to actually take a picture. returns true on success + bool trigger_pic() override; + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const override; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + // initialize the RunCam driver - void init(); + void init() override; // camera button simulation bool simulate_camera_button(const ControlOperation operation, const uint32_t transition_timeout = RUNCAM_DEFAULT_BUTTON_PRESS_DELAY); // start the video void start_recording(); // stop the video void stop_recording(); + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool _start_recording) override { + if (_start_recording) { + start_recording(); + } else { + stop_recording(); + } + return true; + } + // enter the OSD menu void enter_osd(); // exit the OSD menu void exit_osd(); // OSD control determined by camera options void osd_option(); - // update loop - void update(); + // update - should be called at 50hz + void update() override; // Check whether arming is allowed bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; @@ -265,6 +294,10 @@ private: static uint8_t _sub_menu_lengths[RUNCAM_NUM_SUB_MENUS]; // shared inbound scratch space uint8_t _recv_buf[RUNCAM_MAX_PACKET_SIZE]; // all the response contexts use same recv buffer + // the runcam instance + uint8_t _runcam_instance; + + static const char* _models[RUNCAM_MAX_DEVICE_TYPES]; class Request; @@ -435,4 +468,4 @@ namespace AP AP_RunCam *runcam(); }; -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED