From f7ef60565e3e4d8ec5ec6378f072a54f76ea4492 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Wed, 13 Jun 2012 20:55:19 +0200 Subject: [PATCH] Add "3 axis camera stabilization" and "point camera to 3D point" functionality Patch by Gregory Fletcher and reviewed by me --- ArduPlane/ArduPlane.pde | 13 +- ArduPlane/Parameters.pde | 10 + libraries/AP_Mount/AP_Mount.cpp | 594 +++++++++++++----------- libraries/AP_Mount/AP_Mount.h | 200 ++++---- libraries/RC_Channel/RC_Channel_aux.cpp | 47 +- libraries/RC_Channel/RC_Channel_aux.h | 9 +- 6 files changed, 502 insertions(+), 371 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3046aa8178..52a2bbb6f5 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -258,11 +258,6 @@ AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0) AP_Relay relay; -// Camera/Antenna mount tracking and stabilisation stuff -// -------------------------------------- -#if MOUNT == ENABLED -AP_Mount camera_mount(g_gps, &ahrs); -#endif //////////////////////////////////////////////////////////////////////////////// @@ -637,6 +632,14 @@ static unsigned long dTnav; static float load; +// Camera/Antenna mount tracking and stabilisation stuff +// -------------------------------------- +#if MOUNT == ENABLED +// current_loc uses the baro/gps soloution for altitude rather than gps only. +// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? +AP_Mount camera_mount(¤t_loc, g_gps, &ahrs); +#endif + //////////////////////////////////////////////////////////////////////////////// // Top-level logic diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 6c1c2849fd..c09ae4b490 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -277,13 +277,23 @@ static const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(airspeed_use, "ARSPD_USE"), + // RC channel + //----------- GGROUP(channel_roll, "RC1_", RC_Channel), GGROUP(channel_pitch, "RC2_", RC_Channel), GGROUP(channel_throttle, "RC3_", RC_Channel), GGROUP(channel_rudder, "RC4_", RC_Channel), + // @Group: RC5_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp GGROUP(rc_5, "RC5_", RC_Channel_aux), + // @Group: RC6_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp GGROUP(rc_6, "RC6_", RC_Channel_aux), + // @Group: RC7_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp GGROUP(rc_7, "RC7_", RC_Channel_aux), + // @Group: RC8_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp GGROUP(rc_8, "RC8_", RC_Channel_aux), GGROUP(pidNavRoll, "HDNG2RLL_", PID), diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 9861395107..09fae3ca07 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,277 +1,333 @@ -// -*- 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_AHRS *ahrs) -{ - _ahrs=ahrs; - _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; -} - -//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 = _ahrs->get_dcm_matrix(); - m.transpose(); - //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 (_ahrs) - { - roll_angle = -_ahrs->roll_sensor; - pitch_angle = -_ahrs->pitch_sensor; - yaw_angle = -_ahrs->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 = _ahrs->get_dcm_matrix(); - m.transpose(); - 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; -} - +// -*- 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(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs): +_gps(gps) +{ + _ahrs = ahrs; + _current_loc = current_loc; + + //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; +} + +//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) + { + // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage + case MAV_MOUNT_MODE_RETRACT: + _roll_angle =100*_retract_angles.x; + _pitch_angle=100*_retract_angles.y; + _yaw_angle =100*_retract_angles.z; + break; + + // move mount to a neutral position, typically pointing forward + case MAV_MOUNT_MODE_NEUTRAL: + _roll_angle =100*_neutral_angles.x; + _pitch_angle=100*_neutral_angles.y; + _yaw_angle =100*_neutral_angles.z; + break; + + // point to the angles given by a mavlink message + 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 = _ahrs->get_dcm_matrix(); + m.transpose(); + //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; + } + + // RC radio manual angle control, but with stabilization from the AHRS + case MAV_MOUNT_MODE_RC_TARGETING: + { + G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle); + G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle); + G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle); + if (_ahrs){ + calculate(); + } else { + 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; + } + + // point mount to a GPS point given by the mission planner + case MAV_MOUNT_MODE_GPS_POINT: + { + if(_gps->fix){ + calc_GPS_target_angle(&_target_GPS_location); + } + if (_ahrs){ + calculate(); + } + break; + } + default: + //do nothing + break; + } + + // write the results to the servos +/* + G_RC_AUX(k_mount_roll)->angle_out(_roll_angle); + G_RC_AUX(k_mount_pitch)->angle_out(_pitch_angle); + G_RC_AUX(k_mount_yaw)->angle_out(_yaw_angle); +*/ + // 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; - } + __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); + __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; -} - +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_angle(struct Location *target) +{ + float _GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)/(t7*2.0)))*.01113195; + float _GPS_vector_y = (target->lat-_current_loc->lat)*.01113195; + float _GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter). + float target_distance = 100.0*sqrt(_GPS_vector_x*_GPS_vector_x + _GPS_vector_y*_GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters. + _roll_control_angle = 0; + _pitch_control_angle = atan2(_GPS_vector_z, target_distance); + _yaw_control_angle = atan2(_GPS_vector_x,_GPS_vector_y); + // Converts +/- 180 into 0-360. + if(_yaw_control_angle<0){ + _yaw_control_angle += 2*M_PI; + } +} + +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; + } +} + +void +AP_Mount::calculate() +{ + m = _ahrs->get_dcm_matrix(); + m.transpose(); + cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle); + gimbal_target = m * cam; + gimbal_target.to_euler(&_roll, &_pitch, &_yaw); + _roll_angle = degrees(_roll)*100; + _pitch_angle = degrees(_pitch)*100; + _yaw_angle = degrees(_yaw)*100; +} + +// 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; +} + +// For testing and development. Called in the medium loop. +void +AP_Mount::debug_output() +{ Serial3.print("current - "); + Serial3.print("lat "); + Serial3.print(_current_loc->lat); + Serial3.print(",lon "); + Serial3.print(_current_loc->lng); + Serial3.print(",alt "); + Serial3.println(_current_loc->alt); + + Serial3.print("gps - "); + Serial3.print("lat "); + Serial3.print(_gps->latitude); + Serial3.print(",lon "); + Serial3.print(_gps->longitude); + Serial3.print(",alt "); + Serial3.print(_gps->altitude); + Serial3.println(); + + Serial3.print("target - "); + Serial3.print("lat "); + Serial3.print(_target_GPS_location.lat); + Serial3.print(",lon "); + Serial3.print(_target_GPS_location.lng); + Serial3.print(",alt "); + Serial3.print(_target_GPS_location.alt); + Serial3.print(" hdg to targ "); + Serial3.print(degrees(_yaw_control_angle)); + Serial3.println(); +} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 2b64185608..a5d45cb1c4 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -1,95 +1,107 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/************************************************************ -* AP_mount -- library to control a 2 or 3 axis mount. * -* * -* Author: Joe Holdsworth; * -* Ritchie Wilson; * -* Amilcar Lucas; * -* * -* Purpose: Move a 2 or 3 axis mount attached to vehicle, * -* Used for mount to track targets or stabilise * -* camera plus other modes. * -* * -* Usage: Use in main code to control mounts attached to * -* vehicle. * -* * -*Comments: All angles in degrees * 100, distances in meters* -* unless otherwise stated. * - ************************************************************/ -#ifndef AP_Mount_H -#define AP_Mount_H - -#include -#include -#include -#include +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/************************************************************ +* AP_mount -- library to control a 2 or 3 axis mount. * +* * +* Author: Joe Holdsworth; * +* Ritchie Wilson; * +* Amilcar Lucas; * +* Gregory Fletcher; * +* * +* Purpose: Move a 2 or 3 axis mount attached to vehicle, * +* Used for mount to track targets or stabilise * +* camera plus other modes. * +* * +* Usage: Use in main code to control mounts attached to * +* vehicle. * +* * +*Comments: All angles in degrees * 100, distances in meters* +* unless otherwise stated. * + ************************************************************/ +#ifndef AP_Mount_H +#define AP_Mount_H + +#include +#include +#include +#include #include -#include -#include <../RC_Channel/RC_Channel_aux.h> - -class AP_Mount -{ -public: - //Constructors - AP_Mount(GPS *gps, AP_AHRS *ahrs); - - //enums - enum MountType{ - k_pan_tilt = 0, ///< yaw-pitch - k_tilt_roll = 1, ///< pitch-roll - k_pan_tilt_roll = 2, ///< yaw-pitch-roll - }; - - // MAVLink methods - void configure_msg(mavlink_message_t* msg); - void control_msg(mavlink_message_t* msg); - void status_msg(mavlink_message_t* msg); - void set_roi_cmd(); - void configure_cmd(); - void control_cmd(); - - // should be called periodically - void update_mount_position(); - void update_mount_type(); ///< Auto-detect the mount gimbal type depending on the functions assigned to the servos - - // Accessors - enum MountType get_mount_type(); - -private: - - //methods - void set_mode(enum MAV_MOUNT_MODE mode); - - void set_retract_angles(int roll, int pitch, int yaw); ///< set mount retracted position - void set_neutral_angles(int roll, int pitch, int yaw); - void set_mavlink_angles(int roll, int pitch, int yaw); - void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location - - // internal methods - void calc_GPS_target_vector(struct Location *target); - long rc_map(RC_Channel_aux* rc_ch); - - //members - AP_AHRS *_ahrs; - GPS *_gps; - - int roll_angle; ///< degrees*100 - int pitch_angle; ///< degrees*100 - int yaw_angle; ///< degrees*100 - - uint8_t _stab_roll; ///< (1 = yes, 0 = no) - uint8_t _stab_pitch; ///< (1 = yes, 0 = no) - uint8_t _stab_yaw; ///< (1 = yes, 0 = no) - - enum MAV_MOUNT_MODE _mount_mode; - MountType _mount_type; - - struct Location _target_GPS_location; - - Vector3i _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _mavlink_angles; ///< mavlink position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - Vector3f _GPS_vector; ///< target vector calculated stored in meters -}; -#endif +#include +#include <../RC_Channel/RC_Channel_aux.h> + +class AP_Mount +{ +public: + //Constructor + AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs); + + //enums + enum MountType{ + k_pan_tilt = 0, ///< yaw-pitch + k_tilt_roll = 1, ///< pitch-roll + k_pan_tilt_roll = 2, ///< yaw-pitch-roll + }; + + // MAVLink methods + void configure_msg(mavlink_message_t* msg); + void control_msg(mavlink_message_t* msg); + void status_msg(mavlink_message_t* msg); + void set_roi_cmd(); + void configure_cmd(); + void control_cmd(); + + // should be called periodically + void update_mount_position(); + void update_mount_type(); ///< Auto-detect the mount gimbal type depending on the functions assigned to the servos + void debug_output(); ///< For testing and development. Called in the medium loop. + // Accessors + enum MountType get_mount_type(); + +private: + + //methods + void set_mode(enum MAV_MOUNT_MODE mode); + + void set_retract_angles(int roll, int pitch, int yaw); ///< set mount retracted position + void set_neutral_angles(int roll, int pitch, int yaw); + void set_mavlink_angles(int roll, int pitch, int yaw); + void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location + + // internal methods + void calc_GPS_target_angle(struct Location *target); + void calculate(); + long rc_map(RC_Channel_aux* rc_ch); + + //members + Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs + Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input. + Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos. + AP_AHRS *_ahrs; ///< Rotation matrix from earth to plane. + GPS *&_gps; + const struct Location *_current_loc; + static const float t7 = 10000000.0; + float _roll_control_angle; + float _pitch_control_angle; + float _yaw_control_angle; + float _roll; + float _pitch; + float _yaw; + + int16_t _roll_angle; ///< degrees*100 + int16_t _pitch_angle; ///< degrees*100 + int16_t _yaw_angle; ///< degrees*100 + + uint8_t _stab_roll; ///< (1 = yes, 0 = no) + uint8_t _stab_pitch; ///< (1 = yes, 0 = no) + uint8_t _stab_yaw; ///< (1 = yes, 0 = no) + + enum MAV_MOUNT_MODE _mount_mode; + MountType _mount_type; + + struct Location _target_GPS_location; + + Vector3i _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _mavlink_angles; ///< mavlink position for mount, vector.x = roll vector.y = pitch, vector.z=yaw +}; +#endif diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 3a074c6a79..d70672d773 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -5,13 +5,33 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] PROGMEM = { AP_NESTEDGROUPINFO(RC_Channel, 0), + // @Param: FUNCTION + // @DisplayName: Function assigned to this APM servo output + // @Description: Setting this to Disabled(0) will disable this output, any other value will enable the corresponding function + // @Values: 0:Disabled,1:Manual,2:Flap,3:Flap_auto,4:Aileron,5:flaperon,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release + // @User: Standard AP_GROUPINFO("FUNCTION", 1, RC_Channel_aux, function), + // @Param: ANGLE_MIN + // @DisplayName: Minimum physical angular position of the object that this servo output controls + // @Description: Minimum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc + // @Units: Degrees + // @Range: -180 180 + // @Increment: .01 + // @User: Standard AP_GROUPINFO("ANGLE_MIN", 2, RC_Channel_aux, angle_min), + // @Param: ANGLE_MAX + // @DisplayName: Maximum physical angular position of the object that this servo output controls + // @Description: Maximum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc + // @Units: Degrees + // @Range: -180 180 + // @Increment: .01 + // @User: Standard AP_GROUPINFO("ANGLE_MAX", 3, RC_Channel_aux, angle_max), AP_GROUPEND }; -RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function +// Global pointer array, indexed by a "RC function enum" and points to the RC channel output assigned to that function/operation +RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; int16_t RC_Channel_aux::closest_limit(int16_t angle) @@ -49,6 +69,31 @@ RC_Channel_aux::closest_limit(int16_t angle) return angle; } +// Gets the RC and integrates and then compares with the servo out angles to limit control input to servo travel. +// That way the user doesn't get lost. Rotationally. +void +RC_Channel_aux::rc_input(float *control_angle, int16_t angle) +{ + if((radio_in < 1480 && angle < angle_max)||(radio_in > 1520 && angle > angle_min)){ + *control_angle += ( 1500 - radio_in ) * .0001; // .0001 is the control speed scaler. + } +} + +// Takes the desired servo angle(deg) and converts to microSeconds for PWM +// Like this: 45 deg = 2000 us ; -45 deg/1000 us. 1000us/(90*100 deg) = 0.1111111111111 +void +RC_Channel_aux::angle_out(int16_t angle) +{ + if(angle >= angle_max){ + angle = angle_max; + } + if(angle <= angle_min){ + angle = angle_min; + } + // Convert the angle*100 to pwm microseconds. 45 deg = 500 us. + radio_out = (/*_reverse * */ angle * 0.1111111) + 1500; +} + // map a function to a servo channel and output it void RC_Channel_aux::output_ch(unsigned char ch_nr) diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 4db81485ef..879fd041e9 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -1,8 +1,9 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file RC_Channel_aux.h -/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants. +/// @brief RC_Channel manager for auxiliary channels (5..8), with EEPROM-backed storage of constants. /// @author Amilcar Lucas +/// @author Gregory Fletcher #ifndef RC_CHANNEL_AUX_H_ #define RC_CHANNEL_AUX_H_ @@ -50,9 +51,13 @@ public: int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval + void angle_out(int16_t angle); + + void rc_input(float *control_angle, int16_t angle); + void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it - static const struct AP_Param::GroupInfo var_info[]; + static const struct AP_Param::GroupInfo var_info[]; }; void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8);