From dfc086f9e9c18abcb4e7536c4cca15c764c8a10f Mon Sep 17 00:00:00 2001 From: Matthias Badaire Date: Thu, 15 Jan 2015 23:46:15 +0100 Subject: [PATCH] AP_Mount_Alexmos : Add Alexmos Serial support for 8bit cards This is the initial implementation of Alemox gimbal protocol. the uart output is for the moment hard coded but should become a parameter. --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 274 +++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_Alexmos.h | 284 ++++++++++++++++++++++++ 2 files changed, 558 insertions(+) create mode 100644 libraries/AP_Mount/AP_Mount_Alexmos.cpp create mode 100644 libraries/AP_Mount/AP_Mount_Alexmos.h diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp new file mode 100644 index 0000000000..39ccd02ce6 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -0,0 +1,274 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "AP_Mount_Alexmos.h" + +extern const AP_HAL::HAL& hal; + + +void AP_Mount_Alexmos::init (){ + _board_version = 0; + _current_firmware_version = 0.0f; + _firmware_beta_version = 0; + _port = hal.uartE; + _port->begin(115200); + _initialised = true; + _step = 0; + _param_read_once=false; + get_boardinfo(); + read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters +} + +// update mount position - should be called periodically +void AP_Mount_Alexmos::update() +{ + if (!_initialised){ + return; + } + + read_incoming(); // read the incoming messages from the gimbal + + // update based on mount mode + switch(_frontend.get_mode(_instance)) { + // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism + case MAV_MOUNT_MODE_RETRACT: + control_axis(_frontend.state[_instance]._retract_angles.get(), true); + break; + + // move mount to a neutral position, typically pointing forward + case MAV_MOUNT_MODE_NEUTRAL: + control_axis(_frontend.state[_instance]._neutral_angles.get(), true); + 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(); + control_axis(_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(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, true, false); + control_axis(_angle_ef_target_rad, false); + } + 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_Alexmos::has_pan_control() const +{ + return _gimbal_3axis; +} + +// set_mode - sets mount's mode +void AP_Mount_Alexmos::set_mode(enum MAV_MOUNT_MODE mode) +{ + // record the mode change and return success + _frontend.state[_instance]._mode = mode; +} + +// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message +void AP_Mount_Alexmos::status_msg(mavlink_channel_t chan) +{ + mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.x*100, _current_angle.y*100, _current_angle.z*100); +} + + + +/* + * get_angles () + */ + +void AP_Mount_Alexmos::get_angles( ){ + uint8_t data[1] = {(uint8_t)1}; + send_command (CMD_GET_ANGLES, data , 1); +} + + +/* + * set_motor will activate motors if true , and disable them if false. + */ + +void AP_Mount_Alexmos::set_motor(bool on){ + if ( on ){ + uint8_t data[1] = {(uint8_t)1}; + send_command (CMD_MOTORS_ON, data , 1); + } else { + uint8_t data[1] = {(uint8_t)1}; + send_command (CMD_MOTORS_OFF, data , 1); + } +} + +/* + * get board version and firmware version + */ + +void AP_Mount_Alexmos::get_boardinfo(){ + if (_board_version != 0) + return ; + uint8_t data[1] = {(uint8_t)1}; + send_command (CMD_BOARD_INFO, data , 1); +} + + +void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degrees ){ + + // convert to degrees if necessary + Vector3f target_deg = angle; + if (!target_in_degrees) { + target_deg *= RAD_TO_DEG; + } + + uint8_t mode = MODE_ANGLE; + int16_t speed_roll = DEGREE_PER_SEC_TO_VALUE(SPEED); + int16_t angle_roll = DEGREE_TO_VALUE(target_deg.x); + int16_t speed_pitch = DEGREE_PER_SEC_TO_VALUE(SPEED); + int16_t angle_pitch = DEGREE_TO_VALUE(target_deg.y); + int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(SPEED); + int16_t angle_yaw = DEGREE_TO_VALUE(target_deg.z); + uint8_t data[13] = { + (uint8_t)mode, + (uint8_t)speed_roll , (uint8_t)(speed_roll >> 8) , + (uint8_t)angle_roll , (uint8_t)(angle_roll >> 8 ), + (uint8_t)speed_pitch , (uint8_t)(speed_pitch >> 8) , + (uint8_t)angle_pitch , (uint8_t)(angle_pitch >> 8) , + (uint8_t)speed_yaw , (uint8_t)(speed_yaw >> 8 ), + (uint8_t)angle_yaw , (uint8_t)(angle_yaw >> 8) + }; + send_command (CMD_CONTROL, data , 13); +} + +/* + read current profile profile_id and global parameters from the gimbal settings +*/ +void AP_Mount_Alexmos::read_params (uint8_t profile_id){ + uint8_t data[1] = {(uint8_t) profile_id}; + send_command (CMD_READ_PARAMS, data , 1); +} + + +/* + write new parameters to the gimbal settings +*/ +void AP_Mount_Alexmos::write_params (){ + if (!_param_read_once) + return; + send_command (CMD_WRITE_PARAMS, _current_parameters.bytes , sizeof (alexmos_params)); +} + + +/* + send a command to the Alemox Serial API +*/ +void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size){ + if (_port->txspace() < (size + 5)){ + return; + } + uint8_t checksum = 0; + _port->write( '>' ); + _port->write( cmd ); // write command id + _port->write( size ); // write body size + _port->write( cmd+size ); // write header checkum + + for (uint8_t i = 0; i != size ; i++){ + checksum += data[i]; + _port->write ( data[i] ); + } + _port->write (checksum); +} + + +/* + * Parse the body of the message received from the Alexmos gimbal + */ +void AP_Mount_Alexmos::parse_body (){ + switch (_command_id ){ + case CMD_BOARD_INFO: + _board_version = _buffer.version._board_version/ 10; + _current_firmware_version = _buffer.version._firmware_version / 1000.0f ; + _firmware_beta_version = _buffer.version._firmware_version % 10 ; + _gimbal_3axis = ( _buffer.version._board_features & 0x1 ); + _gimbal_bat_monitoring = (_buffer.version._board_features & 0x2 ); + + break; + case CMD_GET_ANGLES: + _current_angle.x = VALUE_TO_DEGREE(_buffer.angles.angle_roll); + _current_angle.y = VALUE_TO_DEGREE(_buffer.angles.angle_pitch); + _current_angle.z = VALUE_TO_DEGREE(_buffer.angles.angle_yaw); + break; + case CMD_READ_PARAMS: + _param_read_once = true; + _current_parameters.params = _buffer.params; + break; + case CMD_WRITE_PARAMS: + break; + default : + _last_command_confirmed = true; + break; + } +} + +/* + * detect and read the header of the incoming message from the gimbal + */ +void AP_Mount_Alexmos::read_incoming (){ + uint8_t data ; + while (_port->available()){ + data = _port->read(); + switch (_step){ + case 0: + if ( '>' == data){ + _step = 1; + _checksum = 0; //reset checksum accumulator + _last_command_confirmed = false; + } + break; + case 1:// command ID + _checksum = data; + _command_id = data; + _step++; + break; + case 2: // Size of the body of the message + _checksum += data; + _payload_length = data; + _step++; + break; + case 3: // checksum of the header + if (_checksum != data ) { + _step = 0; + _checksum = 0; + //checksum erro + break; + } + _step++; + _checksum = 0; + _payload_counter = 0; // prepare to receive payload + break; + case 4://parsing body + _checksum += data; + if (_payload_counter < sizeof(_buffer)) { + _buffer.bytes[_payload_counter] = data; + } + if (++_payload_counter == _payload_length) + _step++; + break; + case 5://body checksum + _step = 0; + if (_checksum != data){ + break; + } + parse_body (); + } + } +} diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h new file mode 100644 index 0000000000..39ce31b899 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -0,0 +1,284 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + Alexmos Serial controlled mount backend class +*/ + +#ifndef __AP_MOUNT_ALEXMOS_H__ +#define __AP_MOUNT_ALEXMOS_H__ + +#include +#include +#include +#include +#include +#include +#include + + +//definition of the commands id for the Alexmos Serial Protocol +#define CMD_READ_PARAMS 'R' +#define CMD_WRITE_PARAMS 'W' +#define CMD_REALTIME_DATA 'D' +#define CMD_BOARD_INFO 'V' +#define CMD_CALIB_ACC 'A' +#define CMD_CALIB_GYRO 'g' +#define CMD_CALIB_EXT_GAIN 'G' +#define CMD_USE_DEFAULTS 'F' +#define CMD_CALIB_POLES 'P' +#define CMD_RESET 'r' +#define CMD_HELPER_DATA 'H' +#define CMD_CALIB_OFFSET 'O' +#define CMD_CALIB_BAT 'B' +#define CMD_MOTORS_ON 'M' +#define CMD_MOTORS_OFF 'm' +#define CMD_CONTROL 'C' +#define CMD_TRIGGER_PIN 'T' +#define CMD_EXECUTE_MENU 'E' +#define CMD_GET_ANGLES 'I' +#define CMD_CONFIRM 'C' +// Board v3.x only +#define CMD_BOARD_INFO_3 20 +#define CMD_READ_PARAMS_3 21 +#define CMD_WRITE_PARAMS_3 22 +#define CMD_REALTIME_DATA_3 23 +#define CMD_SELECT_IMU_3 24 +#define CMD_READ_PROFILE_NAMES 28 +#define CMD_WRITE_PROFILE_NAMES 29 +#define CMD_QUEUE_PARAMS_INFO_3 30 +#define CMD_SET_PARAMS_3 31 +#define CMD_SAVE_PARAMS_3 32 +#define CMD_READ_PARAMS_EXT 33 +#define CMD_WRITE_PARAMS_EXT 34 +#define CMD_AUTO_PID 35 +#define CMD_SERVO_OUT 36 +#define CMD_ERROR 255 + +#define MODE_NO_CONTROL 0 +#define MODE_SPEED 1 +#define MODE_ANGLE 2 +#define MODE_SPEED_ANGLE 3 +#define MODE_RC 4 + +#define SPEED 30 // degree/s2 + +#define VALUE_TO_DEGREE(d) ((float)((d * 720) >> 15)) +#define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.02197265625f))) +#define DEGREE_PER_SEC_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.1220740379f))) + +class AP_Mount_Alexmos : public AP_Mount_Backend +{ +public: + //constructor + AP_Mount_Alexmos(AP_Mount &frontend, uint8_t instance): + AP_Mount_Backend(frontend, instance), + _initialised(false) + {} + + + // init - performs any required initialisation for this instance + virtual void init (); + + // 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 via MAVLink + virtual void status_msg(mavlink_channel_t chan) ; + +private: + + void get_angles( ); + void set_motor(bool on); + void get_boardinfo(); + void control_axis(const Vector3f& angle , bool targets_in_degrees); + void read_params(uint8_t profile_id); + void write_params(); + + bool get_realtimedata( Vector3f& angle); + + //Alexmos Serial Protocol reading part implementation + void send_command(uint8_t cmd, uint8_t* data, uint8_t size); + void parse_body (); + void read_incoming (); + + + //structure for the Serial Protocol + + // CMD_BOARD_INFO + struct PACKED alexmos_version { + uint8_t _board_version; + uint16_t _firmware_version; + uint8_t debug_mode; + uint16_t _board_features; + }; + + // CMD_GET_ANGLES + struct PACKED alexmos_angles { + int16_t angle_roll; + int16_t rc_angle_roll; + int16_t rc_speed_roll; + int16_t angle_pitch; + int16_t rc_angle_pitch; + int16_t rc_speed_pitch; + int16_t angle_yaw; + int16_t rc_angle_yaw; + int16_t rc_speed_yaw; + }; + + // CMD_READ_PARAMS + struct PACKED alexmos_params { + uint8_t profile_id; + uint8_t roll_P; + uint8_t roll_I; + uint8_t roll_D; + uint8_t roll_power; + uint8_t roll_invert; + uint8_t roll_poles; + uint8_t pitch_P; + uint8_t pitch_I; + uint8_t pitch_D; + uint8_t pitch_power; + uint8_t pitch_invert; + uint8_t pitch_poles; + uint8_t yaw_P; + uint8_t yaw_I; + uint8_t yaw_D; + uint8_t yaw_power; + uint8_t yaw_invert; + uint8_t yaw_poles; + uint8_t acc_limiter; + int8_t ext_fc_gain_roll; + int8_t ext_fc_gain_pitch; + int16_t roll_rc_min_angle; + int16_t roll_rc_max_angle; + uint8_t roll_rc_mode; + uint8_t roll_rc_lpf; + uint8_t roll_rc_speed; + uint8_t roll_rc_follow; + int16_t pitch_rc_min_angle; + int16_t pitch_rc_max_angle; + uint8_t pitch_rc_mode; + uint8_t pitch_rc_lpf; + uint8_t pitch_rc_speed; + uint8_t pitch_rc_follow; + int16_t yaw_rc_min_angle; + int16_t yaw_rc_max_angle; + uint8_t yaw_rc_mode; + uint8_t yaw_rc_lpf; + uint8_t yaw_rc_speed; + uint8_t yaw_rc_follow; + uint8_t gyro_trust; + uint8_t use_model; + uint8_t pwm_freq; + uint8_t serial_speed; + int8_t rc_trim_roll; + int8_t rc_trim_pitch; + int8_t rc_trim_yaw; + uint8_t rc_deadband; + uint8_t rc_expo_rate; + uint8_t rc_virt_mode; + uint8_t rc_map_roll; + uint8_t rc_map_pitch; + uint8_t rc_map_yaw; + uint8_t rc_map_cmd; + uint8_t rc_map_fc_roll; + uint8_t rc_map_fc_pitch; + + uint8_t rc_mix_fc_roll; + uint8_t rc_mix_fc_pitch; + + uint8_t follow_mode; + uint8_t follow_deadband; + uint8_t follow_expo_rate; + int8_t follow_offset_roll; + int8_t follow_offset_pitch; + int8_t follow_offset_yaw; + + int8_t axis_top; + int8_t axis_right; + + uint8_t gyro_lpf; + + uint8_t gyro_sens; + uint8_t i2c_internal_pullups; + uint8_t sky_gyro_calib; + + uint8_t rc_cmd_low; + uint8_t rc_cmd_mid; + uint8_t rc_cmd_high; + + uint8_t menu_cmd_1; + uint8_t menu_cmd_2; + uint8_t menu_cmd_3; + uint8_t menu_cmd_4; + uint8_t menu_cmd_5; + uint8_t menu_cmd_long; + + uint8_t output_roll; + uint8_t output_pitch; + uint8_t output_yaw; + + int16_t bat_threshold_alarm; + int16_t bat_threshold_motors; + int16_t bat_comp_ref; + + uint8_t beeper_modes; + + uint8_t follow_roll_mix_start; + uint8_t follow_roll_mix_range; + + uint8_t booster_power_roll; + uint8_t booster_power_pitch; + uint8_t booster_power_yaw; + + uint8_t follow_speed_roll; + uint8_t follow_speed_pitch; + uint8_t follow_speed_yaw; + + uint8_t frame_angle_from_motors; + + uint8_t cur_profile_id; + + }; + union PACKED { + alexmos_version version; + alexmos_angles angles; + alexmos_params params; + uint8_t bytes[]; + } _buffer,_current_parameters; + + AP_HAL::UARTDriver *_port; + bool _initialised; + + // result of the get_boardinfo + uint8_t _board_version; + float _current_firmware_version; + uint8_t _firmware_beta_version; + bool _gimbal_3axis ; + bool _gimbal_bat_monitoring ; + + // keep the last _current_angle values + Vector3f _current_angle; + + //CMD_READ_PARAMS has been called once + bool _param_read_once; + + // Serial Protocol Variables + uint8_t _checksum ; + uint8_t _step ; + uint8_t _command_id; + uint8_t _payload_length ; + uint8_t _payload_counter ; + + // confirmed that last command was ok + bool _last_command_confirmed; +}; + + +#endif