mirror of https://github.com/ArduPilot/ardupilot
Mount_Alexmos: comments and formatting
No functional change
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
1e5ddf3ce7
commit
93323e2136
|
|
@ -3,8 +3,8 @@
|
|||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
void AP_Mount_Alexmos::init (){
|
||||
void AP_Mount_Alexmos::init ()
|
||||
{
|
||||
_board_version = 0;
|
||||
_current_firmware_version = 0.0f;
|
||||
_firmware_beta_version = 0;
|
||||
|
|
@ -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)
|
||||
bool AP_Mount_Alexmos::has_pan_control() const
|
||||
{
|
||||
|
|
@ -85,22 +84,18 @@ 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 ()
|
||||
* get_angles
|
||||
*/
|
||||
|
||||
void AP_Mount_Alexmos::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};
|
||||
|
|
@ -114,10 +109,11 @@ void AP_Mount_Alexmos::set_motor(bool on){
|
|||
/*
|
||||
* get board version and firmware version
|
||||
*/
|
||||
|
||||
void AP_Mount_Alexmos::get_boardinfo(){
|
||||
if (_board_version != 0)
|
||||
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);
|
||||
}
|
||||
|
|
@ -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
|
||||
*/
|
||||
|
||||
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
|
||||
Vector3f target_deg = angle;
|
||||
if (!target_in_degrees) {
|
||||
|
|
@ -161,21 +156,22 @@ void AP_Mount_Alexmos::read_params (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)
|
||||
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){
|
||||
void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size)
|
||||
{
|
||||
if (_port->txspace() < (size + 5)) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -192,11 +188,11 @@ void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size){
|
|||
_port->write (checksum);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* 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:
|
||||
_board_version = _buffer.version._board_version/ 10;
|
||||
|
|
@ -204,19 +200,22 @@ void AP_Mount_Alexmos::parse_body (){
|
|||
_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;
|
||||
|
|
@ -226,7 +225,8 @@ void AP_Mount_Alexmos::parse_body (){
|
|||
/*
|
||||
* 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()) {
|
||||
data = _port->read();
|
||||
|
|
@ -238,27 +238,31 @@ void AP_Mount_Alexmos::read_incoming (){
|
|||
_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
|
||||
// checksum error
|
||||
break;
|
||||
}
|
||||
_step++;
|
||||
_checksum = 0;
|
||||
_payload_counter = 0; // prepare to receive payload
|
||||
break;
|
||||
|
||||
case 4: // parsing body
|
||||
_checksum += data;
|
||||
if (_payload_counter < sizeof(_buffer)) {
|
||||
|
|
@ -267,6 +271,7 @@ void AP_Mount_Alexmos::read_incoming (){
|
|||
if (++_payload_counter == _payload_length)
|
||||
_step++;
|
||||
break;
|
||||
|
||||
case 5:// body checksum
|
||||
_step = 0;
|
||||
if (_checksum != data) {
|
||||
|
|
|
|||
|
|
@ -75,7 +75,6 @@ public:
|
|||
_initialised(false)
|
||||
{}
|
||||
|
||||
|
||||
// init - performs any required initialisation for this instance
|
||||
virtual void init ();
|
||||
|
||||
|
|
@ -93,20 +92,35 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
// get_angles -
|
||||
void get_angles();
|
||||
|
||||
// set_motor will activate motors if true, and disable them if false
|
||||
void set_motor(bool on);
|
||||
|
||||
// get_boardinfo - get board version and firmware version
|
||||
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);
|
||||
|
||||
// read_params - read current profile profile_id and global parameters from the gimbal settings
|
||||
void read_params(uint8_t profile_id);
|
||||
|
||||
// write_params - write new parameters to the gimbal settings
|
||||
void write_params();
|
||||
|
||||
bool get_realtimedata( Vector3f& angle);
|
||||
|
||||
// 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 parse_body ();
|
||||
void read_incoming ();
|
||||
|
||||
// Parse the body of the message received from the Alexmos gimbal
|
||||
void parse_body();
|
||||
|
||||
// read_incoming - detect and read the header of the incoming message from the gimbal
|
||||
void read_incoming();
|
||||
|
||||
// structure for the Serial Protocol
|
||||
|
||||
|
|
@ -280,5 +294,4 @@ private:
|
|||
bool _last_command_confirmed;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue