zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
master
Matthias Badaire 10 years ago committed by Andrew Tridgell
parent
66ad56161b
commit
dfc086f9e9
2 changed files with 558 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 274
      libraries/AP_Mount/AP_Mount_Alexmos.cpp
  2. 284
      libraries/AP_Mount/AP_Mount_Alexmos.h

274
libraries/AP_Mount/AP_Mount_Alexmos.cpp
Unescape View File

@ -0,0 +1,274 @@ @@ -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 ();
}
}
}

284
libraries/AP_Mount/AP_Mount_Alexmos.h
Unescape View File

@ -0,0 +1,284 @@ @@ -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 <AP_Mount.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <AP_Mount_Backend.h>
//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
Write Preview
Loading…
Cancel
Save