2015-08-11 03:28:44 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include "AP_Mount.h"
|
2020-07-24 14:01:21 -03:00
|
|
|
|
|
|
|
#if HAL_MOUNT_ENABLED
|
|
|
|
|
2015-08-11 03:28:44 -03:00
|
|
|
#include "AP_Mount_Backend.h"
|
|
|
|
#include "AP_Mount_Servo.h"
|
2015-12-30 22:19:52 -04:00
|
|
|
#include "AP_Mount_SoloGimbal.h"
|
2015-08-11 03:28:44 -03:00
|
|
|
#include "AP_Mount_Alexmos.h"
|
|
|
|
#include "AP_Mount_SToRM32.h"
|
|
|
|
#include "AP_Mount_SToRM32_serial.h"
|
2022-05-26 23:57:55 -03:00
|
|
|
#include "AP_Mount_Gremsy.h"
|
2022-09-15 23:31:09 -03:00
|
|
|
#include "AP_Mount_Siyi.h"
|
2022-12-27 21:21:46 -04:00
|
|
|
#include "AP_Mount_Scripting.h"
|
2023-06-20 02:44:43 -03:00
|
|
|
#include "AP_Mount_Xacti.h"
|
2023-07-05 02:35:39 -03:00
|
|
|
#include "AP_Mount_Viewpro.h"
|
2022-09-01 21:27:09 -03:00
|
|
|
#include <stdio.h>
|
2019-03-16 04:06:02 -03:00
|
|
|
#include <AP_Math/location.h>
|
2022-06-14 01:57:26 -03:00
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2012-08-26 20:33:45 -03:00
|
|
|
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Param::GroupInfo AP_Mount::var_info[] = {
|
2021-02-26 14:56:15 -04:00
|
|
|
|
2022-08-30 08:15:10 -03:00
|
|
|
// @Group: 1
|
|
|
|
// @Path: AP_Mount_Params.cpp
|
|
|
|
AP_SUBGROUPINFO(_params[0], "1", 43, AP_Mount, AP_Mount_Params),
|
2015-01-12 08:49:46 -04:00
|
|
|
|
|
|
|
#if AP_MOUNT_MAX_INSTANCES > 1
|
2022-08-30 08:15:10 -03:00
|
|
|
// @Group: 2
|
|
|
|
// @Path: AP_Mount_Params.cpp
|
|
|
|
AP_SUBGROUPINFO(_params[1], "2", 44, AP_Mount, AP_Mount_Params),
|
|
|
|
#endif
|
2015-01-12 08:49:46 -04:00
|
|
|
|
2012-08-17 03:20:30 -03:00
|
|
|
AP_GROUPEND
|
2012-07-02 21:21:01 -03:00
|
|
|
};
|
|
|
|
|
2019-03-26 08:36:36 -03:00
|
|
|
AP_Mount::AP_Mount()
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2018-07-24 20:30:23 -03:00
|
|
|
if (_singleton != nullptr) {
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
AP_HAL::panic("Mount must be singleton");
|
|
|
|
#endif
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
_singleton = this;
|
|
|
|
|
2012-12-12 17:52:04 -04:00
|
|
|
AP_Param::setup_object_defaults(this, var_info);
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// init - detect and initialise all mounts
|
2019-08-27 03:23:30 -03:00
|
|
|
void AP_Mount::init()
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2015-01-08 16:10:48 -04:00
|
|
|
// check init has not been called before
|
|
|
|
if (_num_instances != 0) {
|
|
|
|
return;
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
|
|
|
|
2022-06-16 04:30:01 -03:00
|
|
|
// perform any required parameter conversion
|
|
|
|
convert_params();
|
|
|
|
|
2015-01-18 21:21:47 -04:00
|
|
|
// primary is reset to the first instantiated mount
|
|
|
|
bool primary_set = false;
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// create each instance
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2023-05-26 00:16:55 -03:00
|
|
|
switch (get_mount_type(instance)) {
|
|
|
|
case Type::None:
|
|
|
|
break;
|
2022-06-14 01:55:10 -03:00
|
|
|
#if HAL_MOUNT_SERVO_ENABLED
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::Servo:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_Servo(*this, _params[instance], true, instance);
|
2015-01-08 16:10:48 -04:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-06-14 01:55:10 -03:00
|
|
|
#endif
|
2020-07-24 14:01:21 -03:00
|
|
|
#if HAL_SOLO_GIMBAL_ENABLED
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::SoloGimbal:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_SoloGimbal(*this, _params[instance], instance);
|
2015-01-08 16:10:48 -04:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2020-07-24 14:01:21 -03:00
|
|
|
#endif // HAL_SOLO_GIMBAL_ENABLED
|
2015-01-15 18:47:59 -04:00
|
|
|
|
2022-06-14 01:54:57 -03:00
|
|
|
#if HAL_MOUNT_ALEXMOS_ENABLED
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::Alexmos:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_Alexmos(*this, _params[instance], instance);
|
2015-01-15 18:47:59 -04:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-06-14 01:54:57 -03:00
|
|
|
#endif
|
2015-02-14 08:27:58 -04:00
|
|
|
|
2022-06-14 01:55:37 -03:00
|
|
|
#if HAL_MOUNT_STORM32MAVLINK_ENABLED
|
2015-05-26 04:21:12 -03:00
|
|
|
// check for SToRM32 mounts using MAVLink protocol
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::SToRM32:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_SToRM32(*this, _params[instance], instance);
|
2015-02-14 08:27:58 -04:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-06-14 01:55:37 -03:00
|
|
|
#endif
|
2015-05-26 04:21:12 -03:00
|
|
|
|
2022-06-14 01:56:34 -03:00
|
|
|
#if HAL_MOUNT_STORM32SERIAL_ENABLED
|
2015-05-26 04:21:12 -03:00
|
|
|
// check for SToRM32 mounts using serial protocol
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::SToRM32_serial:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance);
|
2015-05-26 04:21:12 -03:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-06-14 01:56:34 -03:00
|
|
|
#endif
|
2022-05-26 23:57:55 -03:00
|
|
|
|
2022-06-14 01:11:27 -03:00
|
|
|
#if HAL_MOUNT_GREMSY_ENABLED
|
2022-05-26 23:57:55 -03:00
|
|
|
// check for Gremsy mounts
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::Gremsy:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_Gremsy(*this, _params[instance], instance);
|
2022-05-26 23:57:55 -03:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-06-14 01:11:27 -03:00
|
|
|
#endif // HAL_MOUNT_GREMSY_ENABLED
|
2022-08-30 05:20:19 -03:00
|
|
|
|
|
|
|
#if HAL_MOUNT_SERVO_ENABLED
|
|
|
|
// check for BrushlessPWM mounts (uses Servo backend)
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::BrushlessPWM:
|
2022-08-30 08:15:10 -03:00
|
|
|
_backends[instance] = new AP_Mount_Servo(*this, _params[instance], false, instance);
|
2022-08-30 05:20:19 -03:00
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-08-30 05:20:19 -03:00
|
|
|
#endif
|
2022-09-15 23:31:09 -03:00
|
|
|
|
|
|
|
#if HAL_MOUNT_SIYI_ENABLED
|
|
|
|
// check for Siyi gimbal
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::Siyi:
|
2022-09-15 23:31:09 -03:00
|
|
|
_backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance);
|
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-09-15 23:31:09 -03:00
|
|
|
#endif // HAL_MOUNT_SIYI_ENABLED
|
|
|
|
|
2022-12-27 21:21:46 -04:00
|
|
|
#if HAL_MOUNT_SCRIPTING_ENABLED
|
|
|
|
// check for Scripting gimbal
|
2023-05-26 00:16:55 -03:00
|
|
|
case Type::Scripting:
|
2022-12-27 21:21:46 -04:00
|
|
|
_backends[instance] = new AP_Mount_Scripting(*this, _params[instance], instance);
|
|
|
|
_num_instances++;
|
2023-05-26 00:16:55 -03:00
|
|
|
break;
|
2022-12-27 21:21:46 -04:00
|
|
|
#endif // HAL_MOUNT_SCRIPTING_ENABLED
|
2023-06-20 02:44:43 -03:00
|
|
|
|
|
|
|
#if HAL_MOUNT_XACTI_ENABLED
|
|
|
|
// check for Xacti gimbal
|
|
|
|
case Type::Xacti:
|
|
|
|
_backends[instance] = new AP_Mount_Xacti(*this, _params[instance], instance);
|
|
|
|
_num_instances++;
|
|
|
|
break;
|
|
|
|
#endif // HAL_MOUNT_XACTI_ENABLED
|
2023-07-05 02:35:39 -03:00
|
|
|
|
|
|
|
#if HAL_MOUNT_VIEWPRO_ENABLED
|
|
|
|
// check for Xacti gimbal
|
|
|
|
case Type::Viewpro:
|
|
|
|
_backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance);
|
|
|
|
_num_instances++;
|
|
|
|
break;
|
|
|
|
#endif // HAL_MOUNT_VIEWPRO_ENABLED
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
2012-08-26 20:33:45 -03:00
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// init new instance
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2015-01-18 21:21:47 -04:00
|
|
|
if (!primary_set) {
|
|
|
|
_primary = instance;
|
|
|
|
primary_set = true;
|
|
|
|
}
|
2015-01-08 16:10:48 -04:00
|
|
|
}
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
2021-08-24 12:05:18 -03:00
|
|
|
|
|
|
|
// init each instance, do it after all instances were created, so that they all know things
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
|
|
|
if (_backends[instance] != nullptr) {
|
|
|
|
_backends[instance]->init();
|
2022-06-20 08:24:26 -03:00
|
|
|
set_mode_to_default(instance);
|
2021-08-24 12:05:18 -03:00
|
|
|
}
|
|
|
|
}
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// update - give mount opportunity to update servos. should be called at 10hz or higher
|
|
|
|
void AP_Mount::update()
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2015-01-08 16:10:48 -04:00
|
|
|
// update each instance
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2015-01-08 16:10:48 -04:00
|
|
|
_backends[instance]->update();
|
|
|
|
}
|
|
|
|
}
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2015-12-30 22:19:52 -04:00
|
|
|
// used for gimbals that need to read INS data at full rate
|
|
|
|
void AP_Mount::update_fast()
|
|
|
|
{
|
|
|
|
// update each instance
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2015-12-30 22:19:52 -04:00
|
|
|
_backends[instance]->update_fast();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// get_mount_type - returns the type of mount
|
2023-05-24 04:07:42 -03:00
|
|
|
AP_Mount::Type AP_Mount::get_mount_type(uint8_t instance) const
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2015-01-08 16:10:48 -04:00
|
|
|
if (instance >= AP_MOUNT_MAX_INSTANCES) {
|
2023-05-24 04:07:42 -03:00
|
|
|
return Type::None;
|
2015-01-08 16:10:48 -04:00
|
|
|
}
|
|
|
|
|
2023-05-24 04:07:42 -03:00
|
|
|
return (Type)_params[instance].type.get();
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// has_pan_control - returns true if the mount has yaw control (required for copters)
|
|
|
|
bool AP_Mount::has_pan_control(uint8_t instance) const
|
2011-10-31 18:55:58 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
const auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2015-01-08 16:10:48 -04:00
|
|
|
return false;
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
2011-10-31 18:55:58 -03:00
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// ask backend if it support pan
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->has_pan_control();
|
2011-10-31 18:55:58 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// 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
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
const auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-06-20 08:24:26 -03:00
|
|
|
return MAV_MOUNT_MODE_RETRACT;
|
2015-01-08 16:10:48 -04:00
|
|
|
}
|
2012-07-24 23:00:38 -03:00
|
|
|
|
2022-06-20 08:24:26 -03:00
|
|
|
// ask backend its mode
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->get_mode();
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2022-08-30 08:15:10 -03:00
|
|
|
// set_mode_to_default - restores the mode to it's default mode held in the MNTx__DEFLT_MODE parameter
|
2015-11-03 09:46:29 -04:00
|
|
|
// this operation requires 60us on a Pixhawk/PX4
|
2015-01-08 16:10:48 -04:00
|
|
|
void AP_Mount::set_mode_to_default(uint8_t instance)
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2023-03-01 23:10:06 -04:00
|
|
|
return;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_mode((enum MAV_MOUNT_MODE)_params[instance].default_mode.get());
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// set_mode - sets mount's mode
|
|
|
|
void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode)
|
2012-08-05 18:48:57 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2015-01-12 07:41:12 -04:00
|
|
|
return;
|
2015-01-08 16:10:48 -04:00
|
|
|
}
|
2015-01-12 07:41:12 -04:00
|
|
|
|
|
|
|
// call backend's set_mode
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_mode(mode);
|
2012-08-05 18:48:57 -03:00
|
|
|
}
|
|
|
|
|
2022-06-01 01:39:32 -03:00
|
|
|
// 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)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-06-01 01:39:32 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2022-06-20 08:24:26 -03:00
|
|
|
// call backend's set_yaw_lock
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_yaw_lock(yaw_lock);
|
2022-06-01 01:39:32 -03:00
|
|
|
}
|
|
|
|
|
2022-06-20 08:50:44 -03:00
|
|
|
// 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)
|
2015-03-21 08:58:57 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2015-03-21 08:58:57 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// send command to backend
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
|
2022-06-20 08:50:44 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-06-20 08:50:44 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// send command to backend
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock);
|
2015-03-21 08:58:57 -03:00
|
|
|
}
|
|
|
|
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int_t &packet)
|
2012-08-05 18:48:57 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_primary();
|
|
|
|
if (backend == nullptr) {
|
2018-10-11 06:30:03 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
|
|
|
|
backend->set_mode((MAV_MOUNT_MODE)packet.param1);
|
2018-10-11 06:30:03 -03:00
|
|
|
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t &packet)
|
2018-10-11 06:30:03 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_primary();
|
|
|
|
if (backend == nullptr) {
|
2018-10-11 06:30:03 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
2015-01-08 16:10:48 -04:00
|
|
|
}
|
|
|
|
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->handle_command_do_mount_control(packet);
|
2012-08-05 18:48:57 -03:00
|
|
|
}
|
|
|
|
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet)
|
2022-06-02 21:47:00 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
AP_Mount_Backend *backend;
|
|
|
|
|
|
|
|
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is
|
|
|
|
// 2nd gimbal, etc
|
2023-08-17 04:19:03 -03:00
|
|
|
const uint8_t instance = packet.z;
|
2023-03-02 00:03:02 -04:00
|
|
|
if (instance == 0) {
|
|
|
|
backend = get_primary();
|
|
|
|
} else {
|
|
|
|
backend = get_instance(instance - 1);
|
2023-03-01 23:08:27 -04:00
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
|
|
|
|
if (backend == nullptr) {
|
2022-06-02 21:47:00 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check flags for change to RETRACT
|
2023-08-17 04:19:03 -03:00
|
|
|
const uint32_t flags = packet.x;
|
2022-06-02 21:47:00 -03:00
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_mode(MAV_MOUNT_MODE_RETRACT);
|
2022-06-02 21:47:00 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
// check flags for change to NEUTRAL
|
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
|
2022-06-02 21:47:00 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// param1 : pitch_angle (in degrees)
|
|
|
|
// param2 : yaw angle (in degrees)
|
|
|
|
const float pitch_angle_deg = packet.param1;
|
|
|
|
const float yaw_angle_deg = packet.param2;
|
|
|
|
if (!isnan(pitch_angle_deg) && !isnan(yaw_angle_deg)) {
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
2022-06-20 08:50:44 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// param3 : pitch_rate (in deg/s)
|
|
|
|
// param4 : yaw rate (in deg/s)
|
|
|
|
const float pitch_rate_degs = packet.param3;
|
|
|
|
const float yaw_rate_degs = packet.param4;
|
|
|
|
if (!isnan(pitch_rate_degs) && !isnan(yaw_rate_degs)) {
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
2022-06-02 21:47:00 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
2023-05-07 20:07:14 -03:00
|
|
|
// handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
|
2023-05-07 20:07:14 -03:00
|
|
|
{
|
|
|
|
AP_Mount_Backend *backend;
|
|
|
|
|
|
|
|
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
|
2023-08-17 04:19:03 -03:00
|
|
|
const uint8_t instance = packet.z;
|
2023-05-07 20:07:14 -03:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2023-03-24 00:50:40 -03:00
|
|
|
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);
|
|
|
|
|
|
|
|
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.gimbal_device_id;
|
|
|
|
if (instance == 0) {
|
|
|
|
backend = get_primary();
|
|
|
|
} else {
|
|
|
|
backend = get_instance(instance - 1);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check flags for change to RETRACT
|
|
|
|
const uint32_t flags = packet.flags;
|
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
|
|
|
|
backend->set_mode(MAV_MOUNT_MODE_RETRACT);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check flags for change to NEUTRAL
|
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
|
|
|
|
backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
const Quaternion att_quat{packet.q};
|
2023-03-24 22:30:22 -03:00
|
|
|
const Vector3f att_rate_degs {
|
|
|
|
packet.angular_velocity_x,
|
|
|
|
packet.angular_velocity_y,
|
|
|
|
packet.angular_velocity_y
|
|
|
|
};
|
|
|
|
|
|
|
|
// ensure that we are only demanded to a specific attitude or to
|
|
|
|
// achieve a specific rate. Do not allow both to be specified at
|
|
|
|
// the same time:
|
|
|
|
if (!att_quat.is_nan() && !att_rate_degs.is_nan()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2023-03-24 00:50:40 -03:00
|
|
|
if (!att_quat.is_nan()) {
|
|
|
|
// convert quaternion to euler angles
|
2023-04-15 10:50:36 -03:00
|
|
|
Vector3f attitude;
|
|
|
|
att_quat.to_euler(attitude); // attitude is in radians here
|
|
|
|
attitude *= RAD_TO_DEG; // convert to degrees
|
|
|
|
|
|
|
|
backend->set_angle_target(attitude.x, attitude.y, attitude.z, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
2023-03-24 00:50:40 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2023-03-24 22:30:22 -03:00
|
|
|
{
|
2023-03-24 00:50:40 -03:00
|
|
|
const float roll_rate_degs = degrees(packet.angular_velocity_x);
|
|
|
|
const float pitch_rate_degs = degrees(packet.angular_velocity_y);
|
|
|
|
const float yaw_rate_degs = degrees(packet.angular_velocity_z);
|
|
|
|
backend->set_rate_target(roll_rate_degs, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
2022-06-02 21:47:00 -03:00
|
|
|
|
2023-05-22 02:51:11 -03:00
|
|
|
void AP_Mount::handle_command_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg)
|
|
|
|
{
|
|
|
|
mavlink_gimbal_manager_set_pitchyaw_t packet;
|
|
|
|
mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg,&packet);
|
|
|
|
|
|
|
|
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.gimbal_device_id;
|
|
|
|
if (instance == 0) {
|
|
|
|
backend = get_primary();
|
|
|
|
} else {
|
|
|
|
backend = get_instance(instance - 1);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check flags for change to RETRACT
|
|
|
|
uint32_t flags = (uint32_t)packet.flags;
|
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
|
|
|
|
backend->set_mode(MAV_MOUNT_MODE_RETRACT);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
// check flags for change to NEUTRAL
|
|
|
|
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
|
|
|
|
backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Do not allow both angle and rate to be specified at the same time
|
|
|
|
if (!isnan(packet.pitch) && !isnan(packet.yaw) && !isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// pitch and yaw from packet are in radians
|
|
|
|
if (!isnan(packet.pitch) && !isnan(packet.yaw)) {
|
|
|
|
const float pitch_angle_deg = degrees(packet.pitch);
|
|
|
|
const float yaw_angle_deg = degrees(packet.yaw);
|
2023-08-03 06:56:50 -03:00
|
|
|
backend->set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
2023-05-22 02:51:11 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// pitch_rate and yaw_rate from packet are in rad/s
|
|
|
|
if (!isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) {
|
|
|
|
const float pitch_rate_degs = degrees(packet.pitch_rate);
|
|
|
|
const float yaw_rate_degs = degrees(packet.yaw_rate);
|
2023-08-03 06:56:50 -03:00
|
|
|
backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
|
2023-05-22 02:51:11 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-08-25 17:45:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet)
|
|
|
|
{
|
|
|
|
set_target_sysid((uint8_t)packet.param1);
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2018-10-11 06:30:03 -03:00
|
|
|
switch (packet.command) {
|
|
|
|
case MAV_CMD_DO_MOUNT_CONFIGURE:
|
|
|
|
return handle_command_do_mount_configure(packet);
|
|
|
|
case MAV_CMD_DO_MOUNT_CONTROL:
|
|
|
|
return handle_command_do_mount_control(packet);
|
2022-06-02 21:47:00 -03:00
|
|
|
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
|
|
|
|
return handle_command_do_gimbal_manager_pitchyaw(packet);
|
2023-05-07 20:07:14 -03:00
|
|
|
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
|
|
|
|
return handle_command_do_gimbal_manager_configure(packet, msg);
|
2023-08-25 17:45:03 -03:00
|
|
|
case MAV_CMD_DO_SET_ROI_SYSID:
|
|
|
|
return handle_command_do_set_roi_sysid(packet);
|
2018-10-11 06:30:03 -03:00
|
|
|
default:
|
|
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-03-16 04:06:02 -03:00
|
|
|
/// Change the configuration of the mount
|
|
|
|
void AP_Mount::handle_global_position_int(const mavlink_message_t &msg)
|
|
|
|
{
|
|
|
|
mavlink_global_position_int_t packet;
|
|
|
|
mavlink_msg_global_position_int_decode(&msg, &packet);
|
|
|
|
|
|
|
|
if (!check_latlng(packet.lat, packet.lon)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2021-08-25 03:04:18 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
|
|
|
_backends[instance]->handle_global_position_int(msg.sysid, packet);
|
2019-03-16 04:06:02 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-08-23 21:10:29 -03:00
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
|
2018-10-11 06:30:03 -03:00
|
|
|
/// Change the configuration of the mount
|
2019-04-30 07:22:48 -03:00
|
|
|
void AP_Mount::handle_mount_configure(const mavlink_message_t &msg)
|
2018-10-11 06:30:03 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_primary();
|
|
|
|
if (backend == nullptr) {
|
2015-01-08 16:10:48 -04:00
|
|
|
return;
|
2014-06-16 14:50:19 -03:00
|
|
|
}
|
2015-01-08 16:10:48 -04:00
|
|
|
|
2018-10-11 06:30:03 -03:00
|
|
|
mavlink_mount_configure_t packet;
|
2019-04-30 07:22:48 -03:00
|
|
|
mavlink_msg_mount_configure_decode(&msg, &packet);
|
2018-10-11 06:30:03 -03:00
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// send message to backend
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->handle_mount_configure(packet);
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
2012-06-13 15:55:19 -03:00
|
|
|
|
2023-08-23 21:10:29 -03:00
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED
|
2018-10-11 06:30:03 -03:00
|
|
|
/// Control the mount (depends on the previously set mount configuration)
|
2019-04-30 07:22:48 -03:00
|
|
|
void AP_Mount::handle_mount_control(const mavlink_message_t &msg)
|
2015-08-13 02:31:31 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_primary();
|
|
|
|
if (backend == nullptr) {
|
2015-08-13 02:31:31 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2018-10-11 06:30:03 -03:00
|
|
|
mavlink_mount_control_t packet;
|
2019-04-30 07:22:48 -03:00
|
|
|
mavlink_msg_mount_control_decode(&msg, &packet);
|
2018-10-11 06:30:03 -03:00
|
|
|
|
2015-08-13 02:31:31 -03:00
|
|
|
// send message to backend
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->handle_mount_control(packet);
|
2015-08-13 02:31:31 -03:00
|
|
|
}
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
2015-08-13 02:31:31 -03:00
|
|
|
|
2023-09-02 02:21:35 -03:00
|
|
|
#if HAL_GCS_ENABLED
|
2022-07-11 05:07:22 -03:00
|
|
|
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
|
|
|
|
void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
|
2012-06-13 15:55:19 -03:00
|
|
|
{
|
2022-07-11 05:07:22 -03:00
|
|
|
// call send_gimbal_device_attitude_status for each instance
|
2015-01-08 16:10:48 -04:00
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2022-07-11 05:07:22 -03:00
|
|
|
_backends[instance]->send_gimbal_device_attitude_status(chan);
|
2014-08-26 09:52:04 -03:00
|
|
|
}
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
2012-06-13 15:55:19 -03:00
|
|
|
}
|
2015-01-08 16:10:48 -04:00
|
|
|
|
2023-04-24 01:58:58 -03:00
|
|
|
// send a GIMBAL_MANAGER_INFORMATION message to GCS
|
|
|
|
void AP_Mount::send_gimbal_manager_information(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_information(chan);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-05-07 20:07:14 -03:00
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2023-09-02 02:21:35 -03:00
|
|
|
#endif // HAL_GCS_ENABLED
|
2023-05-07 20:07:14 -03:00
|
|
|
|
2022-09-20 02:39:35 -03:00
|
|
|
// 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)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-09-20 02:39:35 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2023-02-15 02:50:34 -04:00
|
|
|
// re-use get_attitude_quaternion and convert to Euler angles
|
|
|
|
Quaternion att_quat;
|
2023-03-02 00:03:02 -04:00
|
|
|
if (!backend->get_attitude_quaternion(att_quat)) {
|
2023-02-15 02:50:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
float roll_rad, pitch_rad, yaw_rad;
|
|
|
|
att_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
|
|
|
|
roll_deg = degrees(roll_rad);
|
|
|
|
pitch_deg = degrees(pitch_rad);
|
|
|
|
yaw_bf_deg = degrees(yaw_rad);
|
|
|
|
return true;
|
2022-09-20 02:39:35 -03:00
|
|
|
}
|
|
|
|
|
2022-06-16 05:13:08 -03:00
|
|
|
// run pre-arm check. returns false on failure and fills in failure_msg
|
|
|
|
// any failure_msg returned will not include a prefix
|
|
|
|
bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
|
|
|
|
{
|
|
|
|
// check type parameters
|
|
|
|
for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
|
2023-05-24 04:07:42 -03:00
|
|
|
if ((get_mount_type(i) != Type::None) && (_backends[i] == nullptr)) {
|
2022-06-16 05:13:08 -03:00
|
|
|
strncpy(failure_msg, "check TYPE", failure_msg_len);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// return true if no mount configured
|
|
|
|
if (_num_instances == 0) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check healthy
|
|
|
|
for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
|
|
|
|
if ((_backends[i] != nullptr) && !_backends[i]->healthy()) {
|
|
|
|
strncpy(failure_msg, "not healthy", failure_msg_len);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// get target rate in deg/sec. returns true on success
|
2022-12-27 21:21:46 -04:00
|
|
|
bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-12-27 21:21:46 -04:00
|
|
|
return false;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame);
|
2022-12-27 21:21:46 -04:00
|
|
|
}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// get target angle in deg. returns true on success
|
2022-12-27 21:21:46 -04:00
|
|
|
bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-12-27 21:21:46 -04:00
|
|
|
return false;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
|
2022-12-27 21:21:46 -04:00
|
|
|
}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// accessors for scripting backends and logging
|
2022-12-27 21:21:46 -04:00
|
|
|
bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-12-27 21:21:46 -04:00
|
|
|
return false;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->get_location_target(target_loc);
|
2022-12-27 21:21:46 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-12-27 21:21:46 -04:00
|
|
|
return;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
|
2022-12-27 21:21:46 -04:00
|
|
|
}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// write mount log packet for all backends
|
|
|
|
void AP_Mount::write_log()
|
|
|
|
{
|
|
|
|
// each instance writes log
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
|
|
|
if (_backends[instance] != nullptr) {
|
|
|
|
_backends[instance]->write_log(0);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us)
|
|
|
|
{
|
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
backend->write_log(timestamp_us);
|
|
|
|
}
|
|
|
|
|
2019-03-16 04:06:02 -03:00
|
|
|
// point at system ID sysid
|
|
|
|
void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
2019-03-16 04:06:02 -03:00
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
// call instance's set_roi_cmd
|
|
|
|
backend->set_target_sysid(sysid);
|
2019-03-16 04:06:02 -03:00
|
|
|
}
|
|
|
|
|
2015-01-08 16:10:48 -04:00
|
|
|
// set_roi_target - sets target location that mount should attempt to point towards
|
2022-06-20 07:59:02 -03:00
|
|
|
void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc)
|
2012-08-05 18:48:57 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
2012-08-17 03:20:30 -03:00
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
backend->set_roi_target(target_loc);
|
2012-08-05 18:48:57 -03:00
|
|
|
}
|
|
|
|
|
2023-02-15 00:52:56 -04:00
|
|
|
// clear_roi_target - clears target location that mount should attempt to point towards
|
|
|
|
void AP_Mount::clear_roi_target(uint8_t instance)
|
|
|
|
{
|
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
backend->clear_roi_target();
|
|
|
|
}
|
|
|
|
|
2022-09-27 22:28:56 -03:00
|
|
|
//
|
|
|
|
// camera controls for gimbals that include a camera
|
|
|
|
//
|
|
|
|
|
|
|
|
// take a picture. returns true on success
|
|
|
|
bool AP_Mount::take_picture(uint8_t instance)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-09-27 22:28:56 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->take_picture();
|
2022-09-27 22:28:56 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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)
|
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-09-27 22:28:56 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-03-02 00:03:02 -04:00
|
|
|
return backend->record_video(start_recording);
|
2022-09-27 22:28:56 -03:00
|
|
|
}
|
|
|
|
|
2023-04-12 09:35:43 -03:00
|
|
|
// set zoom specified as a rate or percentage
|
2023-04-18 22:06:46 -03:00
|
|
|
bool AP_Mount::set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value)
|
2022-09-27 22:28:56 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2022-09-27 22:28:56 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-04-12 09:35:43 -03:00
|
|
|
return backend->set_zoom(zoom_type, zoom_value);
|
2022-09-27 22:28:56 -03:00
|
|
|
}
|
|
|
|
|
2023-04-24 09:23:47 -03:00
|
|
|
// set focus specified as rate, percentage or auto
|
2022-09-27 22:28:56 -03:00
|
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
2023-06-21 03:03:39 -03:00
|
|
|
SetFocusResult AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
|
2022-09-27 22:28:56 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
2023-06-21 03:03:39 -03:00
|
|
|
return SetFocusResult::FAILED;
|
2022-09-27 22:28:56 -03:00
|
|
|
}
|
2023-04-24 09:23:47 -03:00
|
|
|
return backend->set_focus(focus_type, focus_value);
|
2022-09-27 22:28:56 -03:00
|
|
|
}
|
|
|
|
|
2023-07-06 01:55:23 -03:00
|
|
|
// set tracking to none, point or rectangle (see TrackingType enum)
|
|
|
|
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
|
|
|
|
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
|
|
|
|
bool AP_Mount::set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
|
|
|
|
{
|
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return backend->set_tracking(tracking_type, p1, p2);
|
|
|
|
}
|
|
|
|
|
2023-07-14 08:21:22 -03:00
|
|
|
// set camera lens as a value from 0 to 5
|
|
|
|
bool AP_Mount::set_lens(uint8_t instance, uint8_t lens)
|
|
|
|
{
|
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return backend->set_lens(lens);
|
|
|
|
}
|
|
|
|
|
2023-06-12 00:10:19 -03:00
|
|
|
// send camera information message to GCS
|
2023-08-01 15:34:14 -03:00
|
|
|
void AP_Mount::send_camera_information(uint8_t instance, mavlink_channel_t chan) const
|
2023-06-12 00:10:19 -03:00
|
|
|
{
|
2023-08-01 15:34:14 -03:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
2023-06-12 00:10:19 -03:00
|
|
|
}
|
2023-08-10 08:21:47 -03:00
|
|
|
backend->send_camera_information(chan);
|
2023-06-12 00:10:19 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// send camera settings message to GCS
|
2023-08-01 15:34:14 -03:00
|
|
|
void AP_Mount::send_camera_settings(uint8_t instance, mavlink_channel_t chan) const
|
2023-06-12 00:10:19 -03:00
|
|
|
{
|
2023-08-01 15:34:14 -03:00
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return;
|
2023-06-12 00:10:19 -03:00
|
|
|
}
|
2023-08-10 08:21:47 -03:00
|
|
|
backend->send_camera_settings(chan);
|
2023-06-12 00:10:19 -03:00
|
|
|
}
|
|
|
|
|
2023-08-21 09:29:28 -03:00
|
|
|
// get rangefinder distance. Returns true on success
|
|
|
|
bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) const
|
|
|
|
{
|
|
|
|
auto *backend = get_instance(instance);
|
|
|
|
if (backend == nullptr) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return backend->get_rangefinder_distance(distance_m);
|
|
|
|
}
|
|
|
|
|
2023-03-02 00:03:02 -04:00
|
|
|
AP_Mount_Backend *AP_Mount::get_primary() const
|
2020-10-19 11:32:08 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
return get_instance(_primary);
|
2020-10-19 11:32:08 -03:00
|
|
|
}
|
|
|
|
|
2023-03-02 00:03:02 -04:00
|
|
|
AP_Mount_Backend *AP_Mount::get_instance(uint8_t instance) const
|
2020-10-19 11:32:08 -03:00
|
|
|
{
|
2023-03-02 00:03:02 -04:00
|
|
|
if (instance >= ARRAY_SIZE(_backends)) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
return _backends[instance];
|
2020-10-19 11:32:08 -03:00
|
|
|
}
|
|
|
|
|
2015-01-29 05:01:55 -04:00
|
|
|
// pass a GIMBAL_REPORT message to the backend
|
2019-04-30 07:22:48 -03:00
|
|
|
void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
|
2015-01-29 05:01:55 -04:00
|
|
|
{
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2015-01-29 05:01:55 -04:00
|
|
|
_backends[instance]->handle_gimbal_report(chan, msg);
|
|
|
|
}
|
2015-12-30 22:19:52 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-04-30 07:22:48 -03:00
|
|
|
void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
|
2018-10-11 06:30:03 -03:00
|
|
|
{
|
2019-04-30 07:22:48 -03:00
|
|
|
switch (msg.msgid) {
|
2018-10-11 06:30:03 -03:00
|
|
|
case MAVLINK_MSG_ID_GIMBAL_REPORT:
|
|
|
|
handle_gimbal_report(chan, msg);
|
|
|
|
break;
|
2023-08-23 21:10:29 -03:00
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
|
2018-10-11 06:30:03 -03:00
|
|
|
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
|
|
|
|
handle_mount_configure(msg);
|
|
|
|
break;
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED
|
2018-10-11 06:30:03 -03:00
|
|
|
case MAVLINK_MSG_ID_MOUNT_CONTROL:
|
|
|
|
handle_mount_control(msg);
|
|
|
|
break;
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
2019-03-16 04:06:02 -03:00
|
|
|
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
|
|
|
handle_global_position_int(msg);
|
|
|
|
break;
|
2023-03-24 00:50:40 -03:00
|
|
|
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
|
|
|
|
handle_gimbal_manager_set_attitude(msg);
|
|
|
|
break;
|
2023-05-22 02:51:11 -03:00
|
|
|
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW:
|
|
|
|
handle_command_gimbal_manager_set_pitchyaw(msg);
|
|
|
|
break;
|
2022-05-26 23:57:55 -03:00
|
|
|
case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
|
|
|
|
handle_gimbal_device_information(msg);
|
|
|
|
break;
|
2022-06-03 05:27:08 -03:00
|
|
|
case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
|
|
|
|
handle_gimbal_device_attitude_status(msg);
|
|
|
|
break;
|
2018-10-11 06:30:03 -03:00
|
|
|
default:
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
AP_HAL::panic("Unhandled mount case");
|
|
|
|
#endif
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-12-30 22:19:52 -04:00
|
|
|
// handle PARAM_VALUE
|
2019-04-30 07:22:48 -03:00
|
|
|
void AP_Mount::handle_param_value(const mavlink_message_t &msg)
|
2015-12-30 22:19:52 -04:00
|
|
|
{
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_backends[instance] != nullptr) {
|
2015-12-30 22:19:52 -04:00
|
|
|
_backends[instance]->handle_param_value(msg);
|
|
|
|
}
|
|
|
|
}
|
2015-01-29 05:01:55 -04:00
|
|
|
}
|
|
|
|
|
2018-07-24 20:30:23 -03:00
|
|
|
|
2022-05-26 23:57:55 -03:00
|
|
|
// handle GIMBAL_DEVICE_INFORMATION message
|
|
|
|
void AP_Mount::handle_gimbal_device_information(const mavlink_message_t &msg)
|
|
|
|
{
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
|
|
|
if (_backends[instance] != nullptr) {
|
|
|
|
_backends[instance]->handle_gimbal_device_information(msg);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-06-03 05:27:08 -03:00
|
|
|
// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
|
|
|
|
void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
|
|
|
|
{
|
|
|
|
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
|
|
|
|
if (_backends[instance] != nullptr) {
|
|
|
|
_backends[instance]->handle_gimbal_device_attitude_status(msg);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2022-06-16 04:30:01 -03:00
|
|
|
// perform any required parameter conversion
|
|
|
|
void AP_Mount::convert_params()
|
|
|
|
{
|
2022-09-01 21:27:09 -03:00
|
|
|
// exit immediately if MNT1_TYPE has already been configured
|
|
|
|
if (_params[0].type.configured()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// below conversions added Sep 2022 ahead of 4.3 release
|
|
|
|
|
|
|
|
// convert MNT_TYPE to MNT1_TYPE
|
|
|
|
int8_t mnt_type = 0;
|
|
|
|
IGNORE_RETURN(AP_Param::get_param_by_index(this, 19, AP_PARAM_INT8, &mnt_type));
|
|
|
|
if (mnt_type > 0) {
|
|
|
|
int8_t stab_roll = 0;
|
|
|
|
int8_t stab_pitch = 0;
|
|
|
|
IGNORE_RETURN(AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &stab_roll));
|
|
|
|
IGNORE_RETURN(AP_Param::get_param_by_index(this, 5, AP_PARAM_INT8, &stab_pitch));
|
|
|
|
if (mnt_type == 1 && stab_roll == 0 && stab_pitch == 0) {
|
|
|
|
// Servo type without stabilization is changed to BrushlessPWM
|
2023-06-07 03:27:04 -03:00
|
|
|
// conversion is still done even if HAL_MOUNT_SERVO_ENABLED is false
|
|
|
|
mnt_type = 7; // (int8_t)Type::BrushlessPWM;
|
2022-09-01 21:27:09 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
_params[0].type.set_and_save(mnt_type);
|
|
|
|
|
|
|
|
// convert MNT_JSTICK_SPD to MNT1_RC_RATE
|
|
|
|
int8_t jstick_spd = 0;
|
|
|
|
if (AP_Param::get_param_by_index(this, 16, AP_PARAM_INT8, &jstick_spd) && (jstick_spd > 0)) {
|
|
|
|
_params[0].rc_rate_max.set_and_save(jstick_spd * 0.3);
|
|
|
|
}
|
|
|
|
|
|
|
|
// find Mount's top level key
|
|
|
|
uint16_t k_param_mount_key;
|
|
|
|
if (!AP_Param::find_top_level_key_by_pointer(this, k_param_mount_key)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// table of mount parameters to be converted without scaling
|
|
|
|
static const AP_Param::ConversionInfo mnt_param_conversion_info[] {
|
|
|
|
{ k_param_mount_key, 0, AP_PARAM_INT8, "MNT1_DEFLT_MODE" },
|
|
|
|
{ k_param_mount_key, 1, AP_PARAM_VECTOR3F, "MNT1_RETRACT" },
|
|
|
|
{ k_param_mount_key, 2, AP_PARAM_VECTOR3F, "MNT1_NEUTRAL" },
|
|
|
|
{ k_param_mount_key, 17, AP_PARAM_FLOAT, "MNT1_LEAD_RLL" },
|
|
|
|
{ k_param_mount_key, 18, AP_PARAM_FLOAT, "MNT1_LEAD_PTCH" },
|
|
|
|
};
|
|
|
|
uint8_t table_size = ARRAY_SIZE(mnt_param_conversion_info);
|
|
|
|
for (uint8_t i=0; i<table_size; i++) {
|
|
|
|
AP_Param::convert_old_parameter(&mnt_param_conversion_info[i], 1.0f);
|
|
|
|
}
|
|
|
|
|
|
|
|
// mount parameters conversion from centi-degrees to degrees
|
|
|
|
static const AP_Param::ConversionInfo mnt_param_deg_conversion_info[] {
|
|
|
|
{ k_param_mount_key, 8, AP_PARAM_INT16, "MNT1_ROLL_MIN" },
|
|
|
|
{ k_param_mount_key, 9, AP_PARAM_INT16, "MNT1_ROLL_MAX" },
|
|
|
|
{ k_param_mount_key, 11, AP_PARAM_INT16, "MNT1_PITCH_MIN" },
|
|
|
|
{ k_param_mount_key, 12, AP_PARAM_INT16, "MNT1_PITCH_MAX" },
|
|
|
|
{ k_param_mount_key, 14, AP_PARAM_INT16, "MNT1_YAW_MIN" },
|
|
|
|
{ k_param_mount_key, 15, AP_PARAM_INT16, "MNT1_YAW_MAX" },
|
|
|
|
};
|
|
|
|
table_size = ARRAY_SIZE(mnt_param_deg_conversion_info);
|
|
|
|
for (uint8_t i=0; i<table_size; i++) {
|
|
|
|
AP_Param::convert_old_parameter(&mnt_param_deg_conversion_info[i], 0.01f);
|
|
|
|
}
|
|
|
|
|
|
|
|
// struct and array holding mapping between old param table index and new RCx_OPTION value
|
|
|
|
struct MountRCConversionTable {
|
|
|
|
uint8_t old_rcin_idx;
|
|
|
|
uint16_t new_rc_option;
|
|
|
|
};
|
|
|
|
const struct MountRCConversionTable mnt_rc_conversion_table[] = {
|
|
|
|
{7, 212}, // MTN_RC_IN_ROLL to RCx_OPTION = 212 (MOUNT1_ROLL)
|
|
|
|
{10, 213}, // MTN_RC_IN_TILT to RCx_OPTION = 213 (MOUNT1_PITCH)
|
|
|
|
{13, 214}, // MTN_RC_IN_PAN to RCx_OPTION = 214 (MOUNT1_YAW)
|
|
|
|
};
|
|
|
|
for (uint8_t i = 0; i < ARRAY_SIZE(mnt_rc_conversion_table); i++) {
|
|
|
|
int8_t mnt_rcin = 0;
|
|
|
|
if (AP_Param::get_param_by_index(this, mnt_rc_conversion_table[i].old_rcin_idx, AP_PARAM_INT8, &mnt_rcin) && (mnt_rcin > 0)) {
|
|
|
|
// get pointers to the appropriate RCx_OPTION parameter
|
|
|
|
char pname[17];
|
|
|
|
enum ap_var_type ptype;
|
|
|
|
snprintf(pname, sizeof(pname), "RC%u_OPTION", (unsigned)mnt_rcin);
|
|
|
|
AP_Int16 *rcx_option = (AP_Int16 *)AP_Param::find(pname, &ptype);
|
|
|
|
if ((rcx_option != nullptr) && !rcx_option->configured()) {
|
|
|
|
rcx_option->set_and_save(mnt_rc_conversion_table[i].new_rc_option);
|
|
|
|
}
|
2022-06-16 04:30:01 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-24 20:30:23 -03:00
|
|
|
// singleton instance
|
|
|
|
AP_Mount *AP_Mount::_singleton;
|
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
|
|
|
|
AP_Mount *mount()
|
|
|
|
{
|
|
|
|
return AP_Mount::get_singleton();
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
2020-07-24 14:01:21 -03:00
|
|
|
|
|
|
|
#endif /* HAL_MOUNT_ENABLED */
|