// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #include extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) { _dcm=dcm; _dcm_hil=NULL; _gps=gps; //set_mode(MAV_MOUNT_MODE_RETRACT); //set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink set_mode(MAV_MOUNT_MODE_GPS_POINT); // FIXME: this is to test ONLY targeting _retract_angles.x=0; _retract_angles.y=0; _retract_angles.z=0; } AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm) { _dcm=NULL; _dcm_hil=dcm; _gps=gps; //set_mode(MAV_MOUNT_MODE_RETRACT); //set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); // FIXME: this is to test ONLY targeting _retract_angles.x=0; _retract_angles.y=0; _retract_angles.z=0; } //sets the servo angles for retraction, note angles are * 100 void AP_Mount::set_retract_angles(int roll, int pitch, int yaw) { _retract_angles.x=roll; _retract_angles.y=pitch; _retract_angles.z=yaw; } //sets the servo angles for neutral, note angles are * 100 void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw) { _neutral_angles.x=roll; _neutral_angles.y=pitch; _neutral_angles.z=yaw; } //sets the servo angles for MAVLink, note angles are * 100 void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw) { _mavlink_angles.x = roll; _mavlink_angles.y = pitch; _mavlink_angles.z = yaw; } // used to tell the mount to track GPS location void AP_Mount::set_GPS_target_location(Location targetGPSLocation) { _target_GPS_location=targetGPSLocation; } // This one should be called periodically void AP_Mount::update_mount_position() { Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs Vector3f targ; //holds target vector, var is used as temp in calcs Vector3f aux_vec; //holds target vector, var is used as temp in calcs switch(_mount_mode) { case MAV_MOUNT_MODE_RETRACT: roll_angle =100*_retract_angles.x; pitch_angle=100*_retract_angles.y; yaw_angle =100*_retract_angles.z; break; case MAV_MOUNT_MODE_NEUTRAL: roll_angle =100*_neutral_angles.x; pitch_angle=100*_neutral_angles.y; yaw_angle =100*_neutral_angles.z; break; case MAV_MOUNT_MODE_MAVLINK_TARGETING: { aux_vec.x = _mavlink_angles.x; aux_vec.y = _mavlink_angles.y; aux_vec.z = _mavlink_angles.z; m = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed(); //rotate vector targ = m*aux_vec; // TODO The next three lines are probably not correct yet roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_mavlink_angles.y; //roll pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:_neutral_angles.x; //pitch yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:_neutral_angles.z; //yaw break; } case MAV_MOUNT_MODE_RC_TARGETING: // radio manual control { // TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one if (_dcm) { roll_angle = -_dcm->roll_sensor; pitch_angle = -_dcm->pitch_sensor; yaw_angle = -_dcm->yaw_sensor; } if (_dcm_hil) { roll_angle = -_dcm_hil->roll_sensor; pitch_angle = -_dcm_hil->pitch_sensor; yaw_angle = -_dcm_hil->yaw_sensor; } if (g_rc_function[RC_Channel_aux::k_mount_roll]) roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]); if (g_rc_function[RC_Channel_aux::k_mount_pitch]) pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]); if (g_rc_function[RC_Channel_aux::k_mount_yaw]) yaw_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]); break; } case MAV_MOUNT_MODE_GPS_POINT: { if(_gps->fix) { calc_GPS_target_vector(&_target_GPS_location); } m = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed(); targ = m*_GPS_vector; /* disable stabilization for now, this will help debug */ _stab_roll = 0;_stab_pitch=0;_stab_yaw=0; /**/ // TODO The next three lines are probably not correct yet roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_GPS_vector.y; //roll pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:0; //pitch yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:degrees(atan2(-_GPS_vector.x,_GPS_vector.y))*100; //yaw break; } default: //do nothing break; } // write the results to the servos // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10); G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10); G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10); } void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) { _mount_mode=mode; } void AP_Mount::configure_msg(mavlink_message_t* msg) { __mavlink_mount_configure_t packet; mavlink_msg_mount_configure_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) { // not for us return; } set_mode((enum MAV_MOUNT_MODE)packet.mount_mode); _stab_pitch = packet.stab_pitch; _stab_roll = packet.stab_roll; _stab_yaw = packet.stab_yaw; } void AP_Mount::control_msg(mavlink_message_t *msg) { __mavlink_mount_control_t packet; mavlink_msg_mount_control_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) { // not for us return; } switch (_mount_mode) { case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization set_retract_angles(packet.input_b, packet.input_a, packet.input_c); if (packet.save_position) { // TODO: Save current trimmed position on EEPROM } break; case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM set_neutral_angles(packet.input_b, packet.input_a, packet.input_c); if (packet.save_position) { // TODO: Save current trimmed position on EEPROM } break; case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization set_mavlink_angles(packet.input_b, packet.input_a, packet.input_c); break; case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization break; case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt Location targetGPSLocation; targetGPSLocation.lat = packet.input_a; targetGPSLocation.lng = packet.input_b; targetGPSLocation.alt = packet.input_c; set_GPS_target_location(targetGPSLocation); break; } } void AP_Mount::status_msg(mavlink_message_t *msg) { __mavlink_mount_status_t packet; mavlink_msg_mount_status_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) { // not for us return; } switch (_mount_mode) { case MAV_MOUNT_MODE_RETRACT: // safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization case MAV_MOUNT_MODE_NEUTRAL: // neutral position (Roll,Pitch,Yaw) from EEPROM case MAV_MOUNT_MODE_MAVLINK_TARGETING: // neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization case MAV_MOUNT_MODE_RC_TARGETING: // neutral position and start RC Roll,Pitch,Yaw control with stabilization packet.pointing_b = roll_angle; ///< degrees*100 packet.pointing_a = pitch_angle; ///< degrees*100 packet.pointing_c = yaw_angle; ///< degrees*100 break; case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt packet.pointing_a = _target_GPS_location.lat; ///< latitude packet.pointing_b = _target_GPS_location.lng; ///< longitude packet.pointing_c = _target_GPS_location.alt; ///< altitude break; } // status reply // TODO: is COMM_3 correct ? mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.pointing_a, packet.pointing_b, packet.pointing_c); } void AP_Mount::set_roi_cmd() { // TODO get the information out of the mission command and use it } void AP_Mount::configure_cmd() { // TODO get the information out of the mission command and use it } void AP_Mount::control_cmd() { // TODO get the information out of the mission command and use it } void AP_Mount::calc_GPS_target_vector(struct Location *target) { _GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195; _GPS_vector.y = (target->lat-_gps->latitude)*.01113195; _GPS_vector.z = (_gps->altitude-target->alt); } void AP_Mount::update_mount_type() { // Auto-detect the mount gimbal type depending on the functions assigned to the servos if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) { _mount_type = k_pan_tilt; } if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL)) { _mount_type = k_tilt_roll; } if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL)) { _mount_type = k_pan_tilt_roll; } } // This function is needed to let the HIL code compile long AP_Mount::rc_map(RC_Channel_aux* rc_ch) { return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min; }