fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

OFFBOARD mode architecture overhaul (#16739)

Browse Source 
- handle SET_POSITION_TARGET_LOCAL_NED and SET_POSITION_TARGET_GLOBAL_INT with ORB_ID(trajectory_setpoint)
 - FlightTaskOffboard not needed at all
 - bypass position_setpoint_triplet entirely (start removing extraneous fields)
 - simplify offboard_control_mode to map to supported control modes
This commit is contained in:
Daniel Agar
2021-03-05 09:39:46 -05:00
committed by GitHub
parent 5233737a86
commit d0c9a5fc93
25 changed files with 392 additions and 1074 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+279 -534
View File
@@ -560,15 +560,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
_mavlink->request_stop_ulog_streaming();
} else if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) {
vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
if (control_mode.flag_control_offboard_enabled) {
// Not differentiating between airspeed and groundspeed yet
set_offb_cruising_speed(cmd_mavlink.param2);
}
}
if (!send_ack) {
@@ -854,187 +845,122 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
mavlink_set_position_target_local_ned_t set_position_target_local_ned;
mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
const bool values_finite =
PX4_ISFINITE(set_position_target_local_ned.x) &&
PX4_ISFINITE(set_position_target_local_ned.y) &&
PX4_ISFINITE(set_position_target_local_ned.z) &&
PX4_ISFINITE(set_position_target_local_ned.vx) &&
PX4_ISFINITE(set_position_target_local_ned.vy) &&
PX4_ISFINITE(set_position_target_local_ned.vz) &&
PX4_ISFINITE(set_position_target_local_ned.afx) &&
PX4_ISFINITE(set_position_target_local_ned.afy) &&
PX4_ISFINITE(set_position_target_local_ned.afz) &&
PX4_ISFINITE(set_position_target_local_ned.yaw);
mavlink_set_position_target_local_ned_t target_local_ned;
mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned);
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
set_position_target_local_ned.target_system == 0) &&
(mavlink_system.compid == set_position_target_local_ned.target_component ||
set_position_target_local_ned.target_component == 0) &&
values_finite) {
if (_mavlink->get_forward_externalsp() &&
(mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) &&
(mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) {
offboard_control_mode_s offboard_control_mode{};
vehicle_local_position_setpoint_s setpoint{};
/* convert mavlink type (local, NED) to uORB offboard control struct */
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_X_IGNORE
| POSITION_TARGET_TYPEMASK_Y_IGNORE
| POSITION_TARGET_TYPEMASK_Z_IGNORE));
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_Z_IGNORE);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_VX_IGNORE
| POSITION_TARGET_TYPEMASK_VY_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE));
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_AX_IGNORE
| POSITION_TARGET_TYPEMASK_AY_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_IGNORE);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
const uint16_t type_mask = target_local_ned.type_mask;
/* yaw ignore flag mapps to ignore_attitude */
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET);
if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? NAN : target_local_ned.x;
setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? NAN : target_local_ned.y;
setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? NAN : target_local_ned.z;
bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
bool is_gliding_sp = (bool)(set_position_target_local_ned.type_mask &
(POSITION_TARGET_TYPEMASK_Z_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_local_ned.vx;
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_local_ned.vy;
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_local_ned.vz;
offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_local_ned.afx;
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_local_ned.afy;
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_local_ned.afz;
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
} else if (target_local_ned.coordinate_frame == MAV_FRAME_BODY_NED) {
vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
const matrix::Dcmf R{matrix::Quatf{vehicle_attitude.q}};
if (control_mode.flag_control_offboard_enabled) {
if (is_force_sp && offboard_control_mode.ignore_position &&
offboard_control_mode.ignore_velocity) {
const bool ignore_velocity = type_mask & (POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE |
POSITION_TARGET_TYPEMASK_VZ_IGNORE);
PX4_WARN("force setpoint not supported");
if (!ignore_velocity) {
const matrix::Vector3f velocity_body_sp{
(type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? 0.f : target_local_ned.vx,
(type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? 0.f : target_local_ned.vy,
(type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz
};
} else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
position_setpoint_triplet_s pos_sp_triplet{};
const matrix::Vector3f velocity_setpoint{R * velocity_body_sp};
setpoint.vx = velocity_setpoint(0);
setpoint.vy = velocity_setpoint(1);
setpoint.vz = velocity_setpoint(2);
pos_sp_triplet.timestamp = hrt_absolute_time();
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
/* Order of statements matters. Takeoff can override loiter.
* See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */
if (is_loiter_sp) {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
} else if (is_takeoff_sp) {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
} else if (is_land_sp) {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
} else if (is_idle_sp) {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
} else if (is_gliding_sp) {
pos_sp_triplet.current.cruising_throttle = 0.0f;
} else {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
/* set the local pos values */
if (!offboard_control_mode.ignore_position) {
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = set_position_target_local_ned.x;
pos_sp_triplet.current.y = set_position_target_local_ned.y;
pos_sp_triplet.current.z = set_position_target_local_ned.z;
pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed();
} else {
pos_sp_triplet.current.position_valid = false;
}
/* set the local vel values */
if (!offboard_control_mode.ignore_velocity) {
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame;
} else {
pos_sp_triplet.current.velocity_valid = false;
}
if (!offboard_control_mode.ignore_alt_hold) {
pos_sp_triplet.current.alt_valid = true;
pos_sp_triplet.current.z = set_position_target_local_ned.z;
} else {
pos_sp_triplet.current.alt_valid = false;
}
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (!offboard_control_mode.ignore_acceleration_force) {
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
pos_sp_triplet.current.acceleration_is_force = is_force_sp;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw sp value */
if (!offboard_control_mode.ignore_attitude) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value */
if (!(offboard_control_mode.ignore_bodyrate_x ||
offboard_control_mode.ignore_bodyrate_y ||
offboard_control_mode.ignore_bodyrate_z)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
_pos_sp_triplet_pub.publish(pos_sp_triplet);
}
} else {
setpoint.vx = NAN;
setpoint.vy = NAN;
setpoint.vz = NAN;
}
const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE |
POSITION_TARGET_TYPEMASK_AZ_IGNORE);
if (!ignore_acceleration) {
const matrix::Vector3f acceleration_body_sp{
(type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? 0.f : target_local_ned.afx,
(type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? 0.f : target_local_ned.afy,
(type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? 0.f : target_local_ned.afz
};
const matrix::Vector3f acceleration_setpoint{R * acceleration_body_sp};
acceleration_setpoint.copyTo(setpoint.acceleration);
} else {
setpoint.acceleration[0] = NAN;
setpoint.acceleration[1] = NAN;
setpoint.acceleration[2] = NAN;
}
setpoint.x = NAN;
setpoint.y = NAN;
setpoint.z = NAN;
} else {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %d unsupported",
target_local_ned.coordinate_frame);
return;
}
setpoint.thrust[0] = NAN;
setpoint.thrust[1] = NAN;
setpoint.thrust[2] = NAN;
setpoint.yaw = (type_mask == POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_local_ned.yaw;
setpoint.yawspeed = (type_mask == POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_local_ned.yaw_rate;
offboard_control_mode_s ocm{};
ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported");
return;
}
if (ocm.position || ocm.velocity || ocm.acceleration) {
// publish offboard_control_mode
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
// only publish setpoint once in OFFBOARD
setpoint.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(setpoint);
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED invalid");
}
}
}
@@ -1042,186 +968,119 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
void
MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t *msg)
{
mavlink_set_position_target_global_int_t set_position_target_global_int;
mavlink_msg_set_position_target_global_int_decode(msg, &set_position_target_global_int);
const bool values_finite =
PX4_ISFINITE(set_position_target_global_int.alt) &&
PX4_ISFINITE(set_position_target_global_int.vx) &&
PX4_ISFINITE(set_position_target_global_int.vy) &&
PX4_ISFINITE(set_position_target_global_int.vz) &&
PX4_ISFINITE(set_position_target_global_int.afx) &&
PX4_ISFINITE(set_position_target_global_int.afy) &&
PX4_ISFINITE(set_position_target_global_int.afz) &&
PX4_ISFINITE(set_position_target_global_int.yaw);
mavlink_set_position_target_global_int_t target_global_int;
mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int);
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_global_int.target_system ||
set_position_target_global_int.target_system == 0) &&
(mavlink_system.compid == set_position_target_global_int.target_component ||
set_position_target_global_int.target_component == 0) &&
values_finite) {
if (_mavlink->get_forward_externalsp() &&
(mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) &&
(mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) {
offboard_control_mode_s offboard_control_mode{};
vehicle_local_position_setpoint_s setpoint{};
/* convert mavlink type (local, NED) to uORB offboard control struct */
offboard_control_mode.ignore_position = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK_X_IGNORE
| POSITION_TARGET_TYPEMASK_Y_IGNORE
| POSITION_TARGET_TYPEMASK_Z_IGNORE));
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_Z_IGNORE);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK_VX_IGNORE
| POSITION_TARGET_TYPEMASK_VY_IGNORE
| POSITION_TARGET_TYPEMASK_VZ_IGNORE));
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask &
(POSITION_TARGET_TYPEMASK_AX_IGNORE
| POSITION_TARGET_TYPEMASK_AY_IGNORE
| POSITION_TARGET_TYPEMASK_AZ_IGNORE));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_YAW_IGNORE);
offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask &
POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
const uint16_t type_mask = target_global_int.type_mask;
bool is_force_sp = (bool)(set_position_target_global_int.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET);
// position
if (!(type_mask & (POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE |
POSITION_TARGET_TYPEMASK_Z_IGNORE))) {
offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);
vehicle_local_position_s local_pos{};
_vehicle_local_position_sub.copy(&local_pos);
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != local_pos.ref_timestamp)) {
vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
map_projection_init_timestamped(&_global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp);
_global_local_alt0 = local_pos.ref_alt;
}
if (control_mode.flag_control_offboard_enabled) {
if (is_force_sp && offboard_control_mode.ignore_position &&
offboard_control_mode.ignore_velocity) {
// global -> local
const double lat = target_global_int.lat_int / 1e7;
const double lon = target_global_int.lon_int / 1e7;
map_projection_project(&_global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y);
PX4_WARN("force setpoint not supported");
if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
setpoint.z = _global_local_alt0 - target_global_int.alt;
} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
home_position_s home_position{};
_home_position_sub.copy(&home_position);
if (home_position.valid_alt) {
const float alt = home_position.alt - target_global_int.alt;
setpoint.z = alt - _global_local_alt0;
} else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
position_setpoint_triplet_s pos_sp_triplet{};
pos_sp_triplet.timestamp = hrt_absolute_time();
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
/* Order of statements matters. Takeoff can override loiter.
* See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */
if (set_position_target_global_int.type_mask & 0x3000) { //Loiter setpoint
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
} else if (set_position_target_global_int.type_mask & 0x1000) { //Takeoff setpoint
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
} else if (set_position_target_global_int.type_mask & 0x2000) { //Land setpoint
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
} else if (set_position_target_global_int.type_mask & 0x4000) { //Idle setpoint
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
} else {
pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
/* set the local pos values */
vehicle_local_position_s local_pos{};
if (!offboard_control_mode.ignore_position && _vehicle_local_position_sub.copy(&local_pos)) {
if (!globallocalconverter_initialized()) {
globallocalconverter_init(local_pos.ref_lat, local_pos.ref_lon,
local_pos.ref_alt, local_pos.ref_timestamp);
pos_sp_triplet.current.position_valid = false;
} else {
float target_altitude;
if (set_position_target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
target_altitude = set_position_target_global_int.alt + local_pos.ref_alt;
} else {
target_altitude = set_position_target_global_int.alt; //MAV_FRAME_GLOBAL_INT
}
globallocalconverter_tolocal(set_position_target_global_int.lat_int / 1e7,
set_position_target_global_int.lon_int / 1e7, target_altitude,
&pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z);
pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed();
pos_sp_triplet.current.position_valid = true;
}
} else {
pos_sp_triplet.current.position_valid = false;
}
/* set the local velocity values */
if (!offboard_control_mode.ignore_velocity) {
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_global_int.vx;
pos_sp_triplet.current.vy = set_position_target_global_int.vy;
pos_sp_triplet.current.vz = set_position_target_global_int.vz;
pos_sp_triplet.current.velocity_frame = set_position_target_global_int.coordinate_frame;
} else {
pos_sp_triplet.current.velocity_valid = false;
}
if (!offboard_control_mode.ignore_alt_hold) {
pos_sp_triplet.current.alt_valid = true;
} else {
pos_sp_triplet.current.alt_valid = false;
}
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (!offboard_control_mode.ignore_acceleration_force) {
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_global_int.afx;
pos_sp_triplet.current.a_y = set_position_target_global_int.afy;
pos_sp_triplet.current.a_z = set_position_target_global_int.afz;
pos_sp_triplet.current.acceleration_is_force = is_force_sp;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw setpoint */
if (!offboard_control_mode.ignore_attitude) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_global_int.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value */
if (!(offboard_control_mode.ignore_bodyrate_x ||
offboard_control_mode.ignore_bodyrate_y ||
offboard_control_mode.ignore_bodyrate_z)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_global_int.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
_pos_sp_triplet_pub.publish(pos_sp_triplet);
// home altitude required
return;
}
} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
vehicle_global_position_s vehicle_global_position{};
_vehicle_global_position_sub.copy(&vehicle_global_position);
if (vehicle_global_position.terrain_alt_valid) {
const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
setpoint.z = _global_local_alt0 - alt;
} else {
// valid terrain alt required
return;
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT invalid coordinate frame %d",
target_global_int.coordinate_frame);
return;
}
} else {
setpoint.x = NAN;
setpoint.y = NAN;
setpoint.z = NAN;
}
// velocity
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_global_int.vx;
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_global_int.vy;
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_global_int.vz;
// acceleration
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_global_int.afx;
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_global_int.afy;
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_global_int.afz;
setpoint.thrust[0] = NAN;
setpoint.thrust[1] = NAN;
setpoint.thrust[2] = NAN;
setpoint.yaw = (type_mask == POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_global_int.yaw;
setpoint.yawspeed = (type_mask == POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_global_int.yaw_rate;
offboard_control_mode_s ocm{};
ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z);
ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported");
return;
}
if (ocm.position || ocm.velocity || ocm.acceleration) {
// publish offboard_control_mode
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
// only publish setpoint once in OFFBOARD
setpoint.timestamp = hrt_absolute_time();
_trajectory_setpoint_pub.publish(setpoint);
}
}
}
@@ -1230,64 +1089,43 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
void
MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
{
mavlink_set_actuator_control_target_t set_actuator_control_target;
mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
bool values_finite =
PX4_ISFINITE(set_actuator_control_target.controls[0]) &&
PX4_ISFINITE(set_actuator_control_target.controls[1]) &&
PX4_ISFINITE(set_actuator_control_target.controls[2]) &&
PX4_ISFINITE(set_actuator_control_target.controls[3]) &&
PX4_ISFINITE(set_actuator_control_target.controls[4]) &&
PX4_ISFINITE(set_actuator_control_target.controls[5]) &&
PX4_ISFINITE(set_actuator_control_target.controls[6]) &&
PX4_ISFINITE(set_actuator_control_target.controls[7]);
if ((mavlink_system.sysid == set_actuator_control_target.target_system ||
set_actuator_control_target.target_system == 0) &&
(mavlink_system.compid == set_actuator_control_target.target_component ||
set_actuator_control_target.target_component == 0) &&
values_finite) {
// TODO
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled");
PX4_ERR("Please disable lockstep for actuator offboard control:");
PX4_ERR("https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation");
return;
PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled");
PX4_ERR("Please disable lockstep for actuator offboard control:");
PX4_ERR("https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation");
return;
#endif
mavlink_set_actuator_control_target_t actuator_target;
mavlink_msg_set_actuator_control_target_decode(msg, &actuator_target);
if (_mavlink->get_forward_externalsp() &&
(mavlink_system.sysid == actuator_target.target_system || actuator_target.target_system == 0) &&
(mavlink_system.compid == actuator_target.target_component || actuator_target.target_component == 0)
) {
/* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */
bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2);
//bool ignore_setpoints = bool(actuator_target.group_mlx != 2);
offboard_control_mode_s offboard_control_mode{};
offboard_control_mode.ignore_thrust = ignore_setpoints;
offboard_control_mode.ignore_attitude = ignore_setpoints;
offboard_control_mode.ignore_bodyrate_x = ignore_setpoints;
offboard_control_mode.ignore_bodyrate_y = ignore_setpoints;
offboard_control_mode.ignore_bodyrate_z = ignore_setpoints;
offboard_control_mode.ignore_position = ignore_setpoints;
offboard_control_mode.ignore_velocity = ignore_setpoints;
offboard_control_mode.ignore_acceleration_force = ignore_setpoints;
offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(offboard_control_mode);
/* If we are in offboard control mode, publish the actuator controls */
vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (control_mode.flag_control_offboard_enabled) {
// Publish actuator controls only once in OFFBOARD
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
actuator_controls_s actuator_controls{};
actuator_controls.timestamp = hrt_absolute_time();
/* Set duty cycles for the servos in the actuator_controls message */
for (size_t i = 0; i < 8; i++) {
actuator_controls.control[i] = set_actuator_control_target.controls[i];
actuator_controls.control[i] = actuator_target.controls[i];
}
switch (set_actuator_control_target.group_mlx) {
switch (actuator_target.group_mlx) {
case 0:
_actuator_controls_pubs[0].publish(actuator_controls);
break;
@@ -1309,7 +1147,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
}
}
}
}
void
@@ -1523,144 +1360,83 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type
void
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
{
mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
bool values_finite =
PX4_ISFINITE(set_attitude_target.q[0]) &&
PX4_ISFINITE(set_attitude_target.q[1]) &&
PX4_ISFINITE(set_attitude_target.q[2]) &&
PX4_ISFINITE(set_attitude_target.q[3]) &&
PX4_ISFINITE(set_attitude_target.thrust) &&
PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
PX4_ISFINITE(set_attitude_target.body_yaw_rate);
mavlink_set_attitude_target_t attitude_target;
mavlink_msg_set_attitude_target_decode(msg, &attitude_target);
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_attitude_target.target_system ||
set_attitude_target.target_system == 0) &&
(mavlink_system.compid == set_attitude_target.target_component ||
set_attitude_target.target_component == 0) &&
values_finite) {
if (_mavlink->get_forward_externalsp() &&
(mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) &&
(mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) {
offboard_control_mode_s offboard_control_mode{};
const uint8_t type_mask = attitude_target.type_mask;
/* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE);
const bool attitude = !(type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE);
const bool body_rates = !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE)
&& !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE);
/*
* The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust
* using different messages. Eg.: First send set_attitude_target containing the attitude and ignore
* bits set for everything else and then send set_attitude_target containing the thrust and ignore bits
* set for everything else.
*/
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
/*
* if attitude or body rate have been used (not ignored) previously and this message only sends
* throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
* body rates to keep the controllers running
*/
bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE);
bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE);
bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE);
bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE);
if (attitude) {
vehicle_attitude_setpoint_s attitude_setpoint{};
if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y ||
ignore_bodyrate_msg_z) &&
ignore_attitude_msg && !offboard_control_mode.ignore_thrust) {
const matrix::Quatf q{attitude_target.q};
q.copyTo(attitude_setpoint.q_d);
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x;
offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y;
offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z;
offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude;
matrix::Eulerf euler{q};
attitude_setpoint.roll_body = euler.phi();
attitude_setpoint.pitch_body = euler.theta();
attitude_setpoint.yaw_body = euler.psi();
} else {
offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x;
offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y;
offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z;
offboard_control_mode.ignore_attitude = ignore_attitude_msg;
}
// TODO: review use case
attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ?
attitude_target.body_yaw_rate : NAN;
offboard_control_mode.ignore_position = true;
offboard_control_mode.ignore_velocity = true;
offboard_control_mode.ignore_acceleration_force = true;
if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
}
offboard_control_mode.timestamp = hrt_absolute_time();
// publish offboard_control_mode
offboard_control_mode_s ocm{};
ocm.attitude = true;
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
_offboard_control_mode_pub.publish(offboard_control_mode);
// Publish attitude setpoint only once in OFFBOARD
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
attitude_setpoint.timestamp = hrt_absolute_time();
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
_mc_virtual_att_sp_pub.publish(attitude_setpoint);
vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
} else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
_fw_virtual_att_sp_pub.publish(attitude_setpoint);
if (control_mode.flag_control_offboard_enabled) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_mode.ignore_attitude)) {
vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = hrt_absolute_time();
if (!ignore_attitude_msg) { // only copy att sp if message contained new data
matrix::Quatf q(set_attitude_target.q);
q.copyTo(att_sp.q_d);
matrix::Eulerf euler{q};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
att_sp.yaw_sp_move_rate = 0.0f;
}
if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}
// Publish attitude setpoint
if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
_mc_virtual_att_sp_pub.publish(att_sp);
} else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
_fw_virtual_att_sp_pub.publish(att_sp);
} else {
_att_sp_pub.publish(att_sp);
}
} else {
_att_sp_pub.publish(attitude_setpoint);
}
}
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
if (!offboard_control_mode.ignore_bodyrate_x ||
!offboard_control_mode.ignore_bodyrate_y ||
!offboard_control_mode.ignore_bodyrate_z) {
} else if (body_rates) {
vehicle_rates_setpoint_s setpoint{};
setpoint.roll = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) ? NAN : attitude_target.body_roll_rate;
setpoint.pitch = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) ? NAN : attitude_target.body_pitch_rate;
setpoint.yaw = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? NAN : attitude_target.body_yaw_rate;
vehicle_rates_setpoint_s rates_sp{};
if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
fill_thrust(setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust);
}
rates_sp.timestamp = hrt_absolute_time();
// publish offboard_control_mode
offboard_control_mode_s ocm{};
ocm.body_rate = true;
ocm.timestamp = hrt_absolute_time();
_offboard_control_mode_pub.publish(ocm);
// only copy att rates sp if message contained new data
if (!ignore_bodyrate_msg_x) {
rates_sp.roll = set_attitude_target.body_roll_rate;
}
if (!ignore_bodyrate_msg_y) {
rates_sp.pitch = set_attitude_target.body_pitch_rate;
}
if (!ignore_bodyrate_msg_z) {
rates_sp.yaw = set_attitude_target.body_yaw_rate;
}
if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
}
_rates_sp_pub.publish(rates_sp);
}
// Publish rate setpoint only once in OFFBOARD
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
setpoint.timestamp = hrt_absolute_time();
_rates_sp_pub.publish(setpoint);
}
}
}
@@ -3403,34 +3179,3 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
pthread_attr_destroy(&receiveloop_attr);
}
float
MavlinkReceiver::get_offb_cruising_speed()
{
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _offb_cruising_speed_mc > 0.0f) {
return _offb_cruising_speed_mc;
} else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && _offb_cruising_speed_fw > 0.0f) {
return _offb_cruising_speed_fw;
} else {
return -1.0f;
}
}
void
MavlinkReceiver::set_offb_cruising_speed(float speed)
{
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_offb_cruising_speed_mc = speed;
} else {
_offb_cruising_speed_fw = speed;
}
}
src/modules/mavlink/mavlink_receiver.h
+9 -13
View File
@@ -52,6 +52,7 @@
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
@@ -93,7 +94,6 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -128,13 +128,6 @@ public:
static void *start_helper(void *context);
/**
* Get the cruising speed in offboard control
*
* @return the desired cruising speed for the current flight mode
*/
float get_offb_cruising_speed();
/**
* Set the cruising speed in offboard control
*
@@ -258,6 +251,8 @@ private:
mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char()
orb_advert_t _mavlink_log_pub{nullptr};
// subset of MAV_COMPONENTs we support
enum SUPPORTED_COMPONENTS : uint8_t {
COMP_ID_ALL,
@@ -369,7 +364,6 @@ private:
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<sensor_gps_s> _gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
@@ -378,6 +372,7 @@ private:
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_land_detected_s> _land_detector_pub{ORB_ID(vehicle_land_detected)};
uORB::Publication<vehicle_local_position_s> _local_pos_pub{ORB_ID(vehicle_local_position)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
@@ -407,9 +402,10 @@ private:
// ORB subscriptions
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
@@ -432,6 +428,9 @@ private:
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
uint16_t _mom_switch_state{0};
map_projection_reference_s _global_local_proj_ref{};
float _global_local_alt0{NAN};
uint64_t _global_ref_timestamp{0};
map_projection_reference_s _hil_local_proj_ref{};
@@ -440,9 +439,6 @@ private:
hrt_abstime _last_utm_global_pos_com{0};
float _offb_cruising_speed_mc{-1.0f};
float _offb_cruising_speed_fw{-1.0f};
// Allocated if needed.
TunePublisher *_tune_publisher{nullptr};