aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: replace send-mount-status with send-gimbal-device-attitude-status

Browse Source
This commit is contained in:
Randy Mackay 2022-07-11 17:07:22 +09:00
parent b75696e4e6
commit b4979e2cfa
16 changed files with 117 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Mount/AP_Mount.cpp
View File

@ -742,13 +742,13 @@ void AP_Mount::handle_mount_control(const mavlink_message_t &msg)
_backends[_primary]->handle_mount_control(packet);
}
/// Return mount status information
void AP_Mount::send_mount_status(mavlink_channel_t chan)
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
{
// call send_mount_status for each instance
// call send_gimbal_device_attitude_status for each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->send_mount_status(chan);
_backends[instance]->send_gimbal_device_attitude_status(chan);
}
}
}

4
libraries/AP_Mount/AP_Mount.h
View File

@ -146,8 +146,8 @@ public:
void handle_param_value(const mavlink_message_t &msg);
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
// send a MOUNT_STATUS message to GCS:
void send_mount_status(mavlink_channel_t chan);
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
// run pre-arm check. returns false on failure and fills in failure_msg
// any failure_msg returned will not include a prefix

12
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -102,15 +102,19 @@ bool AP_Mount_Alexmos::has_pan_control() const
return _gimbal_3axis;
}
// 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)
// get attitude as a quaternion. returns true on success
bool AP_Mount_Alexmos::get_attitude_quaternion(Quaternion& att_quat)
{
if (!_initialised) {
return;
return false;
}
// request attitude from gimbal
get_angles();
mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100, _mode);
// construct quaternion
att_quat.from_euler(radians(_current_angle.x), radians(_current_angle.y), radians(_current_angle.z));
return true;
}
/*

6
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -81,8 +81,10 @@ public:
// has_pan_control - returns true if this mount can control its pan (required for multicopters)
bool has_pan_control() const 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;
protected:
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:

38
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -65,6 +65,34 @@ void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &p
_state._stab_pan.set(packet.stab_yaw);
}
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan)
{
if (suppress_heartbeat()) {
// block heartbeat from transmitting to the GCS
GCS_MAVLINK::disable_channel_routing(chan);
}
Quaternion att_quat;
if (!get_attitude_quaternion(att_quat)) {
return;
}
// construct quaternion array
const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4};
mavlink_msg_gimbal_device_attitude_status_send(chan,
0, // target system
0, // target component
AP_HAL::millis(), // autopilot system time
get_gimbal_device_flags(),
quat_array, // attitude expressed as quaternion
std::numeric_limits<double>::quiet_NaN(), // roll axis angular velocity (NaN for unknown)
std::numeric_limits<double>::quiet_NaN(), // pitch axis angular velocity (NaN for unknown)
std::numeric_limits<double>::quiet_NaN(), // yaw axis angular velocity (NaN for unknown)
0); // failure flags (not supported)
}
// process MOUNT_CONTROL messages received from GCS. deprecated.
void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
{
@ -310,6 +338,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
}
}
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
{
const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
(get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame
return flags;
}
// get angle targets (in radians) to home location
// returns true on success, false on failure
bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const

13
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -82,8 +82,8 @@ public:
// process MOUNT_CONTROL messages received from GCS. deprecated.
void handle_mount_control(const mavlink_mount_control_t &packet);
// send_mount_status - called to allow mounts to send their status to GCS via MAVLink
virtual void send_mount_status(mavlink_channel_t chan) = 0;
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
@ -115,6 +115,12 @@ protected:
bool yaw_is_ef;
};
// returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
virtual bool suppress_heartbeat() const { return false; }
// get attitude as a quaternion. returns true on success
virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0;
// get pilot input (in the range -1 to +1) received through RC
void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
@ -153,6 +159,9 @@ protected:
// assumes a 50hz update rate
void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t get_gimbal_device_flags() const;
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

24
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -121,31 +121,17 @@ bool AP_Mount_Gremsy::healthy() const
return true;
}
// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_Gremsy::send_mount_status(mavlink_channel_t chan)
// get attitude as a quaternion. returns true on success
bool AP_Mount_Gremsy::get_attitude_quaternion(Quaternion& att_quat)
{
// check we have space for the message
if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_ATTITUDE_STATUS)) {
return;
}
// check we have received an updated message
if (_gimbal_device_attitude_status.time_boot_ms == _sent_gimbal_device_attitude_status_ms) {
return;
return false;
}
_sent_gimbal_device_attitude_status_ms = _gimbal_device_attitude_status.time_boot_ms;
// forward on message to GCS
mavlink_msg_gimbal_device_attitude_status_send(chan,
0, // target system
0, // target component
AP_HAL::millis(), // autopilot system time
_gimbal_device_attitude_status.flags,
_gimbal_device_attitude_status.q,
_gimbal_device_attitude_status.angular_velocity_x,
_gimbal_device_attitude_status.angular_velocity_y,
_gimbal_device_attitude_status.angular_velocity_z,
_gimbal_device_attitude_status.failure_flags);
att_quat = _gimbal_device_attitude_status.q;
return true;
}
// search for gimbal in GCS_MAVLink routing table

8
libraries/AP_Mount/AP_Mount_Gremsy.h
View File

@ -36,15 +36,17 @@ public:
// has_pan_control
bool has_pan_control() const override { return true; }
// send_mount_status
void send_mount_status(mavlink_channel_t chan) override;
// handle GIMBAL_DEVICE_INFORMATION message
void handle_gimbal_device_information(const mavlink_message_t &msg) override;
// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override;
protected:
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:
// search for gimbal in GCS_MAVLink routing table

8
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -123,11 +123,11 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_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)
// get attitude as a quaternion. returns true on success
bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat)
{
// 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, degrees(_angle_rad.pitch)*100, degrees(_angle_rad.roll)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
return true;
}
// search for gimbal in GCS_MAVLink routing table

6
libraries/AP_Mount/AP_Mount_SToRM32.h
View File

@ -35,8 +35,10 @@ public:
// 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;
protected:
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:

8
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -152,11 +152,11 @@ void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_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)
// get attitude as a quaternion. returns true on success
bool AP_Mount_SToRM32_serial::get_attitude_quaternion(Quaternion& att_quat)
{
// 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, _mode);
att_quat.from_euler(radians(_current_angle.x * 0.01f), radians(_current_angle.y * 0.01f), radians(_current_angle.z * 0.01f));
return true;
}
bool AP_Mount_SToRM32_serial::can_send(bool with_control) {

6
libraries/AP_Mount/AP_Mount_SToRM32_serial.h
View File

@ -38,8 +38,10 @@ public:
// 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;
protected:
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:

11
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -120,14 +120,15 @@ bool AP_Mount_Servo::has_pan_control() const
return SRV_Channels::function_assigned(_pan_idx);
}
// private methods
// 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)
// get attitude as a quaternion. returns true on success
bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
{
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);
att_quat.from_euler(radians(_angle_bf_output_deg.x), radians(_angle_bf_output_deg.y), radians(_angle_bf_output_deg.z));
return true;
}
// private methods
// update body-frame angle outputs from earth-frame angle targets
void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
{

6
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -39,8 +39,10 @@ public:
// returns true if this mount can control its pan (required for multicopters)
bool has_pan_control() const 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;
protected:
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:

13
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -115,15 +115,14 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_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)
// get attitude as a quaternion. returns true on success
bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
{
if (_gimbal.aligned()) {
mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.roll)*100, degrees(_angle_rad.pitch)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
if (!_gimbal.aligned()) {
return false;
}
// block heartbeat from transmitting to the GCS
GCS_MAVLINK::disable_channel_routing(chan);
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
return true;
}
/*

11
libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -35,9 +35,6 @@ public:
// 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;
// handle a GIMBAL_REPORT message
void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) override;
void handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t &msg);
@ -45,6 +42,14 @@ public:
void update_fast() override;
protected:
// returns true if heart beat should be suppressed for this gimbal
bool suppress_heartbeat() const override { return true; }
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;
private:
// internal variables
bool _initialised; // true once the driver has been initialised