diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp new file mode 100644 index 0000000000..85cb760ef9 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -0,0 +1,289 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#include +#include "include/mavlink/v1.0/checksum.h" +#include "../AP_HAL/utility/RingBuffer.h" + +extern const AP_HAL::HAL& hal; + +AP_Mount_SToRM32_serial::AP_Mount_SToRM32_serial(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : + AP_Mount_Backend(frontend, state, instance), + _port(NULL), + _initialised(false), + _last_send(0), + _reply_length(0), + _reply_counter(0), + _reply_type(ReplyType_UNKNOWN) +{} + +// init - performs any required initialisation for this instance +void AP_Mount_SToRM32_serial::init(const AP_SerialManager& serial_manager) +{ + _port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_SToRM32, 0); + if (_port) { + _initialised = true; + set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); + } + +} + +// update mount position - should be called periodically +void AP_Mount_SToRM32_serial::update() +{ + // exit immediately if not initialised + if (!_initialised) { + return; + } + + read_incoming(); // read the incoming messages from the gimbal + + // 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 + resend_now = resend_now || ((hal.scheduler->millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS); + + if ((hal.scheduler->millis() - _last_send) > AP_MOUNT_STORM32_SERIAL_RESEND_MS*2) { + _reply_type = ReplyType_UNKNOWN; + } + if (can_send(resend_now)) { + if (resend_now) { + 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); + get_angles(); + _reply_type = ReplyType_ACK; + _reply_counter = 0; + _reply_length = get_reply_size(_reply_type); + } else { + get_angles(); + _reply_type = ReplyType_DATA; + _reply_counter = 0; + _reply_length = get_reply_size(_reply_type); + } + } +} + +// has_pan_control - returns true if this mount can control it's pan (required for multicopters) +bool AP_Mount_SToRM32_serial::has_pan_control() const +{ + // we do not have yaw control + return false; +} + +// set_mode - sets mount's mode +void AP_Mount_SToRM32_serial::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_serial::status_msg(mavlink_channel_t chan) +{ + // return target angles as gimbal's actual attitude. + mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z); +} + +bool AP_Mount_SToRM32_serial::can_send(bool with_control) { + uint16_t required_tx = 1; + if (with_control) { + required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct); + } + return (_reply_type == ReplyType_UNKNOWN) && (_port->txspace() >= required_tx); +} + + +// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message +void AP_Mount_SToRM32_serial::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode) +{ + + static cmd_set_angles_struct cmd_set_angles_data = { + 0xFD, + 0x0E, + 0x00, + 0x52, + 0x43, + 0x11, + 0, // pitch + 0, // roll + 0, // yaw + 0, // flags + 0, // type + 0, // crc + }; + + // exit immediately if not initialised + if (!_initialised) { + return; + } + + if ((size_t)_port->txspace() < sizeof(cmd_set_angles_data)) { + return; + } + + // reverse pitch and yaw control + pitch_deg = -pitch_deg; + yaw_deg = -yaw_deg; + + // send CMD_SETANGLE + cmd_set_angles_data.pitch = pitch_deg*100; + cmd_set_angles_data.roll = roll_deg*100; + cmd_set_angles_data.yaw = yaw_deg*100; + + uint8_t* buf = (uint8_t*)&cmd_set_angles_data; + + cmd_set_angles_data.crc = crc_calculate(&buf[1], sizeof(cmd_set_angles_data)-3); + + for (uint8_t i = 0; i != sizeof(cmd_set_angles_data) ; i++) { + _port->write(buf[i]); + } + + // store time of send + _last_send = hal.scheduler->millis(); +} + +void AP_Mount_SToRM32_serial::get_angles() { + // exit immediately if not initialised + if (!_initialised) { + return; + } + + if (_port->txspace() < 1) { + return; + } + + _port->write('d'); +}; + + +uint8_t AP_Mount_SToRM32_serial::get_reply_size(ReplyType reply_type) { + switch (reply_type) { + case ReplyType_DATA: + return sizeof(SToRM32_reply_data_struct); + break; + case ReplyType_ACK: + return sizeof(SToRM32_reply_ack_struct); + break; + default: + return 0; + } +} + + +void AP_Mount_SToRM32_serial::read_incoming() { + uint8_t data; + int16_t numc; + + numc = _port->available(); + + if (numc < 0 ){ + return; + } + + for (int16_t i = 0; i < numc; i++) { // Process bytes received + data = _port->read(); + if (_reply_type == ReplyType_UNKNOWN) { + continue; + } + + _buffer.bytes[_reply_counter++] = data; + if (_reply_counter == _reply_length) { + parse_reply(); + + switch (_reply_type) { + case ReplyType_ACK: + _reply_type = ReplyType_DATA; + _reply_length = get_reply_size(_reply_type); + _reply_counter = 0; + break; + case ReplyType_DATA: + _reply_type = ReplyType_UNKNOWN; + _reply_length = get_reply_size(_reply_type); + _reply_counter = 0; + break; + default: + _reply_length = get_reply_size(_reply_type); + _reply_counter = 0; + break; + } + } + } +} + +void AP_Mount_SToRM32_serial::parse_reply() { + uint16_t crc; + bool crc_ok; + + switch (_reply_type) { + case ReplyType_DATA: + crc = crc_calculate(_buffer.bytes, sizeof(_buffer.data)-3); + crc_ok = crc == _buffer.data.crc; + if (!crc_ok) { + break; + } + + _current_angle.x = _buffer.data.imu1_roll; + _current_angle.y = _buffer.data.imu1_pitch; + _current_angle.z = _buffer.data.imu1_yaw; + break; + case ReplyType_ACK: + crc = crc_calculate(&_buffer.bytes[1], sizeof(SToRM32_reply_ack_struct)-3); + crc_ok = crc == _buffer.ack.crc; + break; + default: + break; + } +} diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h new file mode 100644 index 0000000000..bdda619096 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -0,0 +1,159 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + SToRM32 mount using serial protocol backend class + */ + +#ifndef __AP_MOUNT_STORM32_SERIAL_H__ +#define __AP_MOUNT_STORM32_SERIAL_H__ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#define AP_MOUNT_STORM32_SERIAL_RESEND_MS 1000 // resend angle targets to gimbal once per second + +class AP_Mount_SToRM32_serial : public AP_Mount_Backend +{ + +public: + // Constructor + AP_Mount_SToRM32_serial(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); + + // send read data request + void get_angles(); + + // read_incoming + void read_incoming(); + void parse_reply(); + + enum ReplyType { + ReplyType_UNKNOWN = 0, + ReplyType_DATA, + ReplyType_ACK + }; + + //void add_next_reply(ReplyType reply_type); + uint8_t get_reply_size(ReplyType reply_type); + bool can_send(bool with_control); + + struct PACKED SToRM32_reply_data_struct { + uint16_t state; + uint16_t status; + uint16_t status2; + + uint16_t i2c_errors; + uint16_t lipo_voltage; + uint16_t systicks; + uint16_t cycle_time; + + int16_t imu1_gx; + int16_t imu1_gy; + int16_t imu1_gz; + + int16_t imu1_ax; + int16_t imu1_ay; + int16_t imu1_az; + + int16_t ahrs_x; + int16_t ahrs_y; + int16_t ahrs_z; + + int16_t imu1_pitch; + int16_t imu1_roll; + int16_t imu1_yaw; + + int16_t cpid_pitch; + int16_t cpid_roll; + int16_t cpid_yaw; + + uint16_t input_pitch; + uint16_t input_roll; + uint16_t input_yaw; + + int16_t imu2_pitch; + int16_t imu2_roll; + int16_t imu2_yaw; + + int16_t mag2_yaw; + int16_t mag2_pitch; + + int16_t ahrs_imu_confidence; + + uint16_t function_input_values; + + uint16_t crc; + uint8_t magic; + }; + + struct PACKED SToRM32_reply_ack_struct { + uint8_t magic; + uint8_t something[5]; + uint8_t data; + uint16_t crc; + }; + + struct PACKED cmd_set_angles_struct { + uint8_t byte1; + uint8_t byte2; + uint8_t byte3; + uint8_t byte4; + uint8_t byte5; + uint8_t byte6; + float pitch; + float roll; + float yaw; + uint8_t flags; + uint8_t type; + uint16_t crc; + }; + + + // internal variables + AP_HAL::UARTDriver *_port; + + bool _initialised; // true once the driver has been initialised + uint32_t _last_send; // system time of last do_mount_control sent to gimbal + + uint8_t _reply_length; + uint8_t _reply_counter; + ReplyType _reply_type; + + + union PACKED SToRM32_reply { + SToRM32_reply_data_struct data; + SToRM32_reply_ack_struct ack; + uint8_t bytes[]; + } _buffer; + + // keep the last _current_angle values + Vector3l _current_angle; +}; + +#endif // __AP_MOUNT_STORM32_SERIAL_H__