aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mount: Alexmos formatting fixes

Browse Source
This commit is contained in:
Randy Mackay 2015-06-05 12:09:12 +09:00
parent 7dcd24137e
commit 7a8fe5f4d4
2 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -86,19 +86,20 @@ void AP_Mount_Alexmos::status_msg(mavlink_channel_t chan)
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);
} }
} }
@ -111,13 +112,13 @@ void AP_Mount_Alexmos::get_boardinfo()
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);
} }
/* /*
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;
@ -132,15 +133,16 @@ void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degree
outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(target_deg.y); outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(target_deg.y);
outgoing_buffer.angle_speed.speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED); outgoing_buffer.angle_speed.speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(target_deg.z); outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(target_deg.z);
send_command (CMD_CONTROL, (uint8_t *)&outgoing_buffer.angle_speed , sizeof(alexmos_angles_speed)); send_command(CMD_CONTROL, (uint8_t *)&outgoing_buffer.angle_speed, sizeof(alexmos_angles_speed));
} }
/* /*
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);
} }
/* /*
@ -151,7 +153,7 @@ void AP_Mount_Alexmos::write_params()
if (!_param_read_once) { if (!_param_read_once) {
return; return;
} }
send_command (CMD_WRITE_PARAMS, (uint8_t *)&_current_parameters.params , sizeof(alexmos_params)); send_command(CMD_WRITE_PARAMS, (uint8_t *)&_current_parameters.params, sizeof(alexmos_params));
} }
/* /*
@ -170,9 +172,9 @@ void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size)
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);
} }
/* /*
@ -185,8 +187,8 @@ void AP_Mount_Alexmos::parse_body()
_board_version = _buffer.version._board_version/ 10; _board_version = _buffer.version._board_version/ 10;
_current_firmware_version = _buffer.version._firmware_version / 1000.0f ; _current_firmware_version = _buffer.version._firmware_version / 1000.0f ;
_firmware_beta_version = _buffer.version._firmware_version % 10 ; _firmware_beta_version = _buffer.version._firmware_version % 10 ;
_gimbal_3axis = ( _buffer.version._board_features & 0x1 ); _gimbal_3axis = (_buffer.version._board_features & 0x1);
_gimbal_bat_monitoring = (_buffer.version._board_features & 0x2 ); _gimbal_bat_monitoring = (_buffer.version._board_features & 0x2);
break; break;
case CMD_GET_ANGLES: case CMD_GET_ANGLES:
@ -222,7 +224,7 @@ void AP_Mount_Alexmos::read_incoming()
if (numc < 0 ){ if (numc < 0 ){
return; return;
} }
for (int16_t i = 0; i < numc; i++) { // Process bytes received for (int16_t i = 0; i < numc; i++) { // Process bytes received
data = _port->read(); data = _port->read();
switch (_step) { switch (_step) {
@ -272,7 +274,7 @@ void AP_Mount_Alexmos::read_incoming()
if (_checksum != data) { if (_checksum != data) {
break; break;
} }
parse_body (); parse_body();
} }
} }
} }

10
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -228,7 +228,7 @@ private:
uint8_t rc_map_cmd; uint8_t rc_map_cmd;
uint8_t rc_map_fc_roll; uint8_t rc_map_fc_roll;
uint8_t rc_map_fc_pitch; uint8_t rc_map_fc_pitch;
uint8_t rc_mix_fc_roll; uint8_t rc_mix_fc_roll;
uint8_t rc_mix_fc_pitch; uint8_t rc_mix_fc_pitch;
@ -300,8 +300,8 @@ private:
uint8_t _board_version; uint8_t _board_version;
float _current_firmware_version; float _current_firmware_version;
uint8_t _firmware_beta_version; uint8_t _firmware_beta_version;
bool _gimbal_3axis ; bool _gimbal_3axis;
bool _gimbal_bat_monitoring ; bool _gimbal_bat_monitoring;
// keep the last _current_angle values // keep the last _current_angle values
Vector3f _current_angle; Vector3f _current_angle;
@ -310,8 +310,8 @@ private:
bool _param_read_once; bool _param_read_once;
// Serial Protocol Variables // Serial Protocol Variables
uint8_t _checksum ; uint8_t _checksum;
uint8_t _step ; uint8_t _step;
uint8_t _command_id; uint8_t _command_id;
uint8_t _payload_length; uint8_t _payload_length;
uint8_t _payload_counter; uint8_t _payload_counter;