From d17e1b6fab2ec7044f32ecb29fe45c42aecb25c8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Jun 2022 12:38:12 +0900 Subject: [PATCH] AP_Mount: Alexmos restructure and support for ef/bf angle and rate --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 79 ++++++++++++++----------- libraries/AP_Mount/AP_Mount_Alexmos.h | 8 ++- 2 files changed, 48 insertions(+), 39 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 8d42e8b829..3da8a9f9d4 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -29,59 +29,71 @@ void AP_Mount_Alexmos::update() read_incoming(); // read the incoming messages from the gimbal // update based on mount mode - switch(get_mode()) { + switch (get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism - case MAV_MOUNT_MODE_RETRACT: - control_axis(_state._retract_angles.get(), true); + case MAV_MOUNT_MODE_RETRACT: { + const Vector3f &target = _state._retract_angles.get(); + _angle_rad.roll = radians(target.x); + _angle_rad.pitch = radians(target.y); + _angle_rad.yaw = radians(target.z); + _angle_rad.yaw_is_ef = false; break; + } // move mount to a neutral position, typically pointing forward - case MAV_MOUNT_MODE_NEUTRAL: - control_axis(_state._neutral_angles.get(), true); + case MAV_MOUNT_MODE_NEUTRAL: { + const Vector3f &target = _state._neutral_angles.get(); + _angle_rad.roll = radians(target.x); + _angle_rad.pitch = radians(target.y); + _angle_rad.yaw = radians(target.z); + _angle_rad.yaw_is_ef = false; 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 - control_axis(_angle_ef_target_rad, false); + switch (mavt_target.target_type) { + case MountTargetType::ANGLE: + _angle_rad = mavt_target.angle_rad; + break; + case MountTargetType::RATE: + update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); + break; + } 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(); - control_axis(_angle_ef_target_rad, false); + case MAV_MOUNT_MODE_RC_TARGETING: { + // update targets using pilot's RC inputs + MountTarget rc_target {}; + if (get_rc_rate_target(rc_target)) { + update_angle_target_from_rate(rc_target, _angle_rad); + } else if (get_rc_angle_target(rc_target)) { + _angle_rad = rc_target; + } break; + } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false, true)) { - control_axis(_angle_ef_target_rad, false); - } + IGNORE_RETURN(get_angle_target_to_roi(_angle_rad)); break; case MAV_MOUNT_MODE_HOME_LOCATION: - // constantly update the home location: - if (!AP::ahrs().home_is_set()) { - break; - } - _roi_target = AP::ahrs().get_home(); - _roi_target_set = true; - if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false, true)) { - control_axis(_angle_ef_target_rad, false); - } + IGNORE_RETURN(get_angle_target_to_home(_angle_rad)); break; case MAV_MOUNT_MODE_SYSID_TARGET: - if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, false, true)) { - control_axis(_angle_ef_target_rad, false); - } + IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad)); break; default: // we do not know this mode so do nothing break; } + + // send latest targets to gimbal + control_axis(_angle_rad); } // has_pan_control - returns true if this mount can control it's pan (required for multicopters) @@ -137,23 +149,18 @@ void AP_Mount_Alexmos::get_boardinfo() } /* - control_axis : send new angles to the gimbal at a fixed speed of 30 deg/s + control_axis : send new angle target to the gimbal at a fixed speed of 30 deg/s */ -void AP_Mount_Alexmos::control_axis(const Vector3f& angle, bool target_in_degrees) +void AP_Mount_Alexmos::control_axis(const MountTarget& angle_target_rad) { - // convert to degrees if necessary - Vector3f target_deg = angle; - if (!target_in_degrees) { - target_deg *= RAD_TO_DEG; - } alexmos_parameters outgoing_buffer; outgoing_buffer.angle_speed.mode = AP_MOUNT_ALEXMOS_MODE_ANGLE; outgoing_buffer.angle_speed.speed_roll = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED); - outgoing_buffer.angle_speed.angle_roll = DEGREE_TO_VALUE(target_deg.x); + outgoing_buffer.angle_speed.angle_roll = DEGREE_TO_VALUE(degrees(angle_target_rad.roll)); outgoing_buffer.angle_speed.speed_pitch = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED); - outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(target_deg.y); + outgoing_buffer.angle_speed.angle_pitch = DEGREE_TO_VALUE(degrees(angle_target_rad.pitch)); outgoing_buffer.angle_speed.speed_yaw = DEGREE_PER_SEC_TO_VALUE(AP_MOUNT_ALEXMOS_SPEED); - outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(target_deg.z); + outgoing_buffer.angle_speed.angle_yaw = DEGREE_TO_VALUE(degrees(get_bf_yaw_angle(angle_target_rad))); send_command(CMD_CONTROL, (uint8_t *)&outgoing_buffer.angle_speed, sizeof(alexmos_angles_speed)); } diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index df73e32b96..1c5a781847 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -95,8 +95,8 @@ private: // get_boardinfo - get board version and firmware version void get_boardinfo(); - // control_axis - send new angles to the gimbal at a fixed speed of 30 deg/s - void control_axis(const Vector3f& angle , bool targets_in_degrees); + // send new angles to the gimbal at a fixed speed of 30 deg/s + void control_axis(const MountTarget& angle_target_rad); // read_params - read current profile profile_id and global parameters from the gimbal settings void read_params(uint8_t profile_id); @@ -104,7 +104,7 @@ private: // write_params - write new parameters to the gimbal settings void write_params(); - bool get_realtimedata( Vector3f& angle); + bool get_realtimedata(Vector3f& angle); // Alexmos Serial Protocol reading part implementation // send_command - send a command to the Alemox Serial API @@ -116,6 +116,8 @@ private: // read_incoming - detect and read the header of the incoming message from the gimbal void read_incoming(); + MountTarget _angle_rad; // latest angle target + // structure for the Serial Protocol // CMD_BOARD_INFO