From ace1fd8740d432a271e5ba94b5003f6217fba96b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 15 Jan 2015 14:13:59 +0900 Subject: [PATCH] Mount_MAVLink: handle RC and GPS targeting in lib Previously we expected the mount to do this but it is likely that the first versions of MAVLink enable mounts will only be capable of pointing at a particular angle --- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 80 +++++++++++++++++++++---- libraries/AP_Mount/AP_Mount_MAVLink.h | 10 +++- 2 files changed, 77 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 6d48e4a670..227b9a90a7 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -11,14 +11,43 @@ void AP_Mount_MAVLink::init() // update mount position - should be called periodically void AP_Mount_MAVLink::update() { - // nothing to do for a MAVlink gimbal that supports all modes: - // MAV_MOUNT_MODE_RETRACT - nothing to do if the mount holds the retracted angles and - // we do not implement a separate servo based retract mechanism - // MAV_MOUNT_MODE_NEUTRAL - nothing to do if the mount holds the neutral angles - // MAV_MOUNT_MODE_MAVLINK_TARGETING - targets should already have been sent by a call to control_msg - // MAV_MOUNT_MODE_RC_TARGETING - mount should be able to see RC inputs published by flight controller - // MAV_MOUNT_MODE_GPS_POINT - mount should have received target sent from control_msg and - // should know vehicle's position which has been published by flight controller + + // update based on mount mode + switch(_frontend.get_mode(_instance)) { + // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism + case MAV_MOUNT_MODE_RETRACT: + send_angle_target(_frontend.state[_instance]._retract_angles.get(), true); + break; + + // move mount to a neutral position, typically pointing forward + case MAV_MOUNT_MODE_NEUTRAL: + send_angle_target(_frontend.state[_instance]._neutral_angles.get(), true); + break; + + // point to the angles given by a mavlink message + case MAV_MOUNT_MODE_MAVLINK_TARGETING: + // do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS + break; + + // RC radio manual angle control, but with stabilization from the AHRS + 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(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, true, true); + send_angle_target(_angle_ef_target_rad, false); + } + break; + + default: + // we do not know this mode so do nothing + break; + } } // has_pan_control - returns true if this mount can control it's pan (required for multicopters) @@ -36,17 +65,33 @@ 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, // param1: mode + 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 _frontend.state[_instance]._mode = mode; + _last_mode = mode_to_send; } // set_roi_target - sets target location that mount should attempt to point towards @@ -124,13 +169,24 @@ void AP_Mount_MAVLink::find_mount() } // send_angle_target - send earth-frame angle targets to mount -void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg) +void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees) { // exit immediately if mount has not been found if (!_enabled) { 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 (_frontend.state[_instance]._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 @@ -141,4 +197,8 @@ void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg) 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; } diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 902080e760..190b3a1ca8 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -25,7 +25,8 @@ public: AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) : AP_Mount_Backend(frontend, instance), _enabled(false), - _chan(MAVLINK_COMM_0) + _chan(MAVLINK_COMM_0), + _last_mode(MAV_MOUNT_MODE_RETRACT) {} // init - performs any required initialisation for this instance @@ -57,12 +58,15 @@ private: // find_mount - search for MAVLink enabled mount void find_mount(); - // send_angle_target - send earth-frame angle targets (in degrees) to mount - void send_angle_target(const Vector3f& target_deg); + // 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 _enabled; // gimbal becomes enabled once the mount has been discovered 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 }; #endif // __AP_MOUNT_MAVLINK_H__