ardupilot/libraries/AP_Mount/AP_Mount_SToRM32.cpp

159 lines
5.4 KiB
C++

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_Mount_SToRM32.h"
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
extern const AP_HAL::HAL& hal;
AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance),
_initialised(false),
_sysid(0),
_compid(0),
_chan(MAVLINK_COMM_0),
_last_send(0)
{}
// update mount position - should be called periodically
void AP_Mount_SToRM32::update()
{
// exit immediately if not initialised
if (!_initialised) {
find_gimbal();
return;
}
// flag to trigger sending target angles to gimbal
bool resend_now = false;
// update based on 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 &target = _state._retract_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
const Vector3f &target = _state._neutral_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
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
update_targets_from_rc();
resend_now = true;
break;
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
resend_now = true;
}
break;
default:
// we do not know this mode so do nothing
break;
}
// resend target angles at least once per second
if (resend_now || ((hal.scheduler->millis() - _last_send) > AP_MOUNT_STORM32_RESEND_MS)) {
send_do_mount_control(ToDeg(_angle_ef_target_rad.y), ToDeg(_angle_ef_target_rad.x), ToDeg(_angle_ef_target_rad.z), MAV_MOUNT_MODE_MAVLINK_TARGETING);
}
}
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
bool AP_Mount_SToRM32::has_pan_control() const
{
// we do not have yaw control
return false;
}
// set_mode - sets mount's mode
void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// record the mode change
_state._mode = mode;
}
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_SToRM32::status_msg(mavlink_channel_t chan)
{
// return target angles as gimbal's actual attitude. To-Do: retrieve actual gimbal attitude and send these instead
mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);
}
// search for gimbal in GCS_MAVLink routing table
void AP_Mount_SToRM32::find_gimbal()
{
// return immediately if initialised
if (_initialised) {
return;
}
// return if search time has has passed
if (hal.scheduler->millis() > AP_MOUNT_STORM32_SEARCH_MS) {
return;
}
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_GIMBAL, _sysid, _compid, _chan)) {
_initialised = true;
}
}
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// check we have space for the message
if (comm_get_txspace(_chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_COMMAND_LONG_LEN) {
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send command_long command containing a do_mount_control command
mavlink_msg_command_long_send(_chan,
_sysid,
_compid,
MAV_CMD_DO_MOUNT_CONTROL,
0, // confirmation of zero means this is the first time this message has been sent
pitch_deg,
roll_deg,
yaw_deg,
0, 0, 0, // param4 ~ param6 unused
mount_mode);
// store time of send
_last_send = hal.scheduler->millis();
}