Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: fixed mount MAVLink backend to match SITL sim behaviour

Browse Source 
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2015-02-01 20:34:12 +11:00
parent 75b1330843
commit cf76dcfbf3
2 changed files with 6 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -10,21 +10,17 @@
#include <stdio.h>
#endif
AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) :
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),
_chan(MAVLINK_COMM_0),
_last_mode(MAV_MOUNT_MODE_RETRACT),
_ekf(frontend._ahrs)
{}
// init - performs any required initialisation for this instance
void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager)
{
// use mavlink channel associated with MAVLink2 protocol
if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink2, _chan)) {
_initialised = true;
}
_initialised = true;
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
}
// update mount position - should be called periodically
@ -39,12 +35,10 @@ void AP_Mount_MAVLink::update()
switch(get_mode()) {
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT:
send_angle_target(_state._retract_angles.get(), true);
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
send_angle_target(_state._neutral_angles.get(), true);
break;
// point to the angles given by a mavlink message
@ -56,14 +50,12 @@ void AP_Mount_MAVLink::update()
case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's rc inputs
update_targets_from_rc();
send_angle_target(_angle_ef_target_rad, false);
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, false);
send_angle_target(_angle_ef_target_rad, false);
}
break;
@ -88,33 +80,8 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
return;
}
// map requested mode to mode that mount can actually support
enum MAV_MOUNT_MODE mode_to_send = mode;
switch (mode) {
case MAV_MOUNT_MODE_RETRACT:
case MAV_MOUNT_MODE_NEUTRAL:
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
case MAV_MOUNT_MODE_RC_TARGETING:
case MAV_MOUNT_MODE_GPS_POINT:
mode_to_send = MAV_MOUNT_MODE_MAVLINK_TARGETING;
break;
default:
// unknown mode so just send it and hopefully gimbal supports it
break;
}
// prepare and send command_long message with DO_SET_MODE command
mavlink_msg_command_long_send(
_chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
MAV_CMD_DO_SET_MODE, // command number
0, // confirmation: 0=first confirmation of this command
mode_to_send, // param1: mode
0, // param2: custom mode
0.0f, 0.0f, 0.0f,0.0f, 0.0f); // param3 ~ param 7: not used
// record the mode change
_state._mode = mode;
_last_mode = mode_to_send;
}
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
@ -123,40 +90,6 @@ void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
// do nothing - we rely on the mount sending the messages directly
}
// send_angle_target - send earth-frame angle targets to mount
void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees)
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// convert to degrees if necessary
Vector3f target_deg = target;
if (!target_in_degrees) {
target_deg *= RAD_TO_DEG;
}
// exit immediately if mode and targets have not changed since last time they were sent
if (_state._mode == MAV_MOUNT_MODE_MAVLINK_TARGETING && target_deg == _last_angle_target) {
return;
}
// prepare and send command_long message with DO_MOUNT_CONTROL command
mavlink_msg_command_long_send(
_chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id
MAV_CMD_DO_MOUNT_CONTROL, // command number
0, // confirmation: 0=first confirmation of this command
target_deg.y, // param1: pitch (in degrees) or lat (as int32_t)
target_deg.x, // param2: roll (in degrees) or lon (as int32_t)
target_deg.z, // param3: yaw (in degrees) or alt (in meters).
0.0f,0.0f,0.0f, // param4 ~ param6 : not used
MAV_MOUNT_MODE_MAVLINK_TARGETING); // param7: MAV_MOUNT_MODE enum value
// store sent target and mode
_last_angle_target = target_deg;
_last_mode = MAV_MOUNT_MODE_MAVLINK_TARGETING;
}
/*
handle a GIMBAL_REPORT message
@ -187,8 +120,9 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
// set the demanded quaternion - tilt down with a roll and yaw of zero
Quaternion quatDem;
quatDem[0] = sqrtf(0.75f);
quatDem[2] = -sqrtf(0.25f);
quatDem.from_euler(_angle_ef_target_rad.x,
_angle_ef_target_rad.y,
_angle_ef_target_rad.z);
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;

9
libraries/AP_Mount/AP_Mount_MAVLink.h
View File

@ -19,8 +19,6 @@
#include <AP_Mount_Backend.h>
#include <AP_SmallEKF.h>
#define AP_MOUNT_MAVLINK_COMPID 10 // Use hard-coded component id to communicate with gimbal, sysid is taken from globally defined mavlink_system.sysid
class AP_Mount_MAVLink : public AP_Mount_Backend
{
@ -50,15 +48,8 @@ public:
virtual void send_gimbal_report(mavlink_channel_t chan);
private:
// send_angle_target - send earth-frame angle targets to mount
// set target_in_degrees to true to send degree targets, false if target in radians
void send_angle_target(const Vector3f& target, bool target_in_degrees);
// internal variables
bool _initialised; // true once the driver has been initialised
mavlink_channel_t _chan; // telemetry channel used to communicate with mount
enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
Vector3f _last_angle_target; // last angle target sent to mount
// state of small EKF for gimbal
SmallEKF _ekf;