Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: move mode, yaw_lock, roi_target and sysid target to backend

Browse Source
This commit is contained in:
Randy Mackay 2022-06-20 20:24:26 +09:00
parent ec07c15e1e
commit 33a4efa936
13 changed files with 68 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Mount/AP_Mount.cpp
View File

@ -441,9 +441,6 @@ void AP_Mount::init()
// create each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
// default instance's state
state[instance]._mode = (enum MAV_MOUNT_MODE)state[instance]._default_mode.get();
MountType mount_type = get_mount_type(instance);
// check for servo mounts
@ -502,6 +499,7 @@ void AP_Mount::init()
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->init();
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

9
libraries/AP_Mount/AP_Mount.h
View File

@ -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:

14
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -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);
}
/*

3
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -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;

42
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -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

23
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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:

4
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -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);
}

3
libraries/AP_Mount/AP_Mount_Gremsy.h
View File

@ -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;

8
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -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

8
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -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) {

13
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -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

3
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -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;

8
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -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