Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: support sysid targetting

Browse Source
This commit is contained in:
Peter Barker 2019-03-16 18:06:02 +11:00 committed by Randy Mackay
parent fce6aad00f
commit c649fd1468
9 changed files with 129 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Mount/AP_Mount.cpp
View File

@ -7,6 +7,7 @@
#include "AP_Mount_Alexmos.h"
#include "AP_Mount_SToRM32.h"
#include "AP_Mount_SToRM32_serial.h"
#include <AP_Math/location.h>
const AP_Param::GroupInfo AP_Mount::var_info[] = {
// @Param: _DEFLT_MODE
@ -596,6 +597,33 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
}
}
/// 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++) {
if (_backends[instance] == nullptr) {
continue;
}
struct mount_state &_state = state[instance];
if (_state._target_sysid != msg.sysid) {
continue;
}
_state._target_sysid_location.lat = packet.lat;
_state._target_sysid_location.lng = packet.lon;
// global_position_int.alt is *UP*, so is location.
_state._target_sysid_location.set_alt_cm(packet.alt*0.1,
Location::AltFrame::ABSOLUTE);
_state._target_sysid_location_set = true;
}
}
/// Change the configuration of the mount
void AP_Mount::handle_mount_configure(const mavlink_message_t &msg)
{
@ -635,6 +663,15 @@ void AP_Mount::send_mount_status(mavlink_channel_t chan)
}
}
// point at system ID sysid
void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
{
// call instance's set_roi_cmd
if (instance < AP_MOUNT_MAX_INSTANCES && _backends[instance] != nullptr) {
_backends[instance]->set_target_sysid(sysid);
}
}
// set_roi_target - sets target location that mount should attempt to point towards
void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_loc)
{
@ -666,6 +703,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m
case MAVLINK_MSG_ID_MOUNT_CONTROL:
handle_mount_control(msg);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
handle_global_position_int(msg);
break;
default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Unhandled mount case");

11
libraries/AP_Mount/AP_Mount.h
View File

@ -111,6 +111,10 @@ public:
void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); }
void set_roi_target(uint8_t instance, const struct Location &target_loc);
// point at system ID sysid
void set_target_sysid(uint8_t instance, const uint8_t sysid);
void set_target_sysid(const uint8_t sysid) { set_target_sysid(_primary, sysid); }
// mavlink message handling:
MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
void handle_param_value(const mavlink_message_t &msg);
@ -168,6 +172,11 @@ protected:
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)
struct Location _roi_target; // roi target location
uint32_t _roi_target_set_ms;
uint8_t _target_sysid; // sysid to track
Location _target_sysid_location; // sysid target location
bool _target_sysid_location_set;
} state[AP_MOUNT_MAX_INSTANCES];
private:
@ -178,7 +187,7 @@ private:
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
void handle_global_position_int(const mavlink_message_t &msg);
};
namespace AP {

8
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -57,6 +57,14 @@ void AP_Mount_Alexmos::update()
}
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad,
true,
false)) {
control_axis(_angle_ef_target_rad, false);
}
break;
default:
// we do not know this mode so do nothing
break;

31
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -26,6 +26,15 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
_frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
}
// set_sys_target - sets system that mount should attempt to point towards
void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
{
_state._target_sysid = sysid;
// set the mode to sysid tracking mode
_frontend.set_mode(_instance, MAV_MOUNT_MODE_SYSID_TARGET);
}
// process MOUNT_CONFIGURE messages received from GCS. deprecated.
void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet)
{
@ -135,7 +144,7 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min,
bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan)
bool relative_pan) const
{
if (_state._roi_target_set_ms == 0) {
return false;
@ -143,8 +152,26 @@ bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
}
bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan) const
{
if (!_state._target_sysid_location_set) {
return false;
}
if (!_state._target_sysid) {
return false;
}
return calc_angle_to_location(_state._target_sysid_location,
angles_to_target_rad,
calc_tilt,
calc_pan,
relative_pan);
}
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const
{
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {

16
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -57,6 +57,9 @@ public:
// set_roi_target - sets target location that mount should attempt to point towards
void set_roi_target(const struct Location &target_loc);
// set_sys_target - sets system that mount should attempt to point towards
void set_target_sysid(uint8_t sysid);
// control - control the mount
virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode);
@ -92,14 +95,23 @@ protected:
Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan = true) WARN_IF_UNUSED;
bool relative_pan = true) const WARN_IF_UNUSED;
// calc_angle_to_roi_target - calculates the earth-frame roll, tilt
// and pan angles (and radians) to point at the ROI-target (as set
// by various mavlink messages)
bool calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan = true) WARN_IF_UNUSED;
bool relative_pan = true) const WARN_IF_UNUSED;
// calc_angle_to_sysid_target - calculates the earth-frame roll, tilt
// and pan angles (and radians) to point at the sysid-target (as set
// by various mavlink messages)
bool calc_angle_to_sysid_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan = true) const WARN_IF_UNUSED;
// get the mount mode from frontend
MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }

6
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -64,6 +64,12 @@ void AP_Mount_SToRM32::update()
}
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) {
resend_now = true;
}
break;
default:
// we do not know this mode so do nothing
break;

8
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -80,6 +80,14 @@ void AP_Mount_SToRM32_serial::update()
}
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad,
true,
true)) {
resend_now = true;
}
break;
default:
// we do not know this mode so do nothing
break;

9
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -76,6 +76,15 @@ void AP_Mount_Servo::update()
break;
}
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad,
_flags.tilt_control,
_flags.pan_control,
false)) {
stabilize();
}
break;
default:
//do nothing
break;

5
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -74,6 +74,11 @@ void AP_Mount_SoloGimbal::update()
}
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) {
}
break;
default:
// we do not know this mode so do nothing
break;