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__