mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Matthias Badaire
Andrew Tridgell
parent
66ad56161b
commit
dfc086f9e9
|
|
@ -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 ();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue