Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add support for vtol attitude offboard setpoints #8526

Closed
wants to merge 2 commits into from
Closed

add support for vtol attitude offboard setpoints #8526

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8cda417
add support for vtol attitude offboard setpoints
Boyang--Li Dec 25, 2017
a73a02e
fix coding style
Boyang--Li Dec 26, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 25 additions & 3 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,7 @@
#include <geo/geo.h>

#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>

#include "mavlink_bridge_header.h"
#include "mavlink_receiver.h"
Expand Down Expand Up @@ -136,9 +137,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_debug_vect_pub(nullptr),
_attitude_setpoint_id(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_vehicle_status_sub(orb_subscribe(ORB_ID(vehicle_status))),
_actuator_armed_sub(orb_subscribe(ORB_ID(actuator_armed))),
_global_ref_timestamp(0),
_hil_frames(0),
Expand All @@ -149,6 +152,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_offboard_control_mode{},
_att_sp{},
_rates_sp{},
_vehicle_status{},
_time_offset_avg_alpha(0.8),
_time_offset(0),
_orb_class_instance(-1),
Expand Down Expand Up @@ -1238,10 +1242,28 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode);
}

/* If we are in VTOL mode, publish uORB ID to virtual_setpoint */
bool updated;
orb_check(_vehicle_status_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);

/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}

/* 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) {
Expand All @@ -1267,10 +1289,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}

if (_att_sp_pub == nullptr) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);

} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
}
}

Expand Down
3 changes: 3 additions & 0 deletions src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -244,10 +244,12 @@ class MavlinkReceiver
orb_advert_t _debug_key_value_pub;
orb_advert_t _debug_value_pub;
orb_advert_t _debug_vect_pub;
orb_id_t _attitude_setpoint_id;
static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub;
orb_advert_t _command_ack_pub;
int _control_mode_sub;
int _vehicle_status_sub; /**< vehicle status subscription */
int _actuator_armed_sub;
uint64_t _global_ref_timestamp;
int _hil_frames;
Expand All @@ -258,6 +260,7 @@ class MavlinkReceiver
struct offboard_control_mode_s _offboard_control_mode;
struct vehicle_attitude_setpoint_s _att_sp;
struct vehicle_rates_setpoint_s _rates_sp;
struct vehicle_status_s _vehicle_status; /**< vehicle status */
double _time_offset_avg_alpha;
int64_t _time_offset;
int _orb_class_instance;
Expand Down