From 99b11e4f19c130c46871c296c31a5d97a30d6a54 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 3 Jul 2012 10:21:01 +1000 Subject: [PATCH] Mount: enable mount control via eeprom parameters this enables MNT_* parameter control of the camera mount code. It also fixes the conversion of calculated angles between degrees and integers, and fixes stabilised mount control when yaw control is not available. --- ArduPlane/ArduPlane.pde | 4 - ArduPlane/Parameters.h | 5 +- ArduPlane/Parameters.pde | 7 ++ libraries/AP_Mount/AP_Mount.cpp | 177 +++++++++++++++++--------------- libraries/AP_Mount/AP_Mount.h | 32 +++--- 5 files changed, 121 insertions(+), 104 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 656d0b03a0..7852434b72 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -918,10 +918,6 @@ static void slow_loop() update_servo_switches(); update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - -#if MOUNT == ENABLED - camera_mount.update_mount_type(); -#endif break; case 2: diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c0ecd278f2..2f17c3eec4 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -124,9 +124,10 @@ public: // - // Camera parameters + // Camera and mount parameters // - k_param_camera, + k_param_camera = 159, + k_param_camera_mount, // // 170: Radio settings diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index ed74a74c79..4c8f41a7cf 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -328,6 +328,13 @@ static const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), +#if MOUNT == ENABLED + // @Group: MNT_ + // @Path: ../libraries/AP_Mount/AP_Mount.cpp + GOBJECT(camera_mount, "MNT_", AP_Mount), +#endif + + #ifdef DESKTOP_BUILD // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 313cb692b8..d0f01683ef 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,7 +1,22 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +#include +#include +#include #include +const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { + // index 0 was used for the old orientation matrix + AP_GROUPINFO("MODE", 0, AP_Mount, _mount_mode), + AP_GROUPINFO("RETRACT", 1, AP_Mount, _retract_angles), + AP_GROUPINFO("NEUTRAL", 2, AP_Mount, _neutral_angles), + AP_GROUPINFO("CONTROL", 3, AP_Mount, _control_angles), + AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll), + AP_GROUPINFO("STAB_PITCH", 5, AP_Mount, _stab_pitch), + AP_GROUPINFO("STAB_YAW", 6, AP_Mount, _stab_yaw), + 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 AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs): @@ -10,37 +25,31 @@ _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 + // startup with the mount retracted + set_mode(MAV_MOUNT_MODE_RETRACT); - _retract_angles.x=0; - _retract_angles.y=0; - _retract_angles.z=0; + // default to zero angles + _retract_angles = Vector3f(0,0,0); + _neutral_angles = Vector3f(0,0,0); + _control_angles = Vector3f(0,0,0); } -//sets the servo angles for retraction, note angles are * 100 -void AP_Mount::set_retract_angles(int roll, int pitch, int yaw) +//sets the servo angles for retraction, note angles are in degrees +void AP_Mount::set_retract_angles(float roll, float pitch, float yaw) { - _retract_angles.x=roll; - _retract_angles.y=pitch; - _retract_angles.z=yaw; + _retract_angles = Vector3f(roll, pitch, yaw); } -//sets the servo angles for neutral, note angles are * 100 -void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw) +//sets the servo angles for neutral, note angles are in degrees +void AP_Mount::set_neutral_angles(float roll, float pitch, float yaw) { - _neutral_angles.x=roll; - _neutral_angles.y=pitch; - _neutral_angles.z=yaw; + _neutral_angles = Vector3f(roll, pitch, yaw); } -//sets the servo angles for MAVLink, note angles are * 100 -void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw) +//sets the servo angles for MAVLink, note angles are in degrees +void AP_Mount::set_control_angles(float roll, float pitch, float yaw) { - _mavlink_angles.x = roll; - _mavlink_angles.y = pitch; - _mavlink_angles.z = yaw; + _control_angles = Vector3f(roll, pitch, yaw); } // used to tell the mount to track GPS location @@ -52,28 +61,35 @@ void AP_Mount::set_GPS_target_location(Location targetGPSLocation) // This one should be called periodically void AP_Mount::update_mount_position() { - switch(_mount_mode) + switch((enum MAV_MOUNT_MODE)_mount_mode.get()) { // 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; + { + Vector3f vec = _retract_angles.get(); + _roll_angle = vec.x; + _pitch_angle = vec.y; + _yaw_angle = vec.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; + { + Vector3f vec = _neutral_angles.get(); + _roll_angle = vec.x; + _pitch_angle = vec.y; + _yaw_angle = vec.z; break; + } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: { - _roll_control_angle = _mavlink_angles.x; - _pitch_control_angle = _mavlink_angles.y; - _yaw_control_angle = _mavlink_angles.z; + Vector3f vec = _control_angles.get(); + _roll_control_angle = vec.x; + _pitch_control_angle = vec.y; + _yaw_control_angle = vec.z; calculate(); break; } @@ -81,9 +97,10 @@ void AP_Mount::update_mount_position() // 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); + // 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); if (_ahrs){ calculate(); } else { @@ -117,15 +134,15 @@ void AP_Mount::update_mount_position() 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); + // 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); } void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) { - _mount_mode=mode; + _mount_mode = (int8_t)mode; } // Change the configuration of the mount @@ -155,26 +172,26 @@ void AP_Mount::control_msg(mavlink_message_t *msg) return; } - switch (_mount_mode) + switch ((enum MAV_MOUNT_MODE)_mount_mode.get()) { 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); + set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); if (packet.save_position) { - // TODO: Save current trimmed position on EEPROM + _retract_angles.save(); } 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); + set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); if (packet.save_position) { - // TODO: Save current trimmed position on EEPROM + _neutral_angles.save(); } 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); + set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01); break; case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization @@ -187,6 +204,9 @@ void AP_Mount::control_msg(mavlink_message_t *msg) targetGPSLocation.alt = packet.input_c; set_GPS_target_location(targetGPSLocation); break; + + case MAV_MOUNT_MODE_ENUM_END: + break; } } @@ -201,21 +221,23 @@ void AP_Mount::status_msg(mavlink_message_t *msg) return; } - switch (_mount_mode) + switch ((enum MAV_MOUNT_MODE)_mount_mode.get()) { 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 + packet.pointing_b = _roll_angle*100; ///< degrees*100 + packet.pointing_a = _pitch_angle*100; ///< degrees*100 + packet.pointing_c = _yaw_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 packet.pointing_b = _target_GPS_location.lng; ///< longitude packet.pointing_c = _target_GPS_location.alt; ///< altitude break; + case MAV_MOUNT_MODE_ENUM_END: + break; } // status reply @@ -258,44 +280,35 @@ AP_Mount::calc_GPS_target_angle(struct Location *target) } } -// 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)) - { - _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; - } -} - // Inputs desired _roll_control_angle, _pitch_control_angle and _yaw_control_angle stabilizes them relative to the airframe // and calculates output _roll_angle, _pitch_angle and _yaw_angle void AP_Mount::calculate() { - 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. - float roll; - float pitch; - float yaw; - if (_ahrs){ - 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; + if (_ahrs) { + // only do the full 3D frame transform if we are doing yaw control + if (_stab_yaw) { + 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); + gimbal_target = m * cam; + gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle); + } else { + // otherwise base mount roll and pitch on the ahrs + // roll/pitch attitude, plus any requested angle + _roll_angle = _roll_control_angle; + _pitch_angle = _pitch_control_angle; + _yaw_angle = _yaw_control_angle; + if (_stab_roll) { + _roll_angle -= degrees(_ahrs->roll); + } + if (_stab_pitch) { + _pitch_angle -= degrees(_ahrs->pitch); + } + } } } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b8e03b79b3..db9925c59f 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -52,19 +52,20 @@ public: // 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(); + + // hook for eeprom variables + static const struct AP_Param::GroupInfo var_info[]; 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_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_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location // internal methods @@ -81,21 +82,20 @@ private: float _pitch_control_angle; float _yaw_control_angle; - int16_t _roll_angle; ///< degrees*100 - int16_t _pitch_angle; ///< degrees*100 - int16_t _yaw_angle; ///< degrees*100 + float _roll_angle; ///< degrees + float _pitch_angle; ///< degrees + float _yaw_angle; ///< degrees - uint8_t _stab_roll; ///< (1 = yes, 0 = no) - uint8_t _stab_pitch; ///< (1 = yes, 0 = no) - uint8_t _stab_yaw; ///< (1 = yes, 0 = no) + AP_Int8 _stab_roll; ///< (1 = yes, 0 = no) + AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no) + AP_Int8 _stab_yaw; ///< (1 = yes, 0 = no) - enum MAV_MOUNT_MODE _mount_mode; - MountType _mount_type; + AP_Int8 _mount_mode; 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 + 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 }; #endif