From 92c7949355fb927839e95f77c52705fdcc65e15c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 14 Feb 2015 21:27:32 +0900 Subject: [PATCH] Mount: SToRM32 mount backend --- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 139 ++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_SToRM32.h | 58 ++++++++++ 2 files changed, 197 insertions(+) create mode 100644 libraries/AP_Mount/AP_Mount_SToRM32.cpp create mode 100644 libraries/AP_Mount/AP_Mount_SToRM32.h diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp new file mode 100644 index 0000000000..18dd268478 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -0,0 +1,139 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#include +#include + +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), + _chan(MAVLINK_COMM_0), + _last_send(0) +{} + +// init - performs any required initialisation for this instance +void AP_Mount_SToRM32::init(const AP_SerialManager& serial_manager) +{ + // get_mavlink_channel for MAVLink2 + if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink2, _chan)) { + _initialised = true; + set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); + } +} + +// update mount position - should be called periodically +void AP_Mount_SToRM32::update() +{ + // exit immediately if not initialised + if (!_initialised) { + 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.x)*100, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.z)*100); +} + +// 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; + } + + // send command_long command containing a do_mount_control command + mavlink_msg_command_long_send(_chan, + AP_MOUNT_STORM32_SYSID, + AP_MOUNT_STORM32_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(); +} diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h new file mode 100644 index 0000000000..9d435d3a9a --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -0,0 +1,58 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + SToRM32 mount backend class + */ + +#ifndef __AP_MOUNT_STORM32_H__ +#define __AP_MOUNT_STORM32_H__ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#define AP_MOUNT_STORM32_SYSID 71 // hardcoded system id +#define AP_MOUNT_STORM32_COMPID 67 // hard coded component id for communicating with the gimbal + +#define AP_MOUNT_STORM32_RESEND_MS 1000 // resend angle targets to gimbal once per second + +class AP_Mount_SToRM32 : public AP_Mount_Backend +{ + +public: + // Constructor + AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); + + // init - performs any required initialisation for this instance + virtual void init(const AP_SerialManager& serial_manager); + + // update mount position - should be called periodically + virtual void update(); + + // has_pan_control - returns true if this mount can control it's pan (required for multicopters) + virtual bool has_pan_control() const; + + // set_mode - sets mount's mode + virtual void set_mode(enum MAV_MOUNT_MODE mode); + + // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message + virtual void status_msg(mavlink_channel_t chan); + +private: + + // send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message + void send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode); + + // internal variables + bool _initialised; // true once the driver has been initialised + mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal. Currently hard-coded to Telem2 + uint32_t _last_send; // system time of last do_mount_control sent to gimbal +}; + +#endif // __AP_MOUNT_STORM32_H__