diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 03c48c951e..965d52fd67 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -441,9 +441,6 @@ void AP_Mount::init() // create each instance for (uint8_t instance=0; instanceinit(); + set_mode_to_default(instance); } } } @@ -553,11 +551,12 @@ bool AP_Mount::has_pan_control(uint8_t instance) const MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const { // sanity check instance - if (instance >= AP_MOUNT_MAX_INSTANCES) { - return MAV_MOUNT_MODE_RETRACT; + if (!check_instance(instance)) { + return MAV_MOUNT_MODE_RETRACT; } - return state[instance]._mode; + // ask backend its mode + return _backends[instance]->get_mode(); } // set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter @@ -588,8 +587,8 @@ void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock) return; } - // return immediately if no change - state[instance]._yaw_lock = yaw_lock; + // call backend's set_yaw_lock + _backends[instance]->set_yaw_lock(yaw_lock); } // set_angle_targets - sets angle targets in degrees diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 152812048c..bd873dddb1 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -190,15 +190,6 @@ protected: AP_Float _roll_stb_lead; // roll lead control gain AP_Float _pitch_stb_lead; // pitch lead control gain - MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) - bool _yaw_lock; // If true the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - struct Location _roi_target; // roi target location - bool _roi_target_set; - - uint8_t _target_sysid; // sysid to track - Location _target_sysid_location; // sysid target location - bool _target_sysid_location_set; - } state[AP_MOUNT_MAX_INSTANCES]; private: diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 593aa451dc..8d42e8b829 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -15,6 +15,7 @@ void AP_Mount_Alexmos::init() _initialised = true; get_boardinfo(); read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters + set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); } } @@ -64,8 +65,8 @@ void AP_Mount_Alexmos::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false, true)) { control_axis(_angle_ef_target_rad, false); } @@ -89,13 +90,6 @@ bool AP_Mount_Alexmos::has_pan_control() const return _gimbal_3axis; } -// set_mode - sets mount's mode -void AP_Mount_Alexmos::set_mode(enum MAV_MOUNT_MODE mode) -{ - // record the mode change and return success - _state._mode = mode; -} - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_Alexmos::send_mount_status(mavlink_channel_t chan) { @@ -104,7 +98,7 @@ void AP_Mount_Alexmos::send_mount_status(mavlink_channel_t chan) } get_angles(); - mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100, _state._mode); + mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100, _mode); } /* diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index 8f3dc60c61..df73e32b96 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -81,9 +81,6 @@ public: // has_pan_control - returns true if this mount can control its pan (required for multicopters) bool has_pan_control() const override; - // set_mode - sets mount's mode - void set_mode(enum MAV_MOUNT_MODE mode) override; - // send_mount_status - called to allow mounts to send their status to GCS via MAVLink void send_mount_status(mavlink_channel_t chan) override; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 41413122c6..b879496871 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -15,27 +15,27 @@ void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan) _angle_ef_target_rad.z = radians(pan); // set the mode to mavlink targeting - _frontend.set_mode(_instance, MAV_MOUNT_MODE_MAVLINK_TARGETING); + set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); } // set_roi_target - sets target location that mount should attempt to point towards void AP_Mount_Backend::set_roi_target(const Location &target_loc) { // set the target gps location - _state._roi_target = target_loc; - _state._roi_target_set = true; + _roi_target = target_loc; + _roi_target_set = true; // set the mode to GPS tracking mode - _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT); + set_mode(MAV_MOUNT_MODE_GPS_POINT); } // set_sys_target - sets system that mount should attempt to point towards void AP_Mount_Backend::set_target_sysid(uint8_t sysid) { - _state._target_sysid = sysid; + _target_sysid = sysid; // set the mode to sysid tracking mode - _frontend.set_mode(_instance, MAV_MOUNT_MODE_SYSID_TARGET); + set_mode(MAV_MOUNT_MODE_SYSID_TARGET); } // process MOUNT_CONFIGURE messages received from GCS. deprecated. @@ -50,15 +50,15 @@ void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &p // process MOUNT_CONTROL messages received from GCS. deprecated. void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet) { - control((int32_t)packet.input_a, (int32_t)packet.input_b, (int32_t)packet.input_c, _state._mode); + control((int32_t)packet.input_a, (int32_t)packet.input_b, (int32_t)packet.input_c, _mode); } void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode) { - _frontend.set_mode(_instance, mount_mode); + set_mode(mount_mode); // interpret message fields based on mode - switch (_frontend.get_mode(_instance)) { + switch (get_mode()) { case MAV_MOUNT_MODE_RETRACT: case MAV_MOUNT_MODE_NEUTRAL: // do nothing with request if mount is retracted or in neutral position @@ -89,8 +89,8 @@ void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_ case MAV_MOUNT_MODE_HOME_LOCATION: { // set the target gps location - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; break; } @@ -103,15 +103,15 @@ void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_ // handle a GLOBAL_POSITION_INT message bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet) { - if (_state._target_sysid != msg_sysid) { + if (_target_sysid != msg_sysid) { return false; } - _state._target_sysid_location.lat = packet.lat; - _state._target_sysid_location.lng = packet.lon; + _target_sysid_location.lat = packet.lat; + _target_sysid_location.lng = packet.lon; // global_position_int.alt is *UP*, so is location. - _state._target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE); - _state._target_sysid_location_set = true; + _target_sysid_location.set_alt_cm(packet.alt*0.1, Location::AltFrame::ABSOLUTE); + _target_sysid_location_set = true; return true; } @@ -168,21 +168,21 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const { - if (!_state._roi_target_set) { + if (!_roi_target_set) { return false; } - return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); + return calc_angle_to_location(_roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); } bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const { - if (!_state._target_sysid_location_set) { + if (!_target_sysid_location_set) { return false; } - if (!_state._target_sysid) { + if (!_target_sysid) { return false; } - return calc_angle_to_location(_state._target_sysid_location, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); + return calc_angle_to_location(_target_sysid_location, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); } // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (in radians) to point at the given target diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index ff679f3214..05ee2a6492 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -49,8 +49,15 @@ public: // has_pan_control - returns true if this mount can control it's pan (required for multicopters) virtual bool has_pan_control() const = 0; - // set_mode - sets mount's mode - virtual void set_mode(enum MAV_MOUNT_MODE mode) = 0; + // get mount's mode + enum MAV_MOUNT_MODE get_mode() const { return _mode; } + + // set mount's mode + virtual void set_mode(enum MAV_MOUNT_MODE mode) { _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 set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; } // set_angle_targets - sets angle targets in degrees void set_angle_targets(float roll, float tilt, float pan); @@ -110,15 +117,23 @@ protected: // by various mavlink messages) bool calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const WARN_IF_UNUSED; - // get the mount mode from frontend - MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); } AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount::mount_state &_state; // references to the parameters and state for this backend uint8_t _instance; // this instance's number + + MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) + bool _yaw_lock; // True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame + Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and vehicle-relative pan angles in radians Vector3f _rate_target_rads; // desired roll, pitch, yaw rate in radians/sec bool _rate_target_rads_valid;// true if _rate_target_rads should can be used (e.g. RC input is using rate control) + Location _roi_target; // roi target location + bool _roi_target_set; // true if the roi target has been set + + uint8_t _target_sysid; // sysid to track + Location _target_sysid_location;// sysid target location + bool _target_sysid_location_set;// true if _target_sysid has been set private: diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 8f7738e5cc..8418bb0179 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -66,8 +66,8 @@ void AP_Mount_Gremsy::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, false)) { send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, true); } diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 52950dec34..31cc7c2baf 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -36,9 +36,6 @@ public: // has_pan_control bool has_pan_control() const override { return true; } - // set_mode - void set_mode(enum MAV_MOUNT_MODE mode) override { _state._mode = mode; } - // send_mount_status void send_mount_status(mavlink_channel_t chan) override; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 8a614090ce..690ec79842 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -73,8 +73,8 @@ void AP_Mount_SToRM32::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) { resend_now = true; } @@ -113,14 +113,14 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode) } // record the mode change - _state._mode = mode; + _mode = mode; } // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan) { // return target angles as gimbal's actual attitude. To-Do: retrieve actual gimbal attitude and send these instead - mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100, _state._mode); + mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100, _mode); } // search for gimbal in GCS_MAVLink routing table diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 5cd75c6fa4..9dba14ea82 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -86,8 +86,8 @@ void AP_Mount_SToRM32_serial::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) { resend_now = true; } @@ -142,14 +142,14 @@ void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_MODE mode) } // record the mode change - _state._mode = mode; + _mode = mode; } // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_SToRM32_serial::send_mount_status(mavlink_channel_t chan) { // return target angles as gimbal's actual attitude. - mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z, _state._mode); + mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z, _mode); } bool AP_Mount_SToRM32_serial::can_send(bool with_control) { diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index ce9934466f..9377448006 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -81,8 +81,8 @@ void AP_Mount_Servo::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, true)) { stabilize(); } @@ -115,13 +115,6 @@ void AP_Mount_Servo::update() move_servo(_pan_idx, _angle_bf_output_deg.z*10, _state._pan_angle_min*0.1f, _state._pan_angle_max*0.1f); } -// set_mode - sets mount's mode -void AP_Mount_Servo::set_mode(enum MAV_MOUNT_MODE mode) -{ - // record the mode change and return success - _state._mode = mode; -} - // private methods // check_servo_map - detects which axis we control using the functions assigned to the servos in the RC_Channel_aux @@ -136,7 +129,7 @@ void AP_Mount_Servo::check_servo_map() // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_Servo::send_mount_status(mavlink_channel_t chan) { - mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100, _state._mode); + mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100, _mode); } // stabilize - stabilizes the mount relative to the Earth's frame diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 28708a090c..c8ef7aa310 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -39,9 +39,6 @@ public: // has_pan_control - returns true if this mount can control it's pan (required for multicopters) bool has_pan_control() const override { return _flags.pan_control; } - // set_mode - sets mount's mode - void set_mode(enum MAV_MOUNT_MODE mode) override; - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void send_mount_status(mavlink_channel_t chan) override; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index b9eb258bd5..67972d766f 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -78,8 +78,8 @@ void AP_Mount_SoloGimbal::update() if (!AP::ahrs().home_is_set()) { break; } - _state._roi_target = AP::ahrs().get_home(); - _state._roi_target_set = true; + _roi_target = AP::ahrs().get_home(); + _roi_target_set = true; UNUSED_RESULT(calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)); break; @@ -110,14 +110,14 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode) } // record the mode change - _state._mode = mode; + _mode = mode; } // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan) { if (_gimbal.aligned()) { - mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100, _state._mode); + mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100, _mode); } // block heartbeat from transmitting to the GCS