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

AP_Mount: add MNT log msg for mount's actual and target angles logging

This commit is contained in:
Asif Khan 2023-07-21 12:05:42 +05:30 committed by Andrew Tridgell
parent b5d1363c28
commit fd6db1ef45
21 changed files with 554 additions and 465 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Mount/AP_Mount.cpp
View File

@ -646,7 +646,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
return true;
}
// accessors for scripting backends
// get target rate in deg/sec. returns true on success
bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
{
auto *backend = get_instance(instance);
@ -656,6 +656,7 @@ bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_
return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame);
}
// get target angle in deg. returns true on success
bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
{
auto *backend = get_instance(instance);
@ -665,6 +666,7 @@ bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_
return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
}
// accessors for scripting backends and logging
bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc)
{
auto *backend = get_instance(instance);
@ -683,6 +685,26 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_
backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
}
// 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);
}
// point at system ID sysid
void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
{

12
libraries/AP_Mount/AP_Mount.h
View File

@ -199,12 +199,22 @@ public:
// any failure_msg returned will not include a prefix
bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
// accessors for scripting backends
// get target rate in deg/sec. returns true on success
bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
// get target angle in deg. returns true on success
bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
// accessors for scripting backends and logging
bool get_location_target(uint8_t instance, Location& target_loc);
void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg);
// write mount log packet for all backends
void write_log();
// write mount log packet for a single backend (called by camera library)
void write_log(uint8_t instance, uint64_t timestamp_us);
//
// camera controls for gimbals that include a camera
//

61
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -30,58 +30,59 @@ void AP_Mount_Alexmos::update()
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
}
// mavlink targets are stored while handling the incoming message
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
IGNORE_RETURN(get_angle_target_to_roi(_angle_rad));
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
IGNORE_RETURN(get_angle_target_to_home(_angle_rad));
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad));
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
default:
@ -89,8 +90,16 @@ void AP_Mount_Alexmos::update()
break;
}
// send latest targets to gimbal
control_axis(_angle_rad);
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::RATE:
update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
FALLTHROUGH;
case MountTargetType::ANGLE:
// send latest angle targets to gimbal
control_axis(mnt_target.angle_rad);
break;
}
}
// has_pan_control - returns true if this mount can control its pan (required for multicopters)

2
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -112,8 +112,6 @@ private:
// read_incoming - detect and read the header of the incoming message from the gimbal
void read_incoming();
MountTarget _angle_rad; // latest angle target
// structure for the Serial Protocol
// CMD_BOARD_INFO

172
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -2,6 +2,7 @@
#if HAL_MOUNT_ENABLED
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
extern const AP_HAL::HAL& hal;
@ -49,11 +50,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y
}
// set angle targets
mavt_target.target_type = MountTargetType::ANGLE;
mavt_target.angle_rad.roll = radians(roll_deg);
mavt_target.angle_rad.pitch = radians(pitch_deg);
mavt_target.angle_rad.yaw = radians(yaw_deg);
mavt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame;
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.roll = radians(roll_deg);
mnt_target.angle_rad.pitch = radians(pitch_deg);
mnt_target.angle_rad.yaw = radians(yaw_deg);
mnt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame;
// set the mode to mavlink targeting
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
@ -64,11 +65,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y
void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame)
{
// set rate targets
mavt_target.target_type = MountTargetType::RATE;
mavt_target.rate_rads.roll = radians(roll_degs);
mavt_target.rate_rads.pitch = radians(pitch_degs);
mavt_target.rate_rads.yaw = radians(yaw_degs);
mavt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame;
mnt_target.target_type = MountTargetType::RATE;
mnt_target.rate_rads.roll = radians(roll_degs);
mnt_target.rate_rads.pitch = radians(pitch_degs);
mnt_target.rate_rads.yaw = radians(yaw_degs);
mnt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame;
// set the mode to mavlink targeting
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
@ -356,6 +357,49 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli
return true;
}
// write mount log packet
void AP_Mount_Backend::write_log(uint64_t timestamp_us)
{
// return immediately if no yaw estimate
float ahrs_yaw = AP::ahrs().yaw;
if (isnan(ahrs_yaw)) {
return;
}
const auto nanf = AP::logger().quiet_nanf();
// get_attitude_quaternion and convert to Euler angles
float roll = nanf;
float pitch = nanf;
float yaw_bf = nanf;
float yaw_ef = nanf;
if (_frontend.get_attitude_euler(_instance, roll, pitch, yaw_bf)) {
yaw_ef = wrap_180(yaw_bf + degrees(ahrs_yaw));
}
// get mount's target (desired) angles and convert yaw to earth frame
float target_roll = nanf;
float target_pitch = nanf;
float target_yaw = nanf;
bool target_yaw_is_ef = false;
IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef));
const struct log_Mount pkt {
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(LOG_MOUNT_MSG)),
time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(),
instance : _instance,
desired_roll : target_roll,
actual_roll : roll,
desired_pitch : target_pitch,
actual_pitch : pitch,
desired_yaw_bf: target_yaw_is_ef ? nanf : target_yaw,
actual_yaw_bf : yaw_bf,
desired_yaw_ef: target_yaw_is_ef ? target_yaw : nanf,
actual_yaw_ef : yaw_ef,
};
AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
}
// get pilot input (in the range -1 to +1) received through RC
void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const
{
@ -379,61 +423,44 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_
}
}
// get rate targets (in rad/s) from pilot RC
// returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets)
bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const
// get angle or rate targets from pilot RC
// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s
void AP_Mount_Backend::get_rc_target(MountTargetType& target_type, MountTarget& target_rpy) const
{
// exit immediately if RC is not providing rate targets
if (_params.rc_rate_max <= 0) {
return false;
}
// get RC input from pilot
float roll_in, pitch_in, yaw_in;
get_rc_input(roll_in, pitch_in, yaw_in);
// calculate rates
const float rc_rate_max_rads = radians(_params.rc_rate_max.get());
rate_rads.roll = roll_in * rc_rate_max_rads;
rate_rads.pitch = pitch_in * rc_rate_max_rads;
rate_rads.yaw = yaw_in * rc_rate_max_rads;
// yaw frame
rate_rads.yaw_is_ef = _yaw_lock;
target_rpy.yaw_is_ef = _yaw_lock;
return true;
}
// if RC_RATE is zero, targets are angle
if (_params.rc_rate_max <= 0) {
target_type = MountTargetType::ANGLE;
// get angle targets (in radians) from pilot RC
// returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets)
bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const
{
// exit immediately if RC is not providing angle targets
if (_params.rc_rate_max > 0) {
return false;
// roll angle
target_rpy.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min));
// pitch angle
target_rpy.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min));
// yaw angle
if (target_rpy.yaw_is_ef) {
// if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg
target_rpy.yaw = yaw_in * M_PI;
} else {
// yaw target in body frame so apply body frame limits
target_rpy.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min));
}
return;
}
// get RC input from pilot
float roll_in, pitch_in, yaw_in;
get_rc_input(roll_in, pitch_in, yaw_in);
// roll angle
angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min));
// pitch angle
angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min));
// yaw angle
angle_rad.yaw_is_ef = _yaw_lock;
if (angle_rad.yaw_is_ef) {
// if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg
angle_rad.yaw = yaw_in * M_PI;
} else {
// yaw target in body frame so apply body frame limits
angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min));
}
return true;
// calculate rate targets
target_type = MountTargetType::RATE;
const float rc_rate_max_rads = radians(_params.rc_rate_max.get());
target_rpy.roll = roll_in * rc_rate_max_rads;
target_rpy.pitch = pitch_in * rc_rate_max_rads;
target_rpy.yaw = yaw_in * rc_rate_max_rads;
}
// get angle targets (in radians) to a Location
@ -507,6 +534,15 @@ float AP_Mount_Backend::MountTarget::get_ef_yaw() const
return wrap_PI(yaw + AP::ahrs().yaw);
}
// sets roll, pitch, yaw and yaw_is_ef
void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in)
{
roll = rpy.x;
pitch = rpy.y;
yaw = rpy.z;
yaw_is_ef = yaw_is_ef_in;
}
// update angle targets using a given rate target
// the resulting angle_rad yaw frame will match the rate_rad yaw frame
// assumes a 50hz update rate
@ -572,6 +608,32 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const
return get_angle_target_to_location(_target_sysid_location, angle_rad);
}
// get target rate in deg/sec. returns true on success
bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
{
if (mnt_target.target_type == MountTargetType::RATE) {
roll_degs = degrees(mnt_target.rate_rads.roll);
pitch_degs = degrees(mnt_target.rate_rads.pitch);
yaw_degs = degrees(mnt_target.rate_rads.yaw);
yaw_is_earth_frame = mnt_target.rate_rads.yaw_is_ef;
return true;
}
return false;
}
// get target angle in deg. returns true on success
bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
{
if (mnt_target.target_type == MountTargetType::ANGLE) {
roll_deg = degrees(mnt_target.angle_rad.roll);
pitch_deg = degrees(mnt_target.angle_rad.pitch);
yaw_deg = degrees(mnt_target.angle_rad.yaw);
yaw_is_earth_frame = mnt_target.angle_rad.yaw_is_ef;
return true;
}
return false;
}
// sent warning to GCS. Warnings are throttled to at most once every 30 seconds
void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str)
{

33
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -28,7 +28,7 @@
#include <AP_Common/Location.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Camera/AP_Camera_shareddefs.h>
#include "AP_Mount_Params.h"
#include "AP_Mount.h"
class AP_Mount_Backend
{
@ -133,12 +133,19 @@ public:
// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
// get target rate in deg/sec. returns true on success
bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
// get target angle in deg. returns true on success
bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
// accessors for scripting backends
virtual bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { return false; }
virtual bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { return false; }
virtual bool get_location_target(Location &target_loc) { return false; }
virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {};
// write mount log packet
void write_log(uint64_t timestamp_us);
//
// camera controls for gimbals that include a camera
//
@ -175,8 +182,9 @@ protected:
RATE,
};
// structure for a single angle or rate target
struct MountTarget {
// class for a single angle or rate target
class MountTarget {
public:
float roll;
float pitch;
float yaw;
@ -187,6 +195,9 @@ protected:
// return earth-frame yaw angle from a mount target (in radians)
float get_ef_yaw() const;
// set roll, pitch, yaw and yaw_is_ef from Vector3f
void set(const Vector3f& rpy, bool yaw_is_ef_in);
};
// returns true if user has configured a valid yaw angle range
@ -199,13 +210,9 @@ protected:
// 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;
// get rate targets (in rad/s) from pilot RC
// returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets)
bool get_rc_rate_target(MountTarget& rate_rads) const WARN_IF_UNUSED;
// get angle targets (in radians) from pilot RC
// returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets)
bool get_rc_angle_target(MountTarget& angle_rad) const WARN_IF_UNUSED;
// get angle or rate targets from pilot RC
// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s
void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const;
// get angle targets (in radians) to a Location
// returns true on success, false on failure
@ -246,7 +253,7 @@ protected:
MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate)
MountTarget angle_rad; // angle target in radians
MountTarget rate_rads; // rate target in rad/s
} mavt_target;
} mnt_target;
Location _roi_target; // roi target location
bool _roi_target_set; // true if the roi target has been set

73
libraries/AP_Mount/AP_Mount_Gremsy.cpp
View File

@ -26,70 +26,75 @@ void AP_Mount_Gremsy::update()
// move mount to a "retracted" position. We disable motors
case MAV_MOUNT_MODE_RETRACT:
// handled below
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(Vector3f{0,0,0}, false);
send_gimbal_device_retract();
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
send_gimbal_device_set_attitude(ToRad(angle_bf_target.x), ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
}
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// use angle or rate targets provided by a mavlink message or mission command
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
send_gimbal_device_set_attitude(mavt_target.angle_rad.roll, mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_gimbal_device_set_rate(mavt_target.rate_rads.roll, mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef);
break;
}
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
// mavlink targets are stored while handling the incoming message set_angle_target() or set_rate_target()
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's rc inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
send_gimbal_device_set_rate(rc_target.roll, rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
} else if (get_rc_angle_target(rc_target)) {
send_gimbal_device_set_attitude(rc_target.roll, rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: {
MountTarget angle_target_rad {};
if (get_angle_target_to_roi(angle_target_rad)) {
send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
// point mount to home
case MAV_MOUNT_MODE_HOME_LOCATION: {
MountTarget angle_target_rad {};
if (get_angle_target_to_home(angle_target_rad)) {
send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET: {
MountTarget angle_target_rad {};
if (get_angle_target_to_sysid(angle_target_rad)) {
send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
default:
// unknown mode so do nothing
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_gimbal_device_set_attitude(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_gimbal_device_set_rate(mnt_target.rate_rads.roll, mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
break;
}
}
// return true if healthy

53
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -26,32 +26,24 @@ void AP_Mount_SToRM32::update()
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
// mnt_target should have already been populated by set_angle_target() or set_rate_target(). Update target angle from rate if necessary
if (mnt_target.target_type == MountTargetType::RATE) {
update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
}
resend_now = true;
break;
@ -59,31 +51,40 @@ void AP_Mount_SToRM32::update()
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
resend_now = true;
break;
}
// point mount to a GPS location
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(_angle_rad)) {
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(_angle_rad)) {
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(_angle_rad)) {
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
@ -95,14 +96,14 @@ void AP_Mount_SToRM32::update()
// resend target angles at least once per second
if (resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_RESEND_MS)) {
send_do_mount_control(_angle_rad);
send_do_mount_control(mnt_target.angle_rad);
}
}
// get attitude as a quaternion. returns true on success
bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat)
{
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw());
att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw());
return true;
}

1
libraries/AP_Mount/AP_Mount_SToRM32.h
View File

@ -42,6 +42,5 @@ private:
uint8_t _compid; // component id of gimbal
mavlink_channel_t _chan = MAVLINK_COMM_0; // mavlink channel used to communicate with gimbal
uint32_t _last_send; // system time of last do_mount_control sent to gimbal
MountTarget _angle_rad; // latest angle target
};
#endif // HAL_MOUNT_STORM32MAVLINK_ENABLED

49
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -37,32 +37,24 @@ void AP_Mount_SToRM32_serial::update()
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _params.retract_angles.get();
_angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
// mnt_target should have already been filled in by set_angle_target() or set_rate_target()
if (mnt_target.target_type == MountTargetType::RATE) {
update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
}
resend_now = true;
break;
@ -70,11 +62,15 @@ void AP_Mount_SToRM32_serial::update()
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
resend_now = true;
break;
@ -82,19 +78,24 @@ void AP_Mount_SToRM32_serial::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(_angle_rad)) {
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(_angle_rad)) {
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(_angle_rad)) {
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
resend_now = true;
}
break;
@ -112,7 +113,7 @@ void AP_Mount_SToRM32_serial::update()
}
if (can_send(resend_now)) {
if (resend_now) {
send_target_angles(_angle_rad);
send_target_angles(mnt_target.angle_rad);
get_angles();
_reply_type = ReplyType_ACK;
_reply_counter = 0;

1
libraries/AP_Mount/AP_Mount_SToRM32_serial.h
View File

@ -130,7 +130,6 @@ private:
AP_HAL::UARTDriver *_port;
bool _initialised; // true once the driver has been initialised
MountTarget _angle_rad; // latest angle target
uint32_t _last_send; // system time of last do_mount_control sent to gimbal
uint8_t _reply_length;

126
libraries/AP_Mount/AP_Mount_Scripting.cpp
View File

@ -20,14 +20,8 @@ void AP_Mount_Scripting::update()
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &angle_bf_target = _params.retract_angles.get();
target_angle_rad.roll = ToRad(angle_bf_target.x);
target_angle_rad.pitch = ToRad(angle_bf_target.y);
target_angle_rad.yaw = ToRad(angle_bf_target.z);
target_angle_rad.yaw_is_ef = false;
target_angle_rad_valid = true;
// mark other targets as invalid
target_rate_rads_valid = false;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
target_loc_valid = false;
break;
}
@ -35,92 +29,55 @@ void AP_Mount_Scripting::update()
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
target_angle_rad.roll = ToRad(angle_bf_target.x);
target_angle_rad.pitch = ToRad(angle_bf_target.y);
target_angle_rad.yaw = ToRad(angle_bf_target.z);
target_angle_rad.yaw_is_ef = false;
target_angle_rad_valid = true;
// mark other targets as invalid
target_rate_rads_valid = false;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
mnt_target.target_type = MountTargetType::ANGLE;
target_loc_valid = false;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
target_angle_rad = mavt_target.angle_rad;
target_angle_rad_valid = true;
target_rate_rads_valid = false;
target_loc_valid = false;
break;
case MountTargetType::RATE:
target_rate_rads = mavt_target.rate_rads;
target_rate_rads_valid = true;
target_angle_rad_valid = false;
target_loc_valid = false;
break;
}
// mavlink targets should have been already stored while handling the message
target_loc_valid = false;
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's rc inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
target_rate_rads = rc_target;
target_rate_rads_valid = true;
target_angle_rad_valid = false;
target_loc_valid = false;
} else if (get_rc_angle_target(rc_target)) {
target_angle_rad = rc_target;
target_angle_rad_valid = true;
target_rate_rads_valid = false;
target_loc_valid = false;
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
target_loc_valid = false;
break;
}
// point mount towards a GPS point
case MAV_MOUNT_MODE_GPS_POINT: {
target_loc_valid = _roi_target_set;
if (target_loc_valid) {
target_loc = _roi_target;
target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad);
} else {
target_angle_rad_valid = false;
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
target_rate_rads_valid = false;
break;
}
// point mount towards home
case MAV_MOUNT_MODE_HOME_LOCATION: {
target_loc_valid = AP::ahrs().home_is_set();
if (target_loc_valid) {
target_loc = AP::ahrs().get_home();
target_angle_rad_valid = get_angle_target_to_home(target_angle_rad);
} else {
target_angle_rad_valid = false;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
target_rate_rads_valid = false;
break;
}
// point mount towards another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET: {
target_loc_valid = _target_sysid_location_set;
if (target_loc_valid) {
target_loc = _target_sysid_location;
target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad);
} else {
target_angle_rad_valid = false;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
target_rate_rads_valid = false;
break;
}
default:
// we do not know this mode so raise internal error
@ -136,31 +93,6 @@ bool AP_Mount_Scripting::healthy() const
return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS);
}
// accessors for scripting backends
bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
{
if (target_rate_rads_valid) {
roll_degs = degrees(target_rate_rads.roll);
pitch_degs = degrees(target_rate_rads.pitch);
yaw_degs = degrees(target_rate_rads.yaw);
yaw_is_earth_frame = target_rate_rads.yaw_is_ef;
return true;
}
return false;
}
bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
{
if (target_angle_rad_valid) {
roll_deg = degrees(target_angle_rad.roll);
pitch_deg = degrees(target_angle_rad.pitch);
yaw_deg = degrees(target_angle_rad.yaw);
yaw_is_earth_frame = target_angle_rad.yaw_is_ef;
return true;
}
return false;
}
// return target location if available
// returns true if a target location is available and fills in target_loc argument
bool AP_Mount_Scripting::get_location_target(Location &_target_loc)

9
libraries/AP_Mount/AP_Mount_Scripting.h
View File

@ -32,8 +32,6 @@ public:
bool has_pan_control() const override { return yaw_range_valid(); };
// accessors for scripting backends
bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override;
bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override;
bool get_location_target(Location& _target_loc) override;
void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override;
@ -48,11 +46,8 @@ private:
uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting
Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw)
MountTarget target_rate_rads; // rate target in rad/s
bool target_rate_rads_valid; // true if _target_rate_degs holds a valid rate target
MountTarget target_angle_rad; // angle target in radians
bool target_angle_rad_valid; // true if _target_rate_degs holds a valid rate target
bool target_rate_rads_valid; // true if mnt_target holds a valid rate target
bool target_angle_rad_valid; // true if mnt_target holds a valid angle target
Location target_loc; // target location
bool target_loc_valid; // true if target_loc holds a valid target location

92
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -27,89 +27,87 @@ void AP_Mount_Servo::init()
// update mount position - should be called periodically
void AP_Mount_Servo::update()
{
switch (get_mode()) {
auto mount_mode = get_mode();
switch (mount_mode) {
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT: {
_angle_bf_output_rad = _params.retract_angles.get() * DEG_TO_RAD;
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = _angle_bf_output_rad.x;
_angle_rad.pitch = _angle_bf_output_rad.y;
_angle_rad.yaw = _angle_bf_output_rad.z;
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(_angle_bf_output_rad, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
_angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD;
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTING
_angle_rad.roll = _angle_bf_output_rad.x;
_angle_rad.pitch = _angle_bf_output_rad.y;
_angle_rad.yaw = _angle_bf_output_rad.z;
_angle_rad.yaw_is_ef = false;
mnt_target.angle_rad.set(_angle_bf_output_rad, false);
mnt_target.target_type = MountTargetType::ANGLE;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
}
// update _angle_bf_output_rad based on angle target
update_angle_outputs(_angle_rad);
// mavlink targets are stored while handling the incoming message and considered valid
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
}
// update _angle_bf_output_rad based on angle target
update_angle_outputs(_angle_rad);
break;
}
// point mount to a GPS location
case MAV_MOUNT_MODE_GPS_POINT: {
if (get_angle_target_to_roi(_angle_rad)) {
update_angle_outputs(_angle_rad);
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
case MAV_MOUNT_MODE_HOME_LOCATION: {
if (get_angle_target_to_home(_angle_rad)) {
update_angle_outputs(_angle_rad);
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET: {
if (get_angle_target_to_sysid(_angle_rad)) {
update_angle_outputs(_angle_rad);
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
default:
//do nothing
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::RATE:
update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
FALLTHROUGH;
case MountTargetType::ANGLE:
// update _angle_bf_output_rad based on angle target
if ((mount_mode != MAV_MOUNT_MODE_RETRACT) & (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) {
update_angle_outputs(mnt_target.angle_rad);
}
break;
}
// move mount to a "retracted position" into the fuselage with a fourth servo
const bool mount_open = (get_mode() == MAV_MOUNT_MODE_RETRACT) ? 0 : 1;
const bool mount_open = (mount_mode == MAV_MOUNT_MODE_RETRACT) ? 0 : 1;
move_servo(_open_idx, mount_open, 0, 1);
// write the results to the servos

1
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -56,7 +56,6 @@ private:
SRV_Channel::Aux_servo_function_t _pan_idx; // SRV_Channel mount pan function index
SRV_Channel::Aux_servo_function_t _open_idx; // SRV_Channel mount open function index
MountTarget _angle_rad; // angle target
Vector3f _angle_bf_output_rad; // final body frame output angle in radians
};
#endif // HAL_MOUNT_SERVO_ENABLED

74
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -68,76 +68,78 @@ void AP_Mount_Siyi::update()
// run zoom control
update_zoom_control();
// update based on mount mode
// Get the target angles or rates first depending on the current mount mode
switch (get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &angle_bf_target = _params.retract_angles.get();
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
// mavlink targets are stored while handling the incoming message
break;
}
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef);
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef);
mnt_target.rate_rads = rc_target;
break;
}
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's rc inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
} else if (get_rc_angle_target(rc_target)) {
send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: {
MountTarget angle_target_rad {};
if (get_angle_target_to_roi(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_HOME_LOCATION: {
MountTarget angle_target_rad {};
if (get_angle_target_to_home(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET:{
MountTarget angle_target_rad {};
if (get_angle_target_to_sysid(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
default:
// we do not know this mode so raise internal error
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
break;
}
}
// return true if healthy

59
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -36,61 +36,66 @@ void AP_Mount_SoloGimbal::update()
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT:
_gimbal.set_lockedToBody(true);
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTINg
_angle_rad = {0, 0, 0, false};
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(Vector3f{0,0,0}, false);
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
_gimbal.set_lockedToBody(false);
const Vector3f &target = _params.neutral_angles.get();
_angle_rad.roll = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(target.z);
_angle_rad.yaw_is_ef = false;
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// targets are stored while handling the incoming mavlink message
_gimbal.set_lockedToBody(false);
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
if (mnt_target.target_type == MountTargetType::RATE) {
update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
}
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
_gimbal.set_lockedToBody(false);
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
_gimbal.set_lockedToBody(false);
IGNORE_RETURN(get_angle_target_to_roi(_angle_rad));
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
_gimbal.set_lockedToBody(false);
}
break;
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
_gimbal.set_lockedToBody(false);
IGNORE_RETURN(get_angle_target_to_home(_angle_rad));
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
_gimbal.set_lockedToBody(false);
}
break;
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
_gimbal.set_lockedToBody(false);
IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad));
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
_gimbal.set_lockedToBody(false);
}
break;
default:
@ -105,7 +110,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
if (!_gimbal.aligned()) {
return false;
}
att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw());
att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw());
return true;
}
@ -114,7 +119,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
*/
void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
{
_gimbal.update_target(Vector3f{_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw()});
_gimbal.update_target(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()});
_gimbal.receive_feedback(chan,msg);
AP_Logger *logger = AP_Logger::get_singleton();

1
libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -54,7 +54,6 @@ private:
void Log_Write_Gimbal(SoloGimbal &gimbal);
bool _params_saved;
MountTarget _angle_rad; // angle target
SoloGimbal _gimbal;
};

67
libraries/AP_Mount/AP_Mount_Viewpro.cpp
View File

@ -76,71 +76,76 @@ void AP_Mount_Viewpro::update()
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &angle_bf_target = _params.retract_angles.get();
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef);
break;
}
// mavlink targets are stored while handling the incoming message
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's rc inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
} else if (get_rc_angle_target(rc_target)) {
send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
mnt_target.rate_rads = rc_target;
break;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: {
MountTarget angle_target_rad {};
if (get_angle_target_to_roi(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_HOME_LOCATION: {
MountTarget angle_target_rad {};
if (get_angle_target_to_home(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET:{
MountTarget angle_target_rad {};
if (get_angle_target_to_sysid(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
default:
// we do not know this mode so raise internal error
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
break;
}
}
// return true if healthy

71
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -54,71 +54,74 @@ void AP_Mount_Xacti::update()
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &angle_bf_target = _params.retract_angles.get();
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &angle_bf_target = _params.neutral_angles.get();
send_target_rates(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
mnt_target.target_type = MountTargetType::ANGLE;
mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false);
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mavt_target.target_type) {
case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
// mavlink targets are set while handling the incoming message
break;
}
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target;
get_rc_target(mnt_target.target_type, rc_target);
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef);
mnt_target.angle_rad = rc_target;
break;
case MountTargetType::RATE:
send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef);
mnt_target.rate_rads = rc_target;
break;
}
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's rc inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
} else if (get_rc_angle_target(rc_target)) {
send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT: {
MountTarget angle_target_rad {};
if (get_angle_target_to_roi(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
case MAV_MOUNT_MODE_GPS_POINT:
if (get_angle_target_to_roi(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_HOME_LOCATION: {
MountTarget angle_target_rad {};
if (get_angle_target_to_home(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to Home location
case MAV_MOUNT_MODE_HOME_LOCATION:
if (get_angle_target_to_home(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET:{
MountTarget angle_target_rad {};
if (get_angle_target_to_sysid(angle_target_rad)) {
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
// point mount to another vehicle
case MAV_MOUNT_MODE_SYSID_TARGET:
if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
mnt_target.target_type = MountTargetType::ANGLE;
}
break;
}
default:
// we do not know this mode so raise internal error
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
}
// send target angles or rates depending on the target type
switch (mnt_target.target_type) {
case MountTargetType::ANGLE:
send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef);
break;
case MountTargetType::RATE:
send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef);
break;
}
}
// return true if healthy

38
libraries/AP_Mount/LogStructure.h Normal file
View File

@ -0,0 +1,38 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_MOUNT \
LOG_MOUNT_MSG
// @LoggerMessage: MNT
// @Description: Mount's actual and Target/Desired RPY information
// @Field: TimeUS: Time since system startup
// @Field: I: Instance number
// @Field: DesRoll: mount's desired roll
// @Field: Roll: mount's actual roll
// @Field: DesPitch: mount's desired pitch
// @Field: Pitch: mount's actual pitch
// @Field: DesYawB: mount's desired yaw in body frame
// @Field: YawB: mount's actual yaw in body frame
// @Field: DesYawE: mount's desired yaw in earth frame
// @Field: YawE: mount's actual yaw in earth frame
struct PACKED log_Mount {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t instance;
float desired_roll;
float actual_roll;
float desired_pitch;
float actual_pitch;
float desired_yaw_bf;
float actual_yaw_bf;
float desired_yaw_ef;
float actual_yaw_ef;
};
#define LOG_STRUCTURE_FROM_MOUNT \
{ LOG_MOUNT_MSG, sizeof(log_Mount), \
"MNT", "QBffffffff","TimeUS,I,DesRoll,Roll,DesPitch,Pitch,DesYawB,YawB,DesYawE,YawE", "s#dddddddd", "F---------" },