diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-21 11:14:06 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-21 11:14:06 +0200 |
commit | 78637ff74b4c12edca0fac9ea8eb65f38993b49f (patch) | |
tree | 6f505f08ae106b67ddd6722240a41b09be00d39e | |
parent | 950571eaf6b67344b0c46287c45f06e8f85e8ece (diff) | |
download | px4-firmware-78637ff74b4c12edca0fac9ea8eb65f38993b49f.tar.gz px4-firmware-78637ff74b4c12edca0fac9ea8eb65f38993b49f.tar.bz2 px4-firmware-78637ff74b4c12edca0fac9ea8eb65f38993b49f.zip |
mavlink: publish attitude / rates setpoint in offboard control mode
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 72 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 7 |
2 files changed, 74 insertions, 5 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a..561744755 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,16 +102,21 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _control_mode_sub(-1), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0) { + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + memset(&_control_mode, 0, sizeof(_control_mode)); } MavlinkReceiver::~MavlinkReceiver() @@ -349,33 +354,33 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - uint8_t ml_mode = 0; + enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT; bool ml_armed = false; switch (quad_motors_setpoint.mode) { case 0: - ml_armed = false; break; case 1: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + ml_armed = true; break; case 4: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; break; + default: + break; } offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; @@ -388,7 +393,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); + offboard_control_sp.mode = ml_mode; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -398,6 +403,63 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + 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 (_control_mode.flag_control_position_enabled) { + // TODO + + } else if (_control_mode.flag_control_velocity_enabled) { + // TODO + + } else if (_control_mode.flag_control_attitude_enabled) { + /* attitude control */ + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + + att_sp.timestamp = hrt_absolute_time(); + + 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); + } + + } else if (_control_mode.flag_control_rates_enabled) { + /* rates control */ + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_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); + } + + } else { + /* direct control */ + // TODO + } + } + } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..dc31b4c5a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -36,6 +36,7 @@ * MAVLink 1.0 uORB listener definition * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #pragma once @@ -44,6 +45,7 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> @@ -120,6 +122,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -134,10 +137,14 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; + orb_advert_t _att_sp_pub; + orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + + int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; |