Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: support gimbal_manager_status and do_gimbal_manager_configure 

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
davidsastresas 2023-05-08 01:07:14 +02:00 committed by Randy Mackay
parent 341910bce4
commit cbec7ee47b
4 changed files with 108 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Mount/AP_Mount.cpp
View File

@ -337,6 +337,26 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_FAILED;
}
// handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
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);
}
if (backend == nullptr) {
return MAV_RESULT_FAILED;
}
return backend->handle_command_do_gimbal_manager_configure(packet, msg);
}
void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
mavlink_gimbal_manager_set_attitude_t packet;
mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet);
@ -402,7 +422,7 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
}
}
MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
switch (packet.command) {
case MAV_CMD_DO_MOUNT_CONFIGURE:
@ -411,6 +431,8 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
return handle_command_do_mount_control(packet);
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return handle_command_do_gimbal_manager_pitchyaw(packet);
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
return handle_command_do_gimbal_manager_configure(packet, msg);
default:
return MAV_RESULT_UNSUPPORTED;
}
@ -485,6 +507,17 @@ void AP_Mount::send_gimbal_manager_information(mavlink_channel_t chan)
}
}
// send a GIMBAL_MANAGER_STATUS message to GCS
void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
{
// 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_gimbal_manager_status(chan);
}
}
}
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success
bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)

6
libraries/AP_Mount/AP_Mount.h
View File

@ -149,7 +149,7 @@ public:
void set_target_sysid(uint8_t instance, uint8_t sysid);
// mavlink message handling:
MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
void handle_param_value(const mavlink_message_t &msg);
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
@ -159,6 +159,9 @@ public:
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void send_gimbal_manager_information(mavlink_channel_t chan);
// send a GIMBAL_MANAGER_STATUS message to GCS
void send_gimbal_manager_status(mavlink_channel_t chan);
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success
bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
@ -218,6 +221,7 @@ private:
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
void handle_global_position_int(const mavlink_message_t &msg);
void handle_gimbal_device_information(const mavlink_message_t &msg);

55
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -171,6 +171,25 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
radians(_params.yaw_angle_max)); // yaw_max in radians
}
// send a GIMBAL_MANAGER_STATUS message to GCS
void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
{
uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
if (_yaw_lock) {
flags |= GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
}
mavlink_msg_gimbal_manager_status_send(chan,
AP_HAL::millis(), // autopilot system time
flags, // bitmap of gimbal manager flags
_instance + 1, // gimbal device id
mavlink_control_id.sysid, // primary control system id
mavlink_control_id.compid, // primary control component id
0, // secondary control system id
0); // secondary control component id
}
// process MOUNT_CONTROL messages received from GCS. deprecated.
void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
{
@ -267,6 +286,42 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
}
}
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
// sanity check param1 and param2 values
if ((packet.param1 < -3) || (packet.param1 > UINT8_MAX) || (packet.param2 < -3) || (packet.param2 > UINT8_MAX)) {
return MAV_RESULT_FAILED;
}
// convert negative packet1 and packet2 values
int16_t new_sysid = packet.param1;
switch (new_sysid) {
case -1:
// leave unchanged
break;
case -2:
// set itself in control
mavlink_control_id.sysid = msg.sysid;
mavlink_control_id.compid = msg.compid;
break;
case -3:
// remove control if currently in control
if ((mavlink_control_id.sysid == msg.sysid) && (mavlink_control_id.compid == msg.compid)) {
mavlink_control_id.sysid = 0;
mavlink_control_id.compid = 0;
}
break;
default:
mavlink_control_id.sysid = packet.param1;
mavlink_control_id.compid = packet.param2;
break;
}
return MAV_RESULT_ACCEPTED;
}
// 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)
{

14
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -86,6 +86,10 @@ public:
// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
// requires original message in order to extract caller's sysid and compid
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
// process MOUNT_CONFIGURE messages received from GCS. deprecated.
void handle_mount_configure(const mavlink_mount_configure_t &msg);
@ -102,6 +106,9 @@ public:
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void send_gimbal_manager_information(mavlink_channel_t chan);
// send a GIMBAL_MANAGER_STATUS message to GCS
void send_gimbal_manager_status(mavlink_channel_t chan);
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
@ -229,6 +236,13 @@ protected:
bool _target_sysid_location_set;// true if _target_sysid has been set
uint32_t _last_warning_ms; // system time of last warning sent to GCS
// structure holding mavlink sysid and compid of controller of this gimbal
// see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS
struct {
uint8_t sysid;
uint8_t compid;
} mavlink_control_id;
};
#endif // HAL_MOUNT_ENABLED