ardupilot/libraries/AP_Mount/AP_Mount_Alexmos.cpp

// -*- 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)
{
    get_angles();
    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);
}

/*
  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 ){

    // convert to degrees if necessary
    Vector3f target_deg = angle;
    if (!target_in_degrees) {
        target_deg *= RAD_TO_DEG;
    }

    uint8_t mode = AP_MOUNT_ALEXMOS_MODE_ANGLE;
    int16_t speed_roll = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
    int16_t angle_roll = DEGREE_TO_VALUE(target_deg.x);
    int16_t speed_pitch = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED);
    int16_t angle_pitch = DEGREE_TO_VALUE(target_deg.y);
    int16_t speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_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 ();
	}
    }
}