Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_MOUNT_ALEXMOS_ENABLED
       4             : 
       5             : #include "AP_Mount_Alexmos.h"
       6             : 
       7             : #include <AP_SerialManager/AP_SerialManager.h>
       8             : 
       9             : //definition of the commands id for the Alexmos Serial Protocol
      10             : #define CMD_READ_PARAMS 'R'
      11             : #define CMD_WRITE_PARAMS 'W'
      12             : #define CMD_REALTIME_DATA 'D'
      13             : #define CMD_BOARD_INFO 'V'
      14             : #define CMD_CALIB_ACC 'A'
      15             : #define CMD_CALIB_GYRO 'g'
      16             : #define CMD_CALIB_EXT_GAIN 'G'
      17             : #define CMD_USE_DEFAULTS 'F'
      18             : #define CMD_CALIB_POLES 'P'
      19             : #define CMD_RESET 'r'
      20             : #define CMD_HELPER_DATA 'H'
      21             : #define CMD_CALIB_OFFSET 'O'
      22             : #define CMD_CALIB_BAT 'B'
      23             : #define CMD_MOTORS_ON 'M'
      24             : #define CMD_MOTORS_OFF 'm'
      25             : #define CMD_CONTROL 'C'
      26             : #define CMD_TRIGGER_PIN 'T'
      27             : #define CMD_EXECUTE_MENU 'E'
      28             : #define CMD_GET_ANGLES 'I'
      29             : #define CMD_CONFIRM 'C'
      30             : // Board v3.x only
      31             : #define CMD_BOARD_INFO_3 20
      32             : #define CMD_READ_PARAMS_3 21
      33             : #define CMD_WRITE_PARAMS_3 22
      34             : #define CMD_REALTIME_DATA_3 23
      35             : #define CMD_SELECT_IMU_3 24
      36             : #define CMD_READ_PROFILE_NAMES 28
      37             : #define CMD_WRITE_PROFILE_NAMES 29
      38             : #define CMD_QUEUE_PARAMS_INFO_3 30
      39             : #define CMD_SET_PARAMS_3 31
      40             : #define CMD_SAVE_PARAMS_3 32
      41             : #define CMD_READ_PARAMS_EXT 33
      42             : #define CMD_WRITE_PARAMS_EXT 34
      43             : #define CMD_AUTO_PID 35
      44             : #define CMD_SERVO_OUT 36
      45             : #define CMD_ERROR 255
      46             : 
      47             : #define AP_MOUNT_ALEXMOS_MODE_NO_CONTROL 0
      48             : #define AP_MOUNT_ALEXMOS_MODE_SPEED 1
      49             : #define AP_MOUNT_ALEXMOS_MODE_ANGLE 2
      50             : #define AP_MOUNT_ALEXMOS_MODE_SPEED_ANGLE 3
      51             : #define AP_MOUNT_ALEXMOS_MODE_RC 4
      52             : 
      53             : #define AP_MOUNT_ALEXMOS_SPEED 30 // deg/s
      54             : 
      55             : #define INT14_DEGREES (360.0f / float(0x3FFF)) // 1 full rotation in degrees over 14 bit range
      56             : #define VALUE_TO_DEGREE(d) (float(d)*INT14_DEGREES)
      57             : #define DEGREE_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/INT14_DEGREES)))
      58             : #define DEGREE_PER_SEC_TO_VALUE(d) ((int16_t)((float)(d)*(1.0f/0.1220740379f)))
      59             : 
      60           0 : void AP_Mount_Alexmos::init()
      61             : {
      62           0 :     const AP_SerialManager& serial_manager = AP::serialmanager();
      63             : 
      64             :     // check for alexmos protcol
      65           0 :     if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_AlexMos, 0))) {
      66           0 :         _initialised = true;
      67           0 :         get_boardinfo();
      68           0 :         read_params(0); //we request parameters for profile 0 and therfore get global and profile parameters
      69             :     }
      70           0 :     AP_Mount_Backend::init();
      71           0 : }
      72             : 
      73             : // update mount position - should be called periodically
      74           0 : void AP_Mount_Alexmos::update()
      75             : {
      76           0 :     AP_Mount_Backend::update();
      77             : 
      78           0 :     if (!_initialised) {
      79           0 :         return;
      80             :     }
      81             : 
      82           0 :     read_incoming(); // read the incoming messages from the gimbal
      83             : 
      84             :     // update based on mount mode
      85           0 :     update_mnt_target();
      86             : 
      87             :     // send target angles or rates depending on the target type
      88           0 :     send_target_to_gimbal();
      89             : }
      90             : 
      91             : // has_pan_control - returns true if this mount can control its pan (required for multicopters)
      92           0 : bool AP_Mount_Alexmos::has_pan_control() const
      93             : {
      94           0 :     return _gimbal_3axis && yaw_range_valid();
      95             : }
      96             : 
      97             : // get attitude as a quaternion.  returns true on success
      98           0 : bool AP_Mount_Alexmos::get_attitude_quaternion(Quaternion& att_quat)
      99             : {
     100           0 :     if (!_initialised) {
     101           0 :         return false;
     102             :     }
     103             : 
     104             :     // request attitude from gimbal
     105           0 :     get_angles();
     106             : 
     107             :     // construct quaternion
     108           0 :     att_quat.from_euler(radians(_current_angle.x), radians(_current_angle.y), radians(_current_angle.z));
     109           0 :     return true;
     110             : }
     111             : 
     112             : /*
     113             :  * get_angles
     114             :  */
     115           0 : void AP_Mount_Alexmos::get_angles()
     116             : {
     117           0 :     uint8_t data[1] = {(uint8_t)1};
     118           0 :     send_command(CMD_GET_ANGLES, data, 1);
     119           0 : }
     120             : 
     121             : /*
     122             :  * set_motor will activate motors if true, and disable them if false.
     123             :  */
     124           0 : void AP_Mount_Alexmos::set_motor(bool on)
     125             : {
     126           0 :     if (on) {
     127           0 :         uint8_t data[1] = {(uint8_t)1};
     128           0 :         send_command(CMD_MOTORS_ON, data, 1);
     129             :     } else {
     130           0 :         uint8_t data[1] = {(uint8_t)1};
     131           0 :         send_command(CMD_MOTORS_OFF, data, 1);
     132             :     }
     133           0 : }
     134             : 
     135             : /*
     136             :  * get board version and firmware version
     137             :  */
     138           0 : void AP_Mount_Alexmos::get_boardinfo()
     139             : {
     140           0 :     if (_board_version != 0) {
     141           0 :         return;
     142             :     }
     143           0 :     uint8_t data[1] = {(uint8_t)1};
     144           0 :     send_command(CMD_BOARD_INFO, data, 1);
     145             : }
     146             : 
     147             : /*
     148             :   control_axis : send new angle target to the gimbal at a fixed speed of 30 deg/s
     149             : */
     150           0 : void AP_Mount_Alexmos::send_target_angles(const MountAngleTarget& angle_target_rad)
     151             : {
     152             :     alexmos_parameters outgoing_buffer;
     153           0 :     outgoing_buffer.angle_speed.mode = AP_MOUNT_ALEXMOS_MODE_ANGLE;
     154           0 :     outgoing_buffer.angle_speed.speed_roll = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
     155           0 :     outgoing_buffer.angle_speed.angle_roll = DEGREE_TO_VALUE(degrees(angle_target_rad.roll));
     156           0 :     outgoing_buffer.angle_speed.speed_pitch = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
     157           0 :     outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(degrees(angle_target_rad.pitch));
     158           0 :     outgoing_buffer.angle_speed.speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
     159           0 :     outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(degrees(angle_target_rad.get_bf_yaw()));
     160           0 :     send_command(CMD_CONTROL, (uint8_t *)&outgoing_buffer.angle_speed, sizeof(alexmos_angles_speed));
     161           0 : }
     162             : 
     163             : /*
     164             :   read current profile profile_id and global parameters from the gimbal settings
     165             : */
     166           0 : void AP_Mount_Alexmos::read_params(uint8_t profile_id)
     167             : {
     168           0 :     uint8_t data[1] = {(uint8_t) profile_id}; 
     169           0 :     send_command(CMD_READ_PARAMS, data, 1);
     170           0 : }
     171             : 
     172             : /*
     173             :   write new parameters to the gimbal settings
     174             : */
     175           0 : void AP_Mount_Alexmos::write_params()
     176             : {
     177           0 :     if (!_param_read_once) {
     178           0 :         return;
     179             :     }
     180           0 :     send_command(CMD_WRITE_PARAMS, (uint8_t *)&_current_parameters.params, sizeof(alexmos_params));
     181             : }
     182             : 
     183             : /*
     184             :  send a command to the Alemox Serial API
     185             : */
     186           0 : void AP_Mount_Alexmos::send_command(uint8_t cmd, uint8_t* data, uint8_t size)
     187             : {
     188           0 :     if (_port->txspace() < (size + 5U)) {
     189           0 :         return;
     190             :     }
     191           0 :     uint8_t checksum = 0;
     192           0 :     _port->write( '>' );
     193           0 :     _port->write( cmd );  // write command id
     194           0 :     _port->write( size );  // write body size
     195           0 :     _port->write( cmd+size ); // write header checkum
     196             : 
     197           0 :     for (uint8_t i = 0;  i != size ; i++) {
     198           0 :         checksum += data[i];
     199           0 :         _port->write( data[i] );
     200             :     }
     201           0 :     _port->write(checksum);
     202             : }
     203             : 
     204             : /*
     205             :  * Parse the body of the message received from the Alexmos gimbal
     206             :  */
     207           0 : void AP_Mount_Alexmos::parse_body()
     208             : {
     209           0 :     switch (_command_id ) {
     210           0 :         case CMD_BOARD_INFO:
     211           0 :             _board_version = _buffer.version._board_version/ 10;
     212           0 :             _current_firmware_version = _buffer.version._firmware_version * 0.001f ;
     213           0 :             _firmware_beta_version = _buffer.version._firmware_version % 10 ;
     214           0 :             _gimbal_3axis = (_buffer.version._board_features & 0x1);
     215           0 :             _gimbal_bat_monitoring = (_buffer.version._board_features & 0x2);
     216           0 :             break;
     217             : 
     218           0 :         case CMD_GET_ANGLES:
     219           0 :             _current_angle.x = VALUE_TO_DEGREE(_buffer.angles.angle_roll);
     220           0 :             _current_angle.y = VALUE_TO_DEGREE(_buffer.angles.angle_pitch);
     221           0 :             _current_angle.z = VALUE_TO_DEGREE(_buffer.angles.angle_yaw);
     222           0 :             break;
     223             : 
     224           0 :         case CMD_READ_PARAMS:
     225           0 :             _param_read_once = true;
     226           0 :             _current_parameters.params = _buffer.params;
     227           0 :             break;
     228             : 
     229           0 :         case CMD_WRITE_PARAMS:
     230           0 :             break;
     231             : 
     232           0 :         default :
     233           0 :             _last_command_confirmed = true;
     234           0 :             break;
     235             :     }
     236           0 : }
     237             : 
     238             : /*
     239             :  * detect and read the header of the incoming message from the gimbal
     240             :  */
     241           0 : void AP_Mount_Alexmos::read_incoming()
     242             : {
     243             :     uint8_t data;
     244             :     int16_t numc;
     245             : 
     246           0 :     numc = _port->available();
     247             : 
     248           0 :     if (numc < 0 ) {
     249           0 :         return;
     250             :     }
     251             : 
     252           0 :     for (int16_t i = 0; i < numc; i++) {        // Process bytes received
     253           0 :         data = _port->read();
     254           0 :         switch (_step) {
     255           0 :             case 0:
     256           0 :                 if ( '>' == data) {
     257           0 :                     _step = 1;
     258           0 :                     _checksum = 0; //reset checksum accumulator
     259           0 :                     _last_command_confirmed = false;
     260             :                 }
     261           0 :                 break;
     262             : 
     263           0 :             case 1: // command ID
     264           0 :                 _checksum = data;
     265           0 :                 _command_id = data;
     266           0 :                 _step++;
     267           0 :                 break;
     268             : 
     269           0 :             case 2: // Size of the body of the message
     270           0 :                 _checksum += data;
     271           0 :                 _payload_length = data;
     272           0 :                 _step++;
     273           0 :                 break;
     274             : 
     275           0 :             case 3:     // checksum of the header
     276           0 :                 if (_checksum != data ) {
     277           0 :                     _step = 0;
     278           0 :                     _checksum = 0;
     279             :                     // checksum error
     280           0 :                     break;
     281             :                 }
     282           0 :                 _step++;
     283           0 :                 _checksum = 0;
     284           0 :                 _payload_counter = 0;                               // prepare to receive payload
     285           0 :                 break;
     286             : 
     287           0 :             case 4: // parsing body
     288           0 :                 _checksum += data;
     289           0 :                 if (_payload_counter < sizeof(_buffer)) {
     290           0 :                     _buffer[_payload_counter] = data;
     291             :                 }
     292           0 :                 if (++_payload_counter == _payload_length)
     293           0 :                     _step++;
     294           0 :                 break;
     295             : 
     296           0 :             case 5:// body checksum
     297           0 :                 _step = 0;
     298           0 :                 if (_checksum  != data) {
     299           0 :                     break;
     300             :                 }
     301           0 :                 parse_body();
     302             :         }
     303             :     }
     304             : }
     305             : #endif // HAL_MOUNT_ALEXMOS_ENABLED