Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Alexmos restructure and support for ef/bf angle and rate

This commit is contained in:
Randy Mackay 2022-06-23 12:38:12 +09:00
parent 0d60e47c68
commit d17e1b6fab
2 changed files with 48 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -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));
}

8
libraries/AP_Mount/AP_Mount_Alexmos.h
View File

@ -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