From fb98ef21aab9d12bfb8171ea7bd9d591e9b527ab Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 2 Mar 2023 15:03:02 +1100 Subject: [PATCH] AP_Mount: replace check_instance with get_instance --- libraries/AP_Mount/AP_Mount.cpp | 169 ++++++++++++++++++-------------- libraries/AP_Mount/AP_Mount.h | 8 +- 2 files changed, 102 insertions(+), 75 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index d99d785969..c44014155e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -181,91 +181,97 @@ AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const // has_pan_control - returns true if the mount has yaw control (required for copters) bool AP_Mount::has_pan_control(uint8_t instance) const { - if (!check_instance(instance)) { + const auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } // ask backend if it support pan - return _backends[instance]->has_pan_control(); + return backend->has_pan_control(); } // get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point) MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const { - // sanity check instance - if (!check_instance(instance)) { + const auto *backend = get_instance(instance); + if (backend == nullptr) { return MAV_MOUNT_MODE_RETRACT; } // ask backend its mode - return _backends[instance]->get_mode(); + return backend->get_mode(); } // set_mode_to_default - restores the mode to it's default mode held in the MNTx__DEFLT_MODE parameter // this operation requires 60us on a Pixhawk/PX4 void AP_Mount::set_mode_to_default(uint8_t instance) { - if (instance > ARRAY_SIZE(_params)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } - set_mode(instance, (enum MAV_MOUNT_MODE)_params[instance].default_mode.get()); + backend->set_mode((enum MAV_MOUNT_MODE)_params[instance].default_mode.get()); } // set_mode - sets mount's mode void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) { - // sanity check instance - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } // call backend's set_mode - _backends[instance]->set_mode(mode); + backend->set_mode(mode); } // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock) { - // sanity check instance - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } // call backend's set_yaw_lock - _backends[instance]->set_yaw_lock(yaw_lock); + backend->set_yaw_lock(yaw_lock); } // set angle target in degrees // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame void AP_Mount::set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } // send command to backend - _backends[instance]->set_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); + backend->set_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); } // sets rate target in deg/s // yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle) void AP_Mount::set_rate_target(uint8_t instance, float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } // send command to backend - _backends[instance]->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock); + backend->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock); } MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_long_t &packet) { - if (!check_primary()) { + auto *backend = get_primary(); + if (backend == nullptr) { return MAV_RESULT_FAILED; } - _backends[_primary]->set_mode((MAV_MOUNT_MODE)packet.param1); + + backend->set_mode((MAV_MOUNT_MODE)packet.param1); return MAV_RESULT_ACCEPTED; } @@ -273,33 +279,40 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_lon MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_t &packet) { - if (!check_primary()) { + auto *backend = get_primary(); + if (backend == nullptr) { return MAV_RESULT_FAILED; } - return _backends[_primary]->handle_command_do_mount_control(packet); + return backend->handle_command_do_mount_control(packet); } MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet) { - // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc - if ((packet.param7 < 0) || (packet.param7 > AP_MOUNT_MAX_INSTANCES)) { - return MAV_RESULT_FAILED; + AP_Mount_Backend *backend; + + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is + // 2nd gimbal, etc + const uint8_t instance = packet.param7; + if (instance == 0) { + backend = get_primary(); + } else { + backend = get_instance(instance - 1); } - const uint8_t gimbal_instance = (packet.param7 < 1) ? get_primary() : (uint8_t)packet.param7 - 1; - if (!check_instance(gimbal_instance)) { + + if (backend == nullptr) { return MAV_RESULT_FAILED; } // check flags for change to RETRACT uint32_t flags = (uint32_t)packet.param5; if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { - _backends[gimbal_instance]->set_mode(MAV_MOUNT_MODE_RETRACT); + backend->set_mode(MAV_MOUNT_MODE_RETRACT); return MAV_RESULT_ACCEPTED; } // check flags for change to NEUTRAL if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) { - _backends[gimbal_instance]->set_mode(MAV_MOUNT_MODE_NEUTRAL); + backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); return MAV_RESULT_ACCEPTED; } @@ -308,7 +321,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com const float pitch_angle_deg = packet.param1; const float yaw_angle_deg = packet.param2; if (!isnan(pitch_angle_deg) && !isnan(yaw_angle_deg)) { - set_angle_target(gimbal_instance, 0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + backend->set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return MAV_RESULT_ACCEPTED; } @@ -317,7 +330,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com const float pitch_rate_degs = packet.param3; const float yaw_rate_degs = packet.param4; if (!isnan(pitch_rate_degs) && !isnan(yaw_rate_degs)) { - set_rate_target(gimbal_instance, 0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return MAV_RESULT_ACCEPTED; } @@ -359,7 +372,8 @@ void AP_Mount::handle_global_position_int(const mavlink_message_t &msg) /// Change the configuration of the mount void AP_Mount::handle_mount_configure(const mavlink_message_t &msg) { - if (!check_primary()) { + auto *backend = get_primary(); + if (backend == nullptr) { return; } @@ -367,13 +381,14 @@ void AP_Mount::handle_mount_configure(const mavlink_message_t &msg) mavlink_msg_mount_configure_decode(&msg, &packet); // send message to backend - _backends[_primary]->handle_mount_configure(packet); + backend->handle_mount_configure(packet); } /// Control the mount (depends on the previously set mount configuration) void AP_Mount::handle_mount_control(const mavlink_message_t &msg) { - if (!check_primary()) { + auto *backend = get_primary(); + if (backend == nullptr) { return; } @@ -381,7 +396,7 @@ void AP_Mount::handle_mount_control(const mavlink_message_t &msg) mavlink_msg_mount_control_decode(&msg, &packet); // send message to backend - _backends[_primary]->handle_mount_control(packet); + backend->handle_mount_control(packet); } // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS @@ -399,13 +414,14 @@ void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan) // returns true on success bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } // re-use get_attitude_quaternion and convert to Euler angles Quaternion att_quat; - if (!_backends[instance]->get_attitude_quaternion(att_quat)) { + if (!backend->get_attitude_quaternion(att_quat)) { return false; } @@ -448,60 +464,68 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) // accessors for scripting backends bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame); + return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame); } bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); + return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); } bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->get_location_target(target_loc); + return backend->get_location_target(target_loc); } void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return; } - _backends[instance]->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); + backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } bool AP_Mount::get_camera_state(uint8_t instance, uint16_t& pic_count, bool& record_video, int8_t& zoom_step, int8_t& focus_step, bool& auto_focus) { - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->get_camera_state(pic_count, record_video, zoom_step, focus_step, auto_focus); + return backend->get_camera_state(pic_count, record_video, zoom_step, focus_step, auto_focus); } // point at system ID sysid void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) { - // call instance's set_roi_cmd - if (check_instance(instance)) { - _backends[instance]->set_target_sysid(sysid); + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; } + // call instance's set_roi_cmd + backend->set_target_sysid(sysid); } // set_roi_target - sets target location that mount should attempt to point towards void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc) { - // call instance's set_roi_cmd - if (check_instance(instance)) { - _backends[instance]->set_roi_target(target_loc); + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; } + backend->set_roi_target(target_loc); } // @@ -511,65 +535,68 @@ void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc) // take a picture. returns true on success bool AP_Mount::take_picture(uint8_t instance) { - // call instance's take_picture - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->take_picture(); + return backend->take_picture(); } // start or stop video recording. returns true on success // set start_recording = true to start record, false to stop recording bool AP_Mount::record_video(uint8_t instance, bool start_recording) { - // call instance's record_video - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->record_video(start_recording); + return backend->record_video(start_recording); } // set camera zoom step. returns true on success // zoom out = -1, hold = 0, zoom in = 1 bool AP_Mount::set_zoom_step(uint8_t instance, int8_t zoom_step) { - // call instance's set_zoom_step - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->set_zoom_step(zoom_step); + return backend->set_zoom_step(zoom_step); } // set focus in, out or hold. returns true on success // focus in = -1, focus hold = 0, focus out = 1 bool AP_Mount::set_manual_focus_step(uint8_t instance, int8_t focus_step) { - // call instance's set_manual_focus_step - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->set_manual_focus_step(focus_step); + return backend->set_manual_focus_step(focus_step); } // auto focus. returns true on success bool AP_Mount::set_auto_focus(uint8_t instance) { - // call instance's set_auto_focus - if (!check_instance(instance)) { + auto *backend = get_instance(instance); + if (backend == nullptr) { return false; } - return _backends[instance]->set_auto_focus(); + return backend->set_auto_focus(); } -bool AP_Mount::check_primary() const +AP_Mount_Backend *AP_Mount::get_primary() const { - return check_instance(_primary); + return get_instance(_primary); } -bool AP_Mount::check_instance(uint8_t instance) const +AP_Mount_Backend *AP_Mount::get_instance(uint8_t instance) const { - return instance < AP_MOUNT_MAX_INSTANCES && _backends[instance] != nullptr; + if (instance >= ARRAY_SIZE(_backends)) { + return nullptr; + } + return _backends[instance]; } // pass a GIMBAL_REPORT message to the backend diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 95f2c74467..56158ca8c9 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -103,8 +103,8 @@ public: // used for gimbals that need to read INS data at full rate void update_fast(); - // return primary instance - uint8_t get_primary() const { return _primary; } + // return primary instance ID + uint8_t get_primary_instance() const { return _primary; } // get_mount_type - returns the type of mount AP_Mount::MountType get_mount_type() const { return get_mount_type(_primary); } @@ -213,8 +213,8 @@ protected: private: // Check if instance backend is ok - bool check_primary() const; - bool check_instance(uint8_t instance) const; + AP_Mount_Backend *get_primary() const; + AP_Mount_Backend *get_instance(uint8_t instance) const; void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg); void handle_mount_configure(const mavlink_message_t &msg);