fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: publish attitude / rates setpoint in offboard control mode

Browse Source
This commit is contained in:
Anton Babushkin 2014-05-21 11:14:06 +02:00
parent 950571eaf6
commit 78637ff74b
2 changed files with 74 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
src/modules/mavlink/mavlink_receiver.cpp
View File

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

7
src/modules/mavlink/mavlink_receiver.h
View File

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