diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/commander.cpp | 48 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 7 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 7 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 292 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 6 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/navigator/module.mk | 1 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 6 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 17 | ||||
-rw-r--r-- | src/modules/navigator/offboard.cpp | 129 | ||||
-rw-r--r-- | src/modules/navigator/offboard.h | 72 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 201 | ||||
-rw-r--r-- | src/modules/uORB/topics/position_setpoint_triplet.h | 7 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_command.h | 3 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 2 |
17 files changed, 571 insertions, 240 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9ebe006f0..616b855df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -633,7 +633,29 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; - + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + status_local->offboard_control_set_by_command = false; + } else { + /* Set flag that offboard was set via command, main state is not overridden by rc */ + status_local->offboard_control_set_by_command = true; + } + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + status_local->offboard_control_set_by_command = false; + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -1958,6 +1980,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* if offboard is set allready by a mavlink command, abort */ + if (status.offboard_control_set_by_command) { + return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + } + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); @@ -2150,21 +2177,26 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index e49288a74..bec706bad 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); +/** + * Forward external setpoint messages + * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard + * control mode + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 940e64144..4155d6bf4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -123,6 +123,7 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _use_hil_gps(false), + _forward_externalsp(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -483,6 +484,7 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); } /* update system and component id */ @@ -529,6 +531,11 @@ void Mavlink::mavlink_update_system(void) param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; } int Mavlink::get_system_id() diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7f9d225bb..1f96e586b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -137,6 +137,8 @@ public: bool get_use_hil_gps() { return _use_hil_gps; } + bool get_forward_externalsp() { return _forward_externalsp; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -232,7 +234,7 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(const void *ptr, int size); - + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -275,6 +277,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ @@ -349,6 +352,7 @@ private: param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + param_t _param_forward_externalsp; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0ac62d545..e8d783847 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ /* XXX trim includes */ @@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _range_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), _vision_position_pub(-1), _telemetry_status_pub(-1), @@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); + break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: handle_message_vision_position_estimate(msg); break; @@ -475,6 +485,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(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); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints + + /* 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)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + switch (set_position_target_local_ned.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.position[0] = set_position_target_local_ned.x; + offboard_control_sp.position[1] = set_position_target_local_ned.y; + offboard_control_sp.position[2] = set_position_target_local_ned.z; + offboard_control_sp.velocity[0] = set_position_target_local_ned.vx; + offboard_control_sp.velocity[1] = set_position_target_local_ned.vy; + offboard_control_sp.velocity[2] = set_position_target_local_ned.vz; + offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; + offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; + offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; + offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } + + /* set ignore flags */ + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore &= ~(1 << i); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << + OFB_IGN_BIT_YAW; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << + OFB_IGN_BIT_YAWRATE; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* 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()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_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_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } + + } + + } + + } + } +} + +void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { mavlink_vision_position_estimate_t pos; @@ -514,6 +707,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) } 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); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + /* 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)) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = set_attitude_target.q[i]; + } + offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; + offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + + /* set correct ignore flags for body rate fields: copy from mavlink message */ + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + } + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); + + + offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* 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()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + mavlink_quaternion_to_euler(set_attitude_target.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; + att_sp.thrust = set_attitude_target.thrust; + memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ + ///XXX add support for ignoring individual axes + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = set_attitude_target.body_roll_rate; + rates_sp.pitch = set_attitude_target.body_pitch_rate; + rates_sp.yaw = set_attitude_target.body_yaw_rate; + rates_sp.thrust = set_attitude_target.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } + } +} + +void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c4e12d8d8..e5f2c6a73 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -71,6 +71,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/vehicle_force_setpoint.h> #include "mavlink_ftp.h" @@ -117,6 +118,8 @@ private: void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); @@ -145,10 +148,11 @@ private: orb_advert_t _flow_pub; orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; + orb_advert_t _pos_sp_triplet_pub; orb_advert_t _vicon_position_pub; orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ec7b2a78f..d52138522 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -653,7 +653,6 @@ MulticopterPositionControl::control_offboard(float dt) _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ @@ -663,6 +662,11 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _sp_move_rate(0) = _pos_sp_triplet.current.vx; _sp_move_rate(1) = _pos_sp_triplet.current.vy; + } + + if (_pos_sp_triplet.current.yaw_valid) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c075903b7..f0900da8b 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c \ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index bf42acff9..d550dcc4c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,7 +60,6 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" -#include "offboard.h" #include "datalinkloss.h" #include "enginefailure.h" #include "gpsfailure.h" @@ -70,7 +69,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 7 class Navigator : public control::SuperBlock { @@ -139,7 +138,6 @@ public: int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } - int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -159,7 +157,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -199,7 +196,6 @@ private: RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ - Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bf5e36d39..c6ec96dcb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -68,7 +68,6 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> -#include <uORB/topics/offboard_control_setpoint.h> #include <drivers/drv_baro.h> #include <systemlib/err.h> @@ -105,7 +104,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), - _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -134,7 +132,6 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), - _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), @@ -149,11 +146,10 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; - _navigation_mode_array[3] = &_offboard; - _navigation_mode_array[4] = &_dataLinkLoss; - _navigation_mode_array[5] = &_engineFailure; - _navigation_mode_array[6] = &_gpsFailure; - _navigation_mode_array[7] = &_rcLoss; + _navigation_mode_array[3] = &_dataLinkLoss; + _navigation_mode_array[4] = &_engineFailure; + _navigation_mode_array[5] = &_gpsFailure; + _navigation_mode_array[6] = &_rcLoss; updateParams(); } @@ -282,7 +278,6 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -428,6 +423,7 @@ Navigator::task_main() case NAVIGATION_STATE_POSCTL: case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -462,9 +458,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LANDGPSFAIL: _navigation_mode = &_gpsFailure; break; - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; - break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index dcb5c6000..000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.cpp - * - * Helper class for offboard commands - * - * @author Julian Oes <julian@oes.ch> - */ - -#include <string.h> -#include <stdlib.h> -#include <stdbool.h> -#include <math.h> -#include <fcntl.h> - -#include <mavlink/mavlink_log.h> -#include <systemlib/err.h> - -#include <uORB/uORB.h> -#include <uORB/topics/position_setpoint_triplet.h> - -#include "navigator.h" -#include "offboard.h" - -Offboard::Offboard(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _offboard_control_sp({0}) -{ - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); -} - -Offboard::~Offboard() -{ -} - -void -Offboard::on_inactive() -{ -} - -void -Offboard::on_activation() -{ -} - -void -Offboard::on_active() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool updated; - orb_check(_navigator->get_offboard_control_sp_sub(), &updated); - if (updated) { - update_offboard_control_setpoint(); - } - - /* copy offboard setpoints to the corresponding topics */ - if (_navigator->get_control_mode()->flag_control_position_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - - } else if (_navigator->get_control_mode()->flag_control_velocity_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - -} - - -void -Offboard::update_offboard_control_setpoint() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); - -} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h deleted file mode 100644 index 66b923bdb..000000000 --- a/src/modules/navigator/offboard.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.h - * - * Helper class for offboard commands - * - * @author Julian Oes <julian@oes.ch> - */ - -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H - -#include <controllib/blocks.hpp> -#include <controllib/block/BlockParam.hpp> - -#include <uORB/uORB.h> -#include <uORB/topics/offboard_control_setpoint.h> - -#include "navigator_mode.h" - -class Navigator; - -class Offboard : public NavigatorMode -{ -public: - Offboard(Navigator *navigator, const char *name); - - ~Offboard(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); -private: - void update_offboard_control_setpoint(); - - struct offboard_control_setpoint_s _offboard_control_sp; -}; - -#endif diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index d7b131e3c..72a28e501 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -53,11 +53,42 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, - OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, + OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, + OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +enum OFFBOARD_CONTROL_FRAME { + OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, + OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, + OFFBOARD_CONTROL_FRAME_BODY_NED = 2, + OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, + OFFBOARD_CONTROL_FRAME_GLOBAL = 4 +}; + +/* mappings for the ignore bitmask */ +enum {OFB_IGN_BIT_POS_X, + OFB_IGN_BIT_POS_Y, + OFB_IGN_BIT_POS_Z, + OFB_IGN_BIT_VEL_X, + OFB_IGN_BIT_VEL_Y, + OFB_IGN_BIT_VEL_Z, + OFB_IGN_BIT_ACC_X, + OFB_IGN_BIT_ACC_Y, + OFB_IGN_BIT_ACC_Z, + OFB_IGN_BIT_BODYRATE_X, + OFB_IGN_BIT_BODYRATE_Y, + OFB_IGN_BIT_BODYRATE_Z, + OFB_IGN_BIT_ATT, + OFB_IGN_BIT_THRUST, + OFB_IGN_BIT_YAW, + OFB_IGN_BIT_YAWRATE, }; /** @@ -70,10 +101,21 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - float p1; /**< ailerons roll / roll rate input */ - float p2; /**< elevator / pitch / pitch rate */ - float p3; /**< rudder / yaw rate / yaw */ - float p4; /**< throttle / collective thrust / altitude */ + double position[3]; /**< lat, lon, alt / x, y, z */ + float velocity[3]; /**< x vel, y vel, z vel */ + float acceleration[3]; /**< x acc, y acc, z acc */ + float attitude[4]; /**< attitude of vehicle (quaternion) */ + float attitude_rate[3]; /**< body angular rates (x, y, z) */ + float thrust; /**< thrust */ + float yaw; /**< yaw: this is the yaw from the position_target message + (not from the full attitude_target message) */ + float yaw_rate; /**< yaw rate: this is the yaw from the position_target message + (not from the full attitude_target message) */ + + uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file + for mapping */ + + bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -87,6 +129,147 @@ struct offboard_control_setpoint_s { * @} */ +/** + * Returns true if the position setpoint at index should be ignored + */ +inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); +} + +/** + * Returns true if all position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the velocity setpoint at index should be ignored + */ +inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); +} + +/** + * Returns true if all velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the acceleration setpoint at index should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); +} + +/** + * Returns true if all acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the bodyrate setpoint at index should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); +} + +/** + * Returns true if some of the bodyrate setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the attitude setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); +} + +/** + * Returns true if the thrust setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); +} + +/** + * Returns true if the yaw setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); +} + +/** + * Returns true if the yaw rate setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ec2131abd..cb2262534 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -79,10 +79,17 @@ struct position_setpoint_s double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + bool yaw_valid; /**< true if yaw setpoint valid */ float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ + bool yawspeed_valid; /**< true if yawspeed setpoint valid */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + float a_x; //**< acceleration x setpoint */ + float a_y; //**< acceleration y setpoint */ + float a_z; //**< acceleration z setpoint */ + bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ + bool acceleration_is_force; //*< interprete acceleration as force */ }; /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 44aa50572..f264accbb 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -65,6 +65,9 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b5..ca7705456 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s { bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_force_enabled; /**< true if force control is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 505039d90..a1b2667e3 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -218,6 +218,8 @@ struct vehicle_status_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command + and should not be overridden by RC */ /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; |