void
Gimbal::cycle()
{
if (!_initialized) {
/* get a subscription handle on the vehicle command topic */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic < 0) {
warnx("advert err");
}
_initialized = true;
}
bool updated = false;
perf_begin(_sample_perf);
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
if (_attitude_compensation) {
if (_att_sub < 0) {
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
}
vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
roll = -att.roll;
pitch = -att.pitch;
yaw = att.yaw;
updated = true;
}
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7;
debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2);
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3;
updated = true;
}
if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
if (updated) {
struct actuator_controls_s controls;
// debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
void
Gimbal::cycle()
{
if (!_initialized) {
/* get subscription handles */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic == nullptr) {
warnx("advert err");
}
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
_initialized = true;
}
bool updated = false;
perf_begin(_sample_perf);
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
float out_mount_mode = 0.0f;
/* Parameter update */
bool params_updated;
orb_check(_params_sub, ¶ms_updated);
if (params_updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); // XXX: is this actually necessary?
update_params();
}
/* Control mode update */
bool vehicle_control_mode_updated;
orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);
if (vehicle_control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
}
/* Check attitude */
struct vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
/* Check manual input */
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);
/* only check manual input for mount mode when not in offboard and aux chan is configured */
if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
float aux = 0.0f;
switch (_parameters.aux_mnt_chn) {
case 1:
aux = _manual_control.aux1;
break;
case 2:
aux = _manual_control.aux2;
break;
case 3:
aux = _manual_control.aux3;
break;
}
if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
updated = true;
}
if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
updated = true;
}
}
}
/* Check command input */
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd;
_config_cmd_set = true;
}
}
if (_config_cmd_set) {
_config_cmd_set = false;
_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
/* only check commanded mount mode when in offboard */
if (_control_mode.flag_control_offboard_enabled &&
fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
_mount_mode = int(_config_cmd.param1 + 0.5f);
updated = true;
}
}
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
(double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
/* consider mount mode if parameter is set */
if (_parameters.use_mnt > 0) {
switch (_mount_mode) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
out_mount_mode = -1.0f;
roll = 0.0f;
pitch = 0.0f;
yaw = 0.0f;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
out_mount_mode = 1.0f;
break;
default:
out_mount_mode = -1.0f;
}
}
if (updated) {
struct actuator_controls_s controls;
// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
//controls.control[3] = ; // camera shutter
controls.control[4] = out_mount_mode;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
