diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 21711093c1..90eb3711fb 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1262,31 +1262,8 @@ static void fifty_hz_loop() #endif #if MOUNT == ENABLED - static bool mount_initialised = false; - - // do camera mount initialisation. TO-DO: move to AP_Mount library or init_ardupilot - if( mount_initialised == false ) { - mount_initialised = true; - - // set-up channel 6 to control pitch - camera_mount.set_manual_rc_channel( &g.rc_6 ); - - } - - // update pilot's commands to mount - if( g.radio_tuning == 0 ) { - camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_PITCH ); - }else{ - camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED ); - } - // update camera mount's position camera_mount.update_mount_position(); - - // move camera servos. TO-DO: move this to AP_Mount library - g.rc_camera_roll.output(); - g.rc_camera_pitch.output(); - g.rc_camera_yaw.output(); #endif #if CAMERA == ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5ec3245e18..a3e5d64e59 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -47,23 +47,109 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll), - // @Param: STAB_PITCH - // @DisplayName: Stabilize mount pitch - // @Description: enable pitch/tilt stabilisation relative to Earth + // @Param: STAB_TILT + // @DisplayName: Stabilize mount tilt + // @Description: enable tilt (pitch) stabilisation relative to Earth // @Values: 0:Disabled,1:Enabled // @User: Standard - AP_GROUPINFO("STAB_PITCH", 5, AP_Mount, _stab_pitch), + AP_GROUPINFO("STAB_TILT", 5, AP_Mount, _stab_tilt), - // @Param: STAB_YAW - // @DisplayName: Stabilize mount yaw - // @Description: enable yaw/pan stabilisation relative to Earth + // @Param: STAB_PAN + // @DisplayName: Stabilize mount pan + // @Description: enable pan (yaw) stabilisation relative to Earth // @Values: 0:Disabled,1:Enabled // @User: Standard - AP_GROUPINFO("STAB_YAW", 6, AP_Mount, _stab_yaw), - AP_GROUPEND + AP_GROUPINFO("STAB_PAN", 6, AP_Mount, _stab_pan), + + // @Param: ROLL_RC_IN + // @DisplayName: roll RC input channel + // @Description: 0 for none, any other for the RC channel to be used to control roll movements + // @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8 + // @User: Standard + AP_GROUPINFO("ROLL_RC_IN", 7, AP_Mount, _roll_rc_in), + + // @Param: ROLL_ANGLE_MIN + // @DisplayName: Minimum roll angle + // @Description: Minimum physical roll angular position of mount. + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ROLL_ANGMIN", 8, AP_Mount, _roll_angle_min), + + // @Param: ROLL_ANGLE_MAX + // @DisplayName: Maximum roll angle + // @Description: Maximum physical roll angular position of the mount + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ROLL_ANGMAX", 9, AP_Mount, _roll_angle_max), + + // @Param: TILT_RC_IN + // @DisplayName: tilt (pitch) RC input channel + // @Description: 0 for none, any other for the RC channel to be used to control tilt (pitch) movements + // @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8 + // @User: Standard + AP_GROUPINFO("TILT_RC_IN", 10, AP_Mount, _tilt_rc_in), + + // @Param: TILT_ANGLE_MIN + // @DisplayName: Minimum tilt angle + // @Description: Minimum physical tilt (pitch) angular position of mount. + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("TILT_ANGMIN", 11, AP_Mount, _tilt_angle_min), + + // @Param: TILT_ANGLE_MAX + // @DisplayName: Maximum tilt angle + // @Description: Maximum physical tilt (pitch) angular position of the mount + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("TILT_ANGMAX", 12, AP_Mount, _tilt_angle_max), + + // @Param: PAN_RC_IN + // @DisplayName: pan (yaw) RC input channel + // @Description: 0 for none, any other for the RC channel to be used to control pan (yaw) movements + // @Values: 0:Disabled,5:RC5,6:RC6,7:RC7,8:RC8 + // @User: Standard + AP_GROUPINFO("PAN_RC_IN", 13, AP_Mount, _pan_rc_in), + + // @Param: PAN_ANGLE_MIN + // @DisplayName: Minimum pan angle + // @Description: Minimum physical pan (yaw) angular position of mount. + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("PAN_ANGMIN", 14, AP_Mount, _pan_angle_min), + + // @Param: PAN_ANGLE_MAX + // @DisplayName: Maximum pan angle + // @Description: Maximum physical pan (yaw) angular position of the mount + // @Units: centi-Degrees + // @Range: -18000 17999 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("PAN_ANGMAX", 15, AP_Mount, _pan_angle_max), +/* + Must be commented out because the framework does not support more than 16 parameters + // @Param: JOYSTICK_SPEED + // @DisplayName: mount joystick speed + // @Description: 0 for position control, small for low speeds, 10 for max speed + // @Range: 0 10 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("JOYSTICK_SPEED", 16, AP_Mount, _joystick_speed), +*/ + AP_GROUPEND }; extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function +extern RC_Channel* rc_ch[NUM_CHANNELS]; AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs): _gps(gps) @@ -79,56 +165,65 @@ _gps(gps) _neutral_angles = Vector3f(0,0,0); _control_angles = Vector3f(0,0,0); - // default mount type to roll/pitch (which is the most common for copter) - _mount_type = k_tilt_roll; + // default unknown mount type + _mount_type = k_unknown; // default manual rc channel to disabled - _manual_rc = NULL; - _manual_rc_function = AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED; + _pan_rc_in = 0; + _tilt_rc_in= 0; + _roll_rc_in= 0; + + _pan_angle_min = -4500; // assume -45 degrees min deflection + _pan_angle_max = 4500; // assume 45 degrees max deflection + _tilt_angle_min = -4500; // assume -45 degrees min deflection + _tilt_angle_max = 4500; // assume 45 degrees max deflection + _roll_angle_min = -4500; // assume -45 degrees min deflection + _roll_angle_max = 4500; // assume 45 degrees max deflection } -// Auto-detect the mount gimbal type depending on the functions assigned to the servos -void AP_Mount::update_mount_type() +/// Auto-detect the mount gimbal type depending on the functions assigned to the servos +void +AP_Mount::update_mount_type() { - 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)) + if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != 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)) + if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] == 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)) + if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != NULL)) { _mount_type = k_pan_tilt_roll; } } -//sets the servo angles for retraction, note angles are in degrees -void AP_Mount::set_retract_angles(float roll, float pitch, float yaw) +/// sets the servo angles for retraction, note angles are in degrees +void AP_Mount::set_retract_angles(float roll, float tilt, float pan) { - _retract_angles = Vector3f(roll, pitch, yaw); + _retract_angles = Vector3f(roll, tilt, pan); } //sets the servo angles for neutral, note angles are in degrees -void AP_Mount::set_neutral_angles(float roll, float pitch, float yaw) +void AP_Mount::set_neutral_angles(float roll, float tilt, float pan) { - _neutral_angles = Vector3f(roll, pitch, yaw); + _neutral_angles = Vector3f(roll, tilt, pan); } -//sets the servo angles for MAVLink, note angles are in degrees -void AP_Mount::set_control_angles(float roll, float pitch, float yaw) +/// sets the servo angles for MAVLink, note angles are in degrees +void AP_Mount::set_control_angles(float roll, float tilt, float pan) { - _control_angles = Vector3f(roll, pitch, yaw); + _control_angles = Vector3f(roll, tilt, pan); } -// used to tell the mount to track GPS location +/// 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 +/// This one should be called periodically void AP_Mount::update_mount_position() { switch((enum MAV_MOUNT_MODE)_mount_mode.get()) @@ -138,8 +233,8 @@ void AP_Mount::update_mount_position() { Vector3f vec = _retract_angles.get(); _roll_angle = vec.x; - _pitch_angle = vec.y; - _yaw_angle = vec.z; + _tilt_angle = vec.y; + _pan_angle = vec.z; break; } @@ -148,8 +243,8 @@ void AP_Mount::update_mount_position() { Vector3f vec = _neutral_angles.get(); _roll_angle = vec.x; - _pitch_angle = vec.y; - _yaw_angle = vec.z; + _tilt_angle = vec.y; + _pan_angle = vec.z; break; } @@ -158,8 +253,8 @@ void AP_Mount::update_mount_position() { Vector3f vec = _control_angles.get(); _roll_control_angle = radians(vec.x); - _pitch_control_angle = radians(vec.y); - _yaw_control_angle = radians(vec.z); + _tilt_control_angle = radians(vec.y); + _pan_control_angle = radians(vec.z); stabilize(); break; } @@ -167,40 +262,38 @@ void AP_Mount::update_mount_position() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { -//#define MNT_SPRING_LOADED_JOYSTICK -#ifdef MNT_SPRING_LOADED_JOYSTICK - // rc_input() takes degrees * 100 units - G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100); - G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100); - G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100); -#else - // allow pilot input to come directly from an RC_Channel - if( _manual_rc != NULL ) { - float manual_angle = radians((float)(4500 - _manual_rc->control_in ) / 100.0); - switch( _manual_rc_function ) { - case AP_MOUNT_MANUAL_RC_FUNCTION_ROLL: - _roll_control_angle = manual_angle; - break; - case AP_MOUNT_MANUAL_RC_FUNCTION_PITCH: - _pitch_control_angle = manual_angle; - break; - case AP_MOUNT_MANUAL_RC_FUNCTION_YAW: - _yaw_control_angle = manual_angle; - break; - default: - // do nothing - break; +/* if (_joystick_speed) { // for spring loaded joysticks + // allow pilot speed position input to come directly from an RC_Channel + if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { + //_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed; + _roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; + if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01); + if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01); } - }else{ - // take pilot's input from RC_Channel_aux - if (g_rc_function[RC_Channel_aux::k_mount_roll]) - _roll_control_angle = g_rc_function[RC_Channel_aux::k_mount_roll]->angle_input_rad(); - if (g_rc_function[RC_Channel_aux::k_mount_pitch]) - _pitch_control_angle = g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_input_rad(); - if (g_rc_function[RC_Channel_aux::k_mount_yaw]) - _yaw_control_angle = g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_input_rad(); - } -#endif + if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { + //_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed; + _tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; + if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01); + if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01); + } + if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { + //_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed; + _pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed; + if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01); + if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01); + } + } else { +*/ // allow pilot position input to come directly from an RC_Channel + if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) { + _roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max); + } + if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) { + _tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max); + } + if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) { + _pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max); + } +// } stabilize(); break; } @@ -220,10 +313,9 @@ void AP_Mount::update_mount_position() } // write the results to the servos - // closest_limit() takes degrees * 10 units - 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); + move_servo(g_rc_function[RC_Channel_aux::k_mount_roll], _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); + move_servo(g_rc_function[RC_Channel_aux::k_mount_tilt], _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); + move_servo(g_rc_function[RC_Channel_aux::k_mount_pan ], _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); } void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) @@ -231,8 +323,8 @@ void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) _mount_mode = (int8_t)mode; } -// Change the configuration of the mount -// triggered by a MavLink packet. +/// Change the configuration of the mount +/// triggered by a MavLink packet. void AP_Mount::configure_msg(mavlink_message_t* msg) { __mavlink_mount_configure_t packet; @@ -242,13 +334,13 @@ void AP_Mount::configure_msg(mavlink_message_t* msg) 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; + _stab_tilt = packet.stab_pitch; + _stab_pan = packet.stab_yaw; } -// Control the mount (depends on the previously set mount configuration) -// triggered by a MavLink packet. +/// Control the mount (depends on the previously set mount configuration) +/// triggered by a MavLink packet. void AP_Mount::control_msg(mavlink_message_t *msg) { __mavlink_mount_control_t packet; @@ -284,8 +376,8 @@ void AP_Mount::control_msg(mavlink_message_t *msg) { Vector3f vec = _neutral_angles.get(); _roll_angle = vec.x; - _pitch_angle = vec.y; - _yaw_angle = vec.z; + _tilt_angle = vec.y; + _pan_angle = vec.z; } break; @@ -306,8 +398,8 @@ void AP_Mount::control_msg(mavlink_message_t *msg) } } -// Return mount status information (depends on the previously set mount configuration) -// triggered by a MavLink packet. +/// Return mount status information (depends on the previously set mount configuration) +/// triggered by a MavLink packet. void AP_Mount::status_msg(mavlink_message_t *msg) { __mavlink_mount_status_t packet; @@ -324,8 +416,8 @@ void AP_Mount::status_msg(mavlink_message_t *msg) 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*100; ///< degrees*100 - packet.pointing_a = _pitch_angle*100; ///< degrees*100 - packet.pointing_c = _yaw_angle*100; ///< degrees*100 + packet.pointing_a = _tilt_angle*100; ///< degrees*100 + packet.pointing_c = _pan_angle*100; ///< 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 @@ -342,7 +434,7 @@ void AP_Mount::status_msg(mavlink_message_t *msg) packet.pointing_a, packet.pointing_b, packet.pointing_c); } -// Set mount point/region of interest, triggered by mission script commands +/// Set mount point/region of interest, triggered by mission script commands void AP_Mount::set_roi_cmd(struct Location *target_loc) { // set the target gps location @@ -352,105 +444,91 @@ void AP_Mount::set_roi_cmd(struct Location *target_loc) set_mode(MAV_MOUNT_MODE_GPS_POINT); } -// Set mount configuration, triggered by mission script commands +/// Set mount configuration, triggered by mission script commands void AP_Mount::configure_cmd() { // TODO get the information out of the mission command and use it } -// Control the mount (depends on the previously set mount configuration), triggered by mission script commands +/// Control the mount (depends on the previously set mount configuration), triggered by mission script commands void AP_Mount::control_cmd() { // TODO get the information out of the mission command and use it } +/// returns the angle (degrees*100) that the RC_Channel input is receiving +int32_t +AP_Mount::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return (rc->get_reverse()?-1:1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse()?angle_max:angle_min); +} + +/// returns the angle (radians) that the RC_Channel input is receiving +float +AP_Mount::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return radians(angle_input(rc, angle_min, angle_max)*0.01); +} + 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_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.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; - } -*/ + _roll_control_angle = 0; + _tilt_control_angle = atan2(GPS_vector_z, target_distance); + _pan_control_angle = atan2(GPS_vector_x, GPS_vector_y); } /// Stabilizes mount relative to the Earth's frame /// Inputs: /// _roll_control_angle desired roll angle in radians, -/// _pitch_control_angle desired pitch/tilt angle in radians, -/// _yaw_control_angle desired yaw/pan angle in radians +/// _tilt_control_angle desired tilt/pitch angle in radians, +/// _pan_control_angle desired pan/yaw angle in radians /// Outputs: /// _roll_angle stabilized roll angle in degrees, -/// _pitch_angle stabilized pitch/tilt angle in degrees, -/// _yaw_angle stabilized yaw/pan angle in degrees +/// _tilt_angle stabilized tilt/pitch angle in degrees, +/// _pan_angle stabilized pan/yaw angle in degrees void AP_Mount::stabilize() { if (_ahrs) { - // only do the full 3D frame transform if we are doing yaw control - if (_stab_yaw) { + // only do the full 3D frame transform if we are doing pan control + if (_stab_pan) { 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. m = _ahrs->get_dcm_matrix(); m.transpose(); - cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle); + cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle); gimbal_target = m * cam; - gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle); + gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle); _roll_angle = _stab_roll?degrees(_roll_angle):degrees(_roll_control_angle); - _pitch_angle = _stab_pitch?degrees(_pitch_angle):degrees(_pitch_control_angle); - _yaw_angle = degrees(_yaw_angle); + _tilt_angle = _stab_tilt?degrees(_tilt_angle):degrees(_tilt_control_angle); + _pan_angle = degrees(_pan_angle); } else { - // otherwise base mount roll and pitch on the ahrs - // roll/pitch attitude, plus any requested angle + // otherwise base mount roll and tilt on the ahrs + // roll/tilt attitude, plus any requested angle _roll_angle = degrees(_roll_control_angle); - _pitch_angle = degrees(_pitch_control_angle); - _yaw_angle = degrees(_yaw_control_angle); + _tilt_angle = degrees(_tilt_control_angle); + _pan_angle = degrees(_pan_control_angle); if (_stab_roll) { _roll_angle -= degrees(_ahrs->roll); } - if (_stab_pitch) { - _pitch_angle -= degrees(_ahrs->pitch); + if (_stab_tilt) { + _tilt_angle -= degrees(_ahrs->pitch); } } } else { _roll_angle = degrees(_roll_control_angle); - _pitch_angle = degrees(_pitch_control_angle); - _yaw_angle = degrees(_yaw_control_angle); + _tilt_angle = degrees(_tilt_control_angle); + _pan_angle = degrees(_pan_control_angle); } } - -// set_manual_rc_channel - define which RC_Channel is to be used for manual control -void -AP_Mount::set_manual_rc_channel(RC_Channel* rc) -{ - _manual_rc = rc; -} - -// set_manual_rc_channel_function - set whether manual rc channel is disabled, controls roll (1), pitch (2) or yaw (3). -void -AP_Mount::set_manual_rc_channel_function(int8_t fn) -{ - // update scaler if the function has changed - if( _manual_rc_function != fn ) { - _manual_rc_function = fn; - - // ensure pilot's input appears in the range 0 ~ 9000 ( 90 degrees * 100 ). We will subtract 45 degrees later to make the range +-45 degrees - if( _manual_rc != NULL && fn != AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED ) { - _manual_rc->set_range( 0, 9000 ); - } - } -} - -// For testing and development. Called in the medium loop. +/* +/// For testing and development. Called in the medium loop. void AP_Mount::debug_output() { Serial3.print("current - "); @@ -478,6 +556,51 @@ AP_Mount::debug_output() Serial3.print(",alt "); Serial3.print(_target_GPS_location.alt); Serial3.print(" hdg to targ "); - Serial3.print(degrees(_yaw_control_angle)); + Serial3.print(degrees(_pan_control_angle)); Serial3.println(); } +*/ +/// saturate to the closest angle limit if outside of [min max] angle interval +/// input angle is in degrees * 10 +int16_t +AP_Mount::closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max) +{ + // Make sure the angle lies in the interval [-180 .. 180[ degrees + while (angle < -1800) angle += 3600; + while (angle >= 1800) angle -= 3600; + + // Make sure the angle limits lie in the interval [-180 .. 180[ degrees + while (*angle_min < -1800) *angle_min += 3600; + while (*angle_min >= 1800) *angle_min -= 3600; + while (*angle_max < -1800) *angle_max += 3600; + while (*angle_max >= 1800) *angle_max -= 3600; + // TODO call this function somehow, otherwise this will never work + //set_range(min, max); + + // If the angle is outside servo limits, saturate the angle to the closest limit + // On a circle the closest angular position must be carefully calculated to account for wrap-around + if ((angle < *angle_min) && (angle > *angle_max)){ + // angle error if min limit is used + int16_t err_min = *angle_min - angle + (angle<*angle_min?0:3600); // add 360 degrees if on the "wrong side" + // angle error if max limit is used + int16_t err_max = angle - *angle_max + (angle>*angle_max?0:3600); // add 360 degrees if on the "wrong side" + angle = err_minservo_out = closest_limit(angle, &angle_min, &angle_max); + // This is done every time because the user might change the min, max values on the fly + rc->set_range(angle_min, angle_max); + // convert angle to PWM using a linear transformation (ignores trimming because the servo limits might not be symmetric) + rc->calc_pwm(); + rc->output(); + } +} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 2fe9abd057..96ceaa3c1f 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -29,12 +29,6 @@ #include #include <../RC_Channel/RC_Channel_aux.h> -// #defines to control function of RC Channel used to manually provide angular offset to AP_Mount when we can't use RC_Channel_aux (which is the case for ArduCopter). -#define AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED 0 -#define AP_MOUNT_MANUAL_RC_FUNCTION_ROLL 1 -#define AP_MOUNT_MANUAL_RC_FUNCTION_PITCH 2 -#define AP_MOUNT_MANUAL_RC_FUNCTION_YAW 3 - class AP_Mount { public: @@ -43,9 +37,10 @@ public: //enums enum MountType{ - k_pan_tilt = 0, ///< yaw-pitch - k_tilt_roll = 1, ///< pitch-roll - k_pan_tilt_roll = 2, ///< yaw-pitch-roll + k_unknown = 0, ///< unknown type + k_pan_tilt = 1, ///< yaw-pitch + k_tilt_roll = 2, ///< pitch-roll + k_pan_tilt_roll = 3, ///< yaw-pitch-roll }; // MAVLink methods @@ -62,11 +57,6 @@ public: void debug_output(); ///< For testing and development. Called in the medium loop. // Accessors enum MountType get_mount_type() { return _mount_type; } - - // to allow manual input of an angle from the pilot when RC_Channel_aux cannot be used - void set_manual_rc_channel(RC_Channel *rc); // define which RC_Channel is to be used for manual control - void set_manual_rc_channel_function(int8_t fn); // set whether manual rc channel controlls roll (1), pitch (2) or yaw (3). - // hook for eeprom variables static const struct AP_Param::GroupInfo var_info[]; @@ -75,43 +65,56 @@ private: //methods void set_mode(enum MAV_MOUNT_MODE mode); - void set_retract_angles(float roll, float pitch, float yaw); ///< set mount retracted position - void set_neutral_angles(float roll, float pitch, float yaw); - void set_control_angles(float roll, float pitch, float yaw); + void set_retract_angles(float roll, float tilt, float pan); ///< set mount retracted position + void set_neutral_angles(float roll, float tilt, float pan); + void set_control_angles(float roll, float tilt, float pan); 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 stabilize(); + int16_t closest_limit(int16_t angle, int16_t* angle_min, int16_t* angle_max); + void move_servo(RC_Channel* rc, int16_t angle, int16_t angle_min, int16_t angle_max); + int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max); + float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max); //members 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; ///< radians - float _pitch_control_angle; ///< radians - float _yaw_control_angle; ///< radians - - float _roll_angle; ///< degrees - float _pitch_angle; ///< degrees - float _yaw_angle; ///< degrees - - AP_Int8 _stab_roll; ///< (1 = yes, 0 = no) - AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no) - AP_Int8 _stab_yaw; ///< (1 = yes, 0 = no) - - AP_Int8 _mount_mode; + struct Location _target_GPS_location; MountType _mount_type; - struct Location _target_GPS_location; + float _roll_control_angle; ///< radians + float _tilt_control_angle; ///< radians + float _pan_control_angle; ///< radians - AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw - AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + float _roll_angle; ///< degrees + float _tilt_angle; ///< degrees + float _pan_angle; ///< degrees + // EEPROM parameters + AP_Int8 _stab_roll; ///< (1 = yes, 0 = no) + AP_Int8 _stab_tilt; ///< (1 = yes, 0 = no) + AP_Int8 _stab_pan; ///< (1 = yes, 0 = no) + + AP_Int8 _mount_mode; // RC_Channel for providing direct angular input from pilot - RC_Channel* _manual_rc; - int8_t _manual_rc_function; + AP_Int8 _roll_rc_in; + AP_Int8 _tilt_rc_in; + AP_Int8 _pan_rc_in; + + AP_Int16 _roll_angle_min; ///< min angle limit of actuated surface in 0.01 degree units + AP_Int16 _roll_angle_max; ///< max angle limit of actuated surface in 0.01 degree units + AP_Int16 _tilt_angle_min; ///< min angle limit of actuated surface in 0.01 degree units + AP_Int16 _tilt_angle_max; ///< max angle limit of actuated surface in 0.01 degree units + AP_Int16 _pan_angle_min; ///< min angle limit of actuated surface in 0.01 degree units + AP_Int16 _pan_angle_max; ///< max angle limit of actuated surface in 0.01 degree units + + //AP_Int8 _joystick_speed; + + AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = tilt, vector.z=pan + AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = tilt, vector.z=pan + AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = tilt, vector.z=pan }; #endif diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 5462d05c53..0502d81c32 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -9,95 +9,16 @@ const AP_Param::GroupInfo RC_Channel_aux::var_info[] PROGMEM = { // @Param: FUNCTION // @DisplayName: Servo out function // @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_pitch,8:mount_roll,9:mount_open,10:camera_trigger,11:release + // @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 object position - // @Description: Minimum physical angular position of the object that this servo output controls. For example a camera pan angle, an aileron angle, etc - // @Units: centi-Degrees - // @Range: -18000 17999 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("ANGLE_MIN", 2, RC_Channel_aux, angle_min), - - // @Param: ANGLE_MAX - // @DisplayName: Maximum object position - // @Description: Maximum physical angular position of the object that this servo output controls. For example a camera pan angle, an aileron angle, etc - // @Units: centi-Degrees - // @Range: -18000 17999 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("ANGLE_MAX", 3, RC_Channel_aux, angle_max), AP_GROUPEND }; /// 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]; -/// saturate to the closest angle limit if outside of [min max] angle interval -/// input angle is in degrees * 10 -int16_t -RC_Channel_aux::closest_limit(int16_t angle) -{ - // Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic - int16_t min = angle_min / 10; - int16_t max = angle_max / 10; - - // Make sure the angle lies in the interval [-180 .. 180[ degrees - while (angle < -1800) angle += 3600; - while (angle >= 1800) angle -= 3600; - - // Make sure the angle limits lie in the interval [-180 .. 180[ degrees - while (min < -1800) min += 3600; - while (min >= 1800) min -= 3600; - while (max < -1800) max += 3600; - while (max >= 1800) max -= 3600; - // This is done every time because the user might change the min, max values on the fly - set_range(min, max); - - // If the angle is outside servo limits, saturate the angle to the closest limit - // On a circle the closest angular position must be carefully calculated to account for wrap-around - if ((angle < min) && (angle > max)){ - // angle error if min limit is used - int16_t err_min = min - angle + (anglemax?0:3600); // add 360 degrees if on the "wrong side" - angle = err_min 1520 && angle > angle_min)){ - *control_angle += ( 1500 - radio_in ) * .0001; // .0001 is the control speed scaler. - } -} - -/// returns the angle (degrees*100) that the RC_Channel input is receiving -int32_t -RC_Channel_aux::angle_input() -{ - return (get_reverse()?-1:1) * (radio_in - radio_min) * (int32_t)(angle_max - angle_min) / (radio_max - radio_min) + (get_reverse()?angle_max:angle_min); -} - -/// returns the angle (radians) that the RC_Channel input is receiving -float -RC_Channel_aux::angle_input_rad() -{ - return radians(angle_input()*0.01); -} - /// enable_out_ch - enable the channel through APM_RC void RC_Channel_aux::enable_out_ch(unsigned char ch_nr) @@ -131,6 +52,7 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) /// expects the changes to take effect instantly /// Supports up to seven aux servo outputs (typically CH5 ... CH11) /// All servos must be configured with a single call to this function +/// (do not call this twice with different parameters, the second call will reset the effect of the first call) void update_aux_servo_function( RC_Channel_aux* rc_a, RC_Channel_aux* rc_b, RC_Channel_aux* rc_c, @@ -175,12 +97,13 @@ void update_aux_servo_function( RC_Channel_aux* rc_a, G_RC_AUX(k_flap_auto)->set_range(0,100); G_RC_AUX(k_aileron)->set_angle(4500); G_RC_AUX(k_flaperon)->set_range(0,100); - G_RC_AUX(k_mount_yaw)->set_range( - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10); - G_RC_AUX(k_mount_pitch)->set_range( - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10, - g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10); +/* + G_RC_AUX(k_mount_pan)->set_range( + g_rc_function[RC_Channel_aux::k_mount_pan]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_pan]->angle_max / 10); + G_RC_AUX(k_mount_tilt)->set_range( + g_rc_function[RC_Channel_aux::k_mount_tilt]->angle_min / 10, + g_rc_function[RC_Channel_aux::k_mount_tilt]->angle_max / 10); G_RC_AUX(k_mount_roll)->set_range( g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10, g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10); @@ -188,6 +111,7 @@ void update_aux_servo_function( RC_Channel_aux* rc_a, G_RC_AUX(k_cam_trigger)->set_range( g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_min / 10, g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10); +*/ G_RC_AUX(k_egg_drop)->set_range(0,100); } diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 5cf8eb8123..a32927780f 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -3,7 +3,6 @@ /// @file RC_Channel_aux.h /// @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_ @@ -25,9 +24,7 @@ public: /// RC_Channel_aux(uint8_t ch_out) : RC_Channel(ch_out), - function (0), - angle_min (-4500), // assume -45 degrees min deflection - angle_max (4500) // assume 45 degrees max deflection + function (0) {} typedef enum @@ -38,8 +35,8 @@ public: k_flap_auto = 3, ///< flap automated k_aileron = 4, ///< aileron k_flaperon = 5, ///< flaperon (flaps and aileron combined, needs two independent servos one for each wing) - k_mount_yaw = 6, ///< mount yaw (pan) - k_mount_pitch = 7, ///< mount pitch (tilt) + k_mount_pan = 6, ///< mount yaw (pan) + k_mount_tilt = 7, ///< mount pitch (tilt) k_mount_roll = 8, ///< mount roll k_mount_open = 9, ///< mount open (deploy) / close (retract) k_cam_trigger = 10, ///< camera trigger @@ -48,16 +45,6 @@ public: } Aux_servo_function_t; AP_Int8 function; ///< see Aux_servo_function_t enum - AP_Int16 angle_min; ///< min angle limit of actuated surface in 0.01 degree units - AP_Int16 angle_max; ///< max angle limit of actuated surface in 0.01 degree units - - int16_t closest_limit(int16_t angle); - - void rc_input(float *control_angle, int16_t angle); - - int32_t angle_input(); - - float angle_input_rad(); void enable_out_ch(unsigned char ch_nr);