Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mount_Alexmos: comments and formatting 

No functional change
This commit is contained in:
Randy Mackay 2015-01-16 19:53:54 +09:00 committed by Andrew Tridgell
parent 1e5ddf3ce7
commit 93323e2136
2 changed files with 144 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

241
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -3,8 +3,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
void AP_Mount_Alexmos::init ()
void AP_Mount_Alexmos::init (){ {
_board_version = 0; _board_version = 0;
_current_firmware_version = 0.0f; _current_firmware_version = 0.0f;
_firmware_beta_version = 0; _firmware_beta_version = 0;
@ -20,11 +20,11 @@ void AP_Mount_Alexmos::init (){
// update mount position - should be called periodically // update mount position - should be called periodically
void AP_Mount_Alexmos::update() void AP_Mount_Alexmos::update()
{ {
if (!_initialised){ if (!_initialised) {
return; return;
} }
read_incoming(); // read the incoming messages from the gimbal read_incoming(); // read the incoming messages from the gimbal
// update based on mount mode // update based on mount mode
switch(_frontend.get_mode(_instance)) { switch(_frontend.get_mode(_instance)) {
@ -64,7 +64,6 @@ void AP_Mount_Alexmos::update()
} }
} }
// has_pan_control - returns true if this mount can control it's pan (required for multicopters) // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
bool AP_Mount_Alexmos::has_pan_control() const bool AP_Mount_Alexmos::has_pan_control() const
{ {
@ -85,39 +84,36 @@ 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); mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.x*100, _current_angle.y*100, _current_angle.z*100);
} }
/* /*
* get_angles () * get_angles
*/ */
void AP_Mount_Alexmos::get_angles()
void AP_Mount_Alexmos::get_angles( ){ {
uint8_t data[1] = {(uint8_t)1}; uint8_t data[1] = {(uint8_t)1};
send_command (CMD_GET_ANGLES, data , 1); send_command (CMD_GET_ANGLES, data , 1);
} }
/* /*
* set_motor will activate motors if true , and disable them if false. * set_motor will activate motors if true , and disable them if false.
*/ */
void AP_Mount_Alexmos::set_motor(bool on){ void AP_Mount_Alexmos::set_motor(bool on){
if ( on ){ if (on) {
uint8_t data[1] = {(uint8_t)1}; uint8_t data[1] = {(uint8_t)1};
send_command (CMD_MOTORS_ON, data , 1); send_command (CMD_MOTORS_ON, data, 1);
} else { } else {
uint8_t data[1] = {(uint8_t)1}; uint8_t data[1] = {(uint8_t)1};
send_command (CMD_MOTORS_OFF, data , 1); send_command (CMD_MOTORS_OFF, data, 1);
} }
} }
/* /*
* get board version and firmware version * get board version and firmware version
*/ */
void AP_Mount_Alexmos::get_boardinfo()
void AP_Mount_Alexmos::get_boardinfo(){ {
if (_board_version != 0) if (_board_version != 0) {
return ; return;
}
uint8_t data[1] = {(uint8_t)1}; uint8_t data[1] = {(uint8_t)1};
send_command (CMD_BOARD_INFO, data , 1); send_command (CMD_BOARD_INFO, data , 1);
} }
@ -125,9 +121,8 @@ void AP_Mount_Alexmos::get_boardinfo(){
/* /*
control_axis : send new angles to the gimbal at a fixed speed of 30 deg/s2 control_axis : send new angles to the gimbal at a fixed speed of 30 deg/s2
*/ */
void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degrees )
void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degrees ){ {
// convert to degrees if necessary // convert to degrees if necessary
Vector3f target_deg = angle; Vector3f target_deg = angle;
if (!target_in_degrees) { if (!target_in_degrees) {
@ -142,13 +137,13 @@ void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degree
int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED); int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
int16_t angle_yaw = DEGREE_TO_VALUE(target_deg.z); int16_t angle_yaw = DEGREE_TO_VALUE(target_deg.z);
uint8_t data[13] = { uint8_t data[13] = {
(uint8_t)mode, (uint8_t)mode,
(uint8_t)speed_roll , (uint8_t)(speed_roll >> 8) , (uint8_t)speed_roll, (uint8_t)(speed_roll >> 8),
(uint8_t)angle_roll , (uint8_t)(angle_roll >> 8 ), (uint8_t)angle_roll, (uint8_t)(angle_roll >> 8 ),
(uint8_t)speed_pitch , (uint8_t)(speed_pitch >> 8) , (uint8_t)speed_pitch, (uint8_t)(speed_pitch >> 8),
(uint8_t)angle_pitch , (uint8_t)(angle_pitch >> 8) , (uint8_t)angle_pitch, (uint8_t)(angle_pitch >> 8),
(uint8_t)speed_yaw , (uint8_t)(speed_yaw >> 8 ), (uint8_t)speed_yaw, (uint8_t)(speed_yaw >> 8),
(uint8_t)angle_yaw , (uint8_t)(angle_yaw >> 8) (uint8_t)angle_yaw, (uint8_t)(angle_yaw >> 8)
}; };
send_command (CMD_CONTROL, data , 13); send_command (CMD_CONTROL, data , 13);
} }
@ -156,28 +151,29 @@ void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degree
/* /*
read current profile profile_id and global parameters from the gimbal settings read current profile profile_id and global parameters from the gimbal settings
*/ */
void AP_Mount_Alexmos::read_params (uint8_t profile_id){ void AP_Mount_Alexmos::read_params(uint8_t profile_id){
uint8_t data[1] = {(uint8_t) profile_id}; uint8_t data[1] = {(uint8_t) profile_id};
send_command (CMD_READ_PARAMS, data , 1); send_command (CMD_READ_PARAMS, data , 1);
} }
/* /*
write new parameters to the gimbal settings write new parameters to the gimbal settings
*/ */
void AP_Mount_Alexmos::write_params (){ void AP_Mount_Alexmos::write_params()
if (!_param_read_once) {
return; if (!_param_read_once) {
return;
}
send_command (CMD_WRITE_PARAMS, _current_parameters.bytes , sizeof (alexmos_params)); send_command (CMD_WRITE_PARAMS, _current_parameters.bytes , sizeof (alexmos_params));
} }
/* /*
send a command to the Alemox Serial API send a command to the Alemox Serial API
*/ */
void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size){ void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size)
if (_port->txspace() < (size + 5)){ {
return; if (_port->txspace() < (size + 5)) {
return;
} }
uint8_t checksum = 0; uint8_t checksum = 0;
_port->write( '>' ); _port->write( '>' );
@ -185,94 +181,103 @@ void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size){
_port->write( size ); // write body size _port->write( size ); // write body size
_port->write( cmd+size ); // write header checkum _port->write( cmd+size ); // write header checkum
for (uint8_t i = 0; i != size ; i++){ for (uint8_t i = 0; i != size ; i++) {
checksum += data[i]; checksum += data[i];
_port->write ( data[i] ); _port->write ( data[i] );
} }
_port->write (checksum); _port->write (checksum);
} }
/* /*
* Parse the body of the message received from the Alexmos gimbal * Parse the body of the message received from the Alexmos gimbal
*/ */
void AP_Mount_Alexmos::parse_body (){ void AP_Mount_Alexmos::parse_body()
switch (_command_id ){ {
case CMD_BOARD_INFO: switch (_command_id ) {
_board_version = _buffer.version._board_version/ 10; case CMD_BOARD_INFO:
_current_firmware_version = _buffer.version._firmware_version / 1000.0f ; _board_version = _buffer.version._board_version/ 10;
_firmware_beta_version = _buffer.version._firmware_version % 10 ; _current_firmware_version = _buffer.version._firmware_version / 1000.0f ;
_gimbal_3axis = ( _buffer.version._board_features & 0x1 ); _firmware_beta_version = _buffer.version._firmware_version % 10 ;
_gimbal_bat_monitoring = (_buffer.version._board_features & 0x2 ); _gimbal_3axis = ( _buffer.version._board_features & 0x1 );
_gimbal_bat_monitoring = (_buffer.version._board_features & 0x2 );
break;
break; case CMD_GET_ANGLES:
case CMD_GET_ANGLES: _current_angle.x = VALUE_TO_DEGREE(_buffer.angles.angle_roll);
_current_angle.x = VALUE_TO_DEGREE(_buffer.angles.angle_roll); _current_angle.y = VALUE_TO_DEGREE(_buffer.angles.angle_pitch);
_current_angle.y = VALUE_TO_DEGREE(_buffer.angles.angle_pitch); _current_angle.z = VALUE_TO_DEGREE(_buffer.angles.angle_yaw);
_current_angle.z = VALUE_TO_DEGREE(_buffer.angles.angle_yaw); break;
break;
case CMD_READ_PARAMS: case CMD_READ_PARAMS:
_param_read_once = true; _param_read_once = true;
_current_parameters.params = _buffer.params; _current_parameters.params = _buffer.params;
break; break;
case CMD_WRITE_PARAMS:
break; case CMD_WRITE_PARAMS:
default : break;
_last_command_confirmed = true;
break; default :
_last_command_confirmed = true;
break;
} }
} }
/* /*
* detect and read the header of the incoming message from the gimbal * detect and read the header of the incoming message from the gimbal
*/ */
void AP_Mount_Alexmos::read_incoming (){ void AP_Mount_Alexmos::read_incoming()
uint8_t data ; {
while (_port->available()){ uint8_t data;
data = _port->read(); while (_port->available()) {
switch (_step){ data = _port->read();
case 0: switch (_step) {
if ( '>' == data){ case 0:
_step = 1; if ( '>' == data) {
_checksum = 0; //reset checksum accumulator _step = 1;
_last_command_confirmed = false; _checksum = 0; //reset checksum accumulator
} _last_command_confirmed = false;
break; }
case 1:// command ID break;
_checksum = data;
_command_id = data; case 1: // command ID
_step++; _checksum = data;
break; _command_id = data;
case 2: // Size of the body of the message _step++;
_checksum += data; break;
_payload_length = data;
_step++; case 2: // Size of the body of the message
break; _checksum += data;
case 3: // checksum of the header _payload_length = data;
if (_checksum != data ) { _step++;
_step = 0; break;
_checksum = 0;
//checksum erro case 3: // checksum of the header
break; if (_checksum != data ) {
} _step = 0;
_step++; _checksum = 0;
_checksum = 0; // checksum error
_payload_counter = 0; // prepare to receive payload break;
break; }
case 4://parsing body _step++;
_checksum += data; _checksum = 0;
if (_payload_counter < sizeof(_buffer)) { _payload_counter = 0; // prepare to receive payload
_buffer.bytes[_payload_counter] = data; break;
}
if (++_payload_counter == _payload_length) case 4: // parsing body
_step++; _checksum += data;
break; if (_payload_counter < sizeof(_buffer)) {
case 5://body checksum _buffer.bytes[_payload_counter] = data;
_step = 0; }
if (_checksum != data){ if (++_payload_counter == _payload_length)
break; _step++;
} break;
parse_body ();
} case 5:// body checksum
_step = 0;
if (_checksum != data) {
break;
}
parse_body ();
}
} }
} }

29
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -74,7 +74,6 @@ public:
AP_Mount_Backend(frontend, instance), AP_Mount_Backend(frontend, instance),
_initialised(false) _initialised(false)
{} {}
// init - performs any required initialisation for this instance // init - performs any required initialisation for this instance
virtual void init (); virtual void init ();
@ -93,22 +92,37 @@ public:
private: private:
void get_angles( ); // get_angles -
void get_angles();
// set_motor will activate motors if true, and disable them if false
void set_motor(bool on); void set_motor(bool on);
// get_boardinfo - get board version and firmware version
void get_boardinfo(); void get_boardinfo();
// control_axis - send new angles to the gimbal at a fixed speed of 30 deg/s
void control_axis(const Vector3f& angle , bool targets_in_degrees); void control_axis(const Vector3f& angle , bool targets_in_degrees);
// read_params - read current profile profile_id and global parameters from the gimbal settings
void read_params(uint8_t profile_id); void read_params(uint8_t profile_id);
// write_params - write new parameters to the gimbal settings
void write_params(); void write_params();
bool get_realtimedata( Vector3f& angle); bool get_realtimedata( Vector3f& angle);
//Alexmos Serial Protocol reading part implementation // Alexmos Serial Protocol reading part implementation
// send_command - send a command to the Alemox Serial API
void send_command(uint8_t cmd, uint8_t* data, uint8_t size); void send_command(uint8_t cmd, uint8_t* data, uint8_t size);
void parse_body ();
void read_incoming ();
// Parse the body of the message received from the Alexmos gimbal
void parse_body();
//structure for the Serial Protocol // read_incoming - detect and read the header of the incoming message from the gimbal
void read_incoming();
// structure for the Serial Protocol
// CMD_BOARD_INFO // CMD_BOARD_INFO
struct PACKED alexmos_version { struct PACKED alexmos_version {
@ -266,7 +280,7 @@ private:
// keep the last _current_angle values // keep the last _current_angle values
Vector3f _current_angle; Vector3f _current_angle;
//CMD_READ_PARAMS has been called once // CMD_READ_PARAMS has been called once
bool _param_read_once; bool _param_read_once;
// Serial Protocol Variables // Serial Protocol Variables
@ -280,5 +294,4 @@ private:
bool _last_command_confirmed; bool _last_command_confirmed;
}; };
#endif #endif