You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
3.3 KiB
111 lines
3.3 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
#include "AP_Mount_MAVLink.h" |
|
#if AP_AHRS_NAVEKF_AVAILABLE |
|
#include <GCS_MAVLink/GCS_MAVLink.h> |
|
#include <stdio.h> |
|
#include "AP_Gimbal.h" |
|
|
|
#if MOUNT_DEBUG |
|
#include <stdio.h> |
|
#endif |
|
|
|
AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : |
|
AP_Mount_Backend(frontend, state, instance), |
|
_initialised(false), |
|
_gimbal(frontend._ahrs, MAV_COMP_ID_GIMBAL, _state._gimbalParams) |
|
{} |
|
|
|
// init - performs any required initialisation for this instance |
|
void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager) |
|
{ |
|
_initialised = true; |
|
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); |
|
} |
|
|
|
// update mount position - should be called periodically |
|
void AP_Mount_MAVLink::update() |
|
{ |
|
// exit immediately if not initialised |
|
if (!_initialised) { |
|
return; |
|
} |
|
|
|
// update based on mount mode |
|
switch(get_mode()) { |
|
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism |
|
case MAV_MOUNT_MODE_RETRACT: |
|
break; |
|
|
|
// move mount to a neutral position, typically pointing forward |
|
case MAV_MOUNT_MODE_NEUTRAL: |
|
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(); |
|
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); |
|
} |
|
break; |
|
|
|
default: |
|
// we do not know this mode so do nothing |
|
break; |
|
} |
|
} |
|
|
|
// has_pan_control - returns true if this mount can control it's pan (required for multicopters) |
|
bool AP_Mount_MAVLink::has_pan_control() const |
|
{ |
|
// we do not have yaw control |
|
return false; |
|
} |
|
|
|
// set_mode - sets mount's mode |
|
void AP_Mount_MAVLink::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_MAVLink::status_msg(mavlink_channel_t chan) |
|
{ |
|
Vector3f est = _gimbal.getGimbalEstimateEF(); |
|
mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100); |
|
} |
|
|
|
/* |
|
handle a GIMBAL_REPORT message |
|
*/ |
|
void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) |
|
{ |
|
_gimbal.update_target(_angle_ef_target_rad); |
|
_gimbal.receive_feedback(chan,msg); |
|
} |
|
|
|
/* |
|
send a GIMBAL_REPORT message to the GCS |
|
*/ |
|
void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan) |
|
{ |
|
_gimbal.send_report(chan); |
|
} |
|
|
|
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
|
|