From cf76dcfbf3e5eafade435e465414c9004c83166f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 1 Feb 2015 20:34:12 +1100 Subject: [PATCH] AP_Mount: fixed mount MAVLink backend to match SITL sim behaviour Pair-Programmed-With: Paul Riseborough --- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 78 ++----------------------- libraries/AP_Mount/AP_Mount_MAVLink.h | 9 --- 2 files changed, 6 insertions(+), 81 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 64e4174c2c..55b416a288 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -10,21 +10,17 @@ #include #endif -AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) : +AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_Backend(frontend, state, instance), _initialised(false), - _chan(MAVLINK_COMM_0), - _last_mode(MAV_MOUNT_MODE_RETRACT), _ekf(frontend._ahrs) {} // init - performs any required initialisation for this instance void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager) { - // use mavlink channel associated with MAVLink2 protocol - if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink2, _chan)) { - _initialised = true; - } + _initialised = true; + set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); } // update mount position - should be called periodically @@ -39,12 +35,10 @@ void AP_Mount_MAVLink::update() switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: - send_angle_target(_state._retract_angles.get(), true); break; // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: - send_angle_target(_state._neutral_angles.get(), true); break; // point to the angles given by a mavlink message @@ -56,14 +50,12 @@ void AP_Mount_MAVLink::update() case MAV_MOUNT_MODE_RC_TARGETING: // update targets using pilot's rc inputs update_targets_from_rc(); - send_angle_target(_angle_ef_target_rad, false); break; // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) { calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, false); - send_angle_target(_angle_ef_target_rad, false); } break; @@ -88,33 +80,8 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) return; } - // map requested mode to mode that mount can actually support - enum MAV_MOUNT_MODE mode_to_send = mode; - switch (mode) { - case MAV_MOUNT_MODE_RETRACT: - case MAV_MOUNT_MODE_NEUTRAL: - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - case MAV_MOUNT_MODE_RC_TARGETING: - case MAV_MOUNT_MODE_GPS_POINT: - mode_to_send = MAV_MOUNT_MODE_MAVLINK_TARGETING; - break; - default: - // unknown mode so just send it and hopefully gimbal supports it - break; - } - - // prepare and send command_long message with DO_SET_MODE command - mavlink_msg_command_long_send( - _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id - MAV_CMD_DO_SET_MODE, // command number - 0, // confirmation: 0=first confirmation of this command - mode_to_send, // param1: mode - 0, // param2: custom mode - 0.0f, 0.0f, 0.0f,0.0f, 0.0f); // param3 ~ param 7: not used - // record the mode change _state._mode = mode; - _last_mode = mode_to_send; } // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message @@ -123,40 +90,6 @@ void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan) // do nothing - we rely on the mount sending the messages directly } -// send_angle_target - send earth-frame angle targets to mount -void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees) -{ - // exit immediately if not initialised - if (!_initialised) { - return; - } - - // convert to degrees if necessary - Vector3f target_deg = target; - if (!target_in_degrees) { - target_deg *= RAD_TO_DEG; - } - - // exit immediately if mode and targets have not changed since last time they were sent - if (_state._mode == MAV_MOUNT_MODE_MAVLINK_TARGETING && target_deg == _last_angle_target) { - return; - } - - // prepare and send command_long message with DO_MOUNT_CONTROL command - mavlink_msg_command_long_send( - _chan, mavlink_system.sysid, AP_MOUNT_MAVLINK_COMPID, // channel, system id, component id - MAV_CMD_DO_MOUNT_CONTROL, // command number - 0, // confirmation: 0=first confirmation of this command - target_deg.y, // param1: pitch (in degrees) or lat (as int32_t) - target_deg.x, // param2: roll (in degrees) or lon (as int32_t) - target_deg.z, // param3: yaw (in degrees) or alt (in meters). - 0.0f,0.0f,0.0f, // param4 ~ param6 : not used - MAV_MOUNT_MODE_MAVLINK_TARGETING); // param7: MAV_MOUNT_MODE enum value - - // store sent target and mode - _last_angle_target = target_deg; - _last_mode = MAV_MOUNT_MODE_MAVLINK_TARGETING; -} /* handle a GIMBAL_REPORT message @@ -187,8 +120,9 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess // set the demanded quaternion - tilt down with a roll and yaw of zero Quaternion quatDem; - quatDem[0] = sqrtf(0.75f); - quatDem[2] = -sqrtf(0.25f); + quatDem.from_euler(_angle_ef_target_rad.x, + _angle_ef_target_rad.y, + _angle_ef_target_rad.z); //divide the demanded quaternion by the estimated to get the error Quaternion quatErr = quatDem / quatEst; diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 1a25099378..21d6c1fe07 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -19,8 +19,6 @@ #include #include -#define AP_MOUNT_MAVLINK_COMPID 10 // Use hard-coded component id to communicate with gimbal, sysid is taken from globally defined mavlink_system.sysid - class AP_Mount_MAVLink : public AP_Mount_Backend { @@ -50,15 +48,8 @@ public: virtual void send_gimbal_report(mavlink_channel_t chan); private: - // send_angle_target - send earth-frame angle targets to mount - // set target_in_degrees to true to send degree targets, false if target in radians - void send_angle_target(const Vector3f& target, bool target_in_degrees); - // internal variables bool _initialised; // true once the driver has been initialised - mavlink_channel_t _chan; // telemetry channel used to communicate with mount - enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount - Vector3f _last_angle_target; // last angle target sent to mount // state of small EKF for gimbal SmallEKF _ekf;