mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 16:24:08 +08:00
Not building yet, things are coming together slowly on mavlink app
This commit is contained in:
Lorenz Meier
parent
e28910127d
commit
1e3d2acbf6
@ -56,7 +56,6 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
@ -196,6 +195,11 @@ Mavlink::~Mavlink()
|
||||
}
|
||||
}
|
||||
|
||||
void Mavlink::set_mode(enum MAVLINK_MODE mode)
|
||||
{
|
||||
_mode = mode;
|
||||
}
|
||||
|
||||
int Mavlink::instance_count()
|
||||
{
|
||||
/* note: a local buffer count will help if this ever is called often */
|
||||
@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
|
||||
_mode = MODE_ONBOARD;
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
warnx("MAVLink v1.0 serial interface starting...");
|
||||
|
||||
/* inform about mode */
|
||||
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
|
||||
warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
|
||||
|
||||
/* Flush stdout in case MAVLink is about to take it over */
|
||||
fflush(stdout);
|
||||
@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
mavlink_update_system();
|
||||
|
||||
/* start the MAVLink receiver */
|
||||
MavlinkReceiver rcv(this);
|
||||
receive_thread = rcv.receive_start(uart);
|
||||
// MavlinkReceiver rcv(this);
|
||||
receive_thread = MavlinkReceiver::receive_start(this);
|
||||
|
||||
/* start the ORB receiver */
|
||||
MavlinkOrbListener listener(this);
|
||||
uorb_receive_thread = listener.uorb_receive_start();
|
||||
//MavlinkOrbListener listener(this);
|
||||
uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
|
||||
|
||||
/* initialize waypoint manager */
|
||||
mavlink_wpm_init(wpm);
|
||||
|
||||
@ -54,9 +54,7 @@
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
@ -72,7 +70,8 @@
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include <nuttx/fs/fs.h>
|
||||
|
||||
@ -139,6 +138,7 @@ struct mavlink_wpm_storage {
|
||||
|
||||
class Mavlink
|
||||
{
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
@ -170,6 +170,31 @@ public:
|
||||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
int get_uart_fd() { return _mavlink_fd; }
|
||||
|
||||
enum MAVLINK_MODE {
|
||||
MODE_TX_HEARTBEAT_ONLY=0,
|
||||
MODE_OFFBOARD,
|
||||
MODE_ONBOARD,
|
||||
MODE_HIL
|
||||
};
|
||||
|
||||
void set_mode(enum MAVLINK_MODE);
|
||||
enum MAVLINK_MODE get_mode();
|
||||
|
||||
bool hil_enabled() { return _mavlink_hil_enabled; };
|
||||
|
||||
/**
|
||||
* Handle waypoint related messages.
|
||||
*/
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Handle parameter related messages.
|
||||
*/
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
||||
|
||||
struct mavlink_subscriptions {
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
@ -196,10 +221,15 @@ public:
|
||||
int airspeed_sub;
|
||||
int navigation_capabilities_sub;
|
||||
int control_mode_sub;
|
||||
int rc_sub;
|
||||
int status_sub;
|
||||
};
|
||||
|
||||
struct mavlink_subscriptions subs;
|
||||
|
||||
struct mavlink_subscriptions* get_subs() { return &subs; }
|
||||
mavlink_channel_t get_chan() { return chan; }
|
||||
|
||||
/** Global position */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
/** Local position */
|
||||
@ -243,6 +273,7 @@ private:
|
||||
uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
|
||||
bool thread_running;
|
||||
bool thread_should_exit;
|
||||
MAVLINK_MODE _mode;
|
||||
|
||||
uint8_t mavlink_wpm_comp_id;
|
||||
mavlink_channel_t chan;
|
||||
@ -269,12 +300,6 @@ private:
|
||||
*/
|
||||
unsigned int mavlink_param_queue_index;
|
||||
|
||||
/* interface mode */
|
||||
enum {
|
||||
MAVLINK_INTERFACE_MODE_OFFBOARD,
|
||||
MAVLINK_INTERFACE_MODE_ONBOARD
|
||||
} mavlink_link_mode;
|
||||
|
||||
struct mavlink_logbuffer lb;
|
||||
bool mavlink_link_termination_allowed;
|
||||
|
||||
@ -293,12 +318,6 @@ private:
|
||||
*/
|
||||
int set_hil_on_off(bool hil_enabled);
|
||||
|
||||
|
||||
/**
|
||||
* Handle parameter related messages.
|
||||
*/
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Send all parameters at once.
|
||||
*
|
||||
@ -352,7 +371,6 @@ private:
|
||||
|
||||
void mavlink_update_system();
|
||||
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
|
||||
void mavlink_waypoint_eventloop(uint64_t now);
|
||||
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
|
||||
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
|
||||
|
||||
@ -62,11 +62,7 @@
|
||||
|
||||
void *uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener {
|
||||
void (*callback)(const struct listener *l);
|
||||
int *subp;
|
||||
uintptr_t arg;
|
||||
};
|
||||
|
||||
|
||||
uint16_t cm_uint16_from_m_float(float m);
|
||||
|
||||
@ -86,41 +82,58 @@ cm_uint16_from_m_float(float m)
|
||||
|
||||
MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
|
||||
|
||||
_task_should_exit(false),
|
||||
thread_should_exit(false),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
|
||||
_mavlink(parent),
|
||||
_listeners(nullptr),
|
||||
_n_listeners(0)
|
||||
{
|
||||
static const struct listener listeners[] = {
|
||||
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
|
||||
{l_vehicle_attitude, &mavlink_subs.att_sub, 0},
|
||||
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
|
||||
{l_vehicle_status, &status_sub, 0},
|
||||
{l_rc_channels, &rc_sub, 0},
|
||||
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
|
||||
{l_global_position, &mavlink_subs.global_pos_sub, 0},
|
||||
{l_local_position, &mavlink_subs.local_pos_sub, 0},
|
||||
{l_global_position_setpoint, &mavlink_subs.triplet_sub, 0},
|
||||
{l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
|
||||
{l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
|
||||
{l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
|
||||
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
|
||||
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
|
||||
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
|
||||
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
|
||||
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
|
||||
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
|
||||
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
|
||||
{l_optical_flow, &mavlink_subs.optical_flow, 0},
|
||||
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
|
||||
{l_home, &mavlink_subs.home_sub, 0},
|
||||
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
|
||||
{l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0},
|
||||
{l_control_mode, &mavlink_subs.control_mode_sub, 0},
|
||||
};
|
||||
add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1);
|
||||
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2);
|
||||
add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3);
|
||||
add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0);
|
||||
add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0);
|
||||
add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0);
|
||||
add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0);
|
||||
|
||||
_n_listeners = sizeof(listeners) / sizeof(listeners[0]);
|
||||
}
|
||||
|
||||
void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg)
|
||||
{
|
||||
struct listener *nl = new listener;
|
||||
|
||||
nl->callback = callback;
|
||||
nl->subp = subp;
|
||||
nl->arg = arg;
|
||||
nl->next = nullptr;
|
||||
|
||||
// Register it
|
||||
struct listener *next = _listeners;
|
||||
while (next->next != nullptr) {
|
||||
next = next->next;
|
||||
}
|
||||
|
||||
// Attach
|
||||
next->next = nl;
|
||||
_n_listeners++;
|
||||
}
|
||||
|
||||
void
|
||||
@ -129,43 +142,37 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l)
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
/* copy sensors raw data into local buffer */
|
||||
orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
|
||||
orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
|
||||
|
||||
last_sensor_timestamp = raw.timestamp;
|
||||
|
||||
/* mark individual fields as changed */
|
||||
/* mark individual fields as _mavlink->get_chan()ged */
|
||||
uint16_t fields_updated = 0;
|
||||
static unsigned accel_counter = 0;
|
||||
static unsigned gyro_counter = 0;
|
||||
static unsigned mag_counter = 0;
|
||||
static unsigned baro_counter = 0;
|
||||
|
||||
if (accel_counter != raw.accelerometer_counter) {
|
||||
/* mark first three dimensions as changed */
|
||||
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
accel_counter = raw.accelerometer_counter;
|
||||
}
|
||||
// if (accel_counter != raw.accelerometer_counter) {
|
||||
// /* mark first three dimensions as _mavlink->get_chan()ged */
|
||||
// fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
// accel_counter = raw.accelerometer_counter;
|
||||
// }
|
||||
|
||||
if (gyro_counter != raw.gyro_counter) {
|
||||
/* mark second group dimensions as changed */
|
||||
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
gyro_counter = raw.gyro_counter;
|
||||
}
|
||||
// if (gyro_counter != raw.gyro_counter) {
|
||||
// /* mark second group dimensions as _mavlink->get_chan()ged */
|
||||
// fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
// gyro_counter = raw.gyro_counter;
|
||||
// }
|
||||
|
||||
if (mag_counter != raw.magnetometer_counter) {
|
||||
/* mark third group dimensions as changed */
|
||||
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
mag_counter = raw.magnetometer_counter;
|
||||
}
|
||||
// if (mag_counter != raw.magnetometer_counter) {
|
||||
// /* mark third group dimensions as _mavlink->get_chan()ged */
|
||||
// fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
// mag_counter = raw.magnetometer_counter;
|
||||
// }
|
||||
|
||||
if (baro_counter != raw.baro_counter) {
|
||||
/* mark last group dimensions as changed */
|
||||
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
baro_counter = raw.baro_counter;
|
||||
}
|
||||
// if (baro_counter != raw.baro_counter) {
|
||||
// /* mark last group dimensions as _mavlink->get_chan()ged */
|
||||
// fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
// baro_counter = raw.baro_counter;
|
||||
// }
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp,
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
|
||||
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
|
||||
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
|
||||
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
|
||||
@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l)
|
||||
raw.baro_alt_meter, raw.baro_temp_celcius,
|
||||
fields_updated);
|
||||
|
||||
sensors_raw_counter++;
|
||||
l->listener->sensors_raw_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
|
||||
{
|
||||
/* copy attitude data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
|
||||
orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att);
|
||||
|
||||
if (gcs_link) {
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
|
||||
/* send sensor values */
|
||||
mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
att.roll,
|
||||
att.pitch,
|
||||
att.yaw,
|
||||
att.rollspeed,
|
||||
att.pitchspeed,
|
||||
att.yawspeed);
|
||||
mavlink_msg_attitude_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
l->listener->att.roll,
|
||||
l->listener->att.pitch,
|
||||
l->listener->att.yaw,
|
||||
l->listener->att.rollspeed,
|
||||
l->listener->att.pitchspeed,
|
||||
l->listener->att.yawspeed);
|
||||
|
||||
/* limit VFR message rate to 10Hz */
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
if (t >= last_sent_vfr + 100000) {
|
||||
last_sent_vfr = t;
|
||||
float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
|
||||
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
|
||||
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
|
||||
mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
|
||||
if (t >= l->listener->last_sent_vfr + 100000) {
|
||||
l->listener->last_sent_vfr = t;
|
||||
float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e);
|
||||
uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F;
|
||||
float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f;
|
||||
mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d);
|
||||
}
|
||||
|
||||
/* send quaternion values if it exists */
|
||||
if(att.q_valid) {
|
||||
mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
att.q[0],
|
||||
att.q[1],
|
||||
att.q[2],
|
||||
att.q[3],
|
||||
att.rollspeed,
|
||||
att.pitchspeed,
|
||||
att.yawspeed);
|
||||
if(l->listener->att.q_valid) {
|
||||
mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
l->listener->att.q[0],
|
||||
l->listener->att.q[1],
|
||||
l->listener->att.q[2],
|
||||
l->listener->att.q[3],
|
||||
l->listener->att.rollspeed,
|
||||
l->listener->att.pitchspeed,
|
||||
l->listener->att.yawspeed);
|
||||
}
|
||||
}
|
||||
|
||||
attitude_counter++;
|
||||
l->listener->attitude_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
@ -228,13 +235,13 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
|
||||
struct vehicle_gps_position_s gps;
|
||||
|
||||
/* copy gps data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
|
||||
orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps);
|
||||
|
||||
/* GPS COG is 0..2PI in degrees * 1e2 */
|
||||
float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
|
||||
|
||||
/* GPS position */
|
||||
mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_chan(),
|
||||
mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
|
||||
gps.timestamp_position,
|
||||
gps.fix_type,
|
||||
gps.lat,
|
||||
@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
|
||||
gps.satellites_visible);
|
||||
|
||||
/* update SAT info every 10 seconds */
|
||||
if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
|
||||
mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(),
|
||||
if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) {
|
||||
mavlink_msg_gps_status_send(l->mavlink->get_chan(),
|
||||
gps.satellites_visible,
|
||||
gps.satellite_prn,
|
||||
gps.satellite_used,
|
||||
@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
|
||||
gps.satellite_snr);
|
||||
}
|
||||
|
||||
gps_counter++;
|
||||
l->listener->gps_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_vehicle_status(const struct listener *l)
|
||||
{
|
||||
/* immediately communicate state changes back to user */
|
||||
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
|
||||
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||
/* immediately communicate state _mavlink->get_chan()ges back to user */
|
||||
orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status);
|
||||
orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
|
||||
|
||||
/* enable or disable HIL */
|
||||
if (v_status.hil_state == HIL_STATE_ON)
|
||||
set_hil_on_off(true);
|
||||
else if (v_status.hil_state == HIL_STATE_OFF)
|
||||
set_hil_on_off(false);
|
||||
if (l->listener->v_status.hil_state == HIL_STATE_ON)
|
||||
l->mavlink->set_hil_on_off(true);
|
||||
else if (l->listener->v_status.hil_state == HIL_STATE_OFF)
|
||||
l->mavlink->set_hil_on_off(false);
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_base_mode = 0;
|
||||
uint32_t mavlink_custom_mode = 0;
|
||||
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan,
|
||||
mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
|
||||
mavlink_system.type,
|
||||
MAV_AUTOPILOT_PX4,
|
||||
mavlink_base_mode,
|
||||
@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_rc_channels(const struct listener *l)
|
||||
MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l)
|
||||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
// XXX Add RC channels scaled message here
|
||||
/* copy rc _mavlink->get_chan()nels into local buffer */
|
||||
orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc);
|
||||
// XXX Add RC _mavlink->get_chan()nels scaled message here
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_input_rc(const struct listener *l)
|
||||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
|
||||
/* copy rc _mavlink->get_chan()nels into local buffer */
|
||||
orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw);
|
||||
|
||||
if (gcs_link) {
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
|
||||
|
||||
const unsigned port_width = 8;
|
||||
|
||||
for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
|
||||
for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(chan,
|
||||
rc_raw.timestamp / 1000,
|
||||
mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(),
|
||||
l->listener->rc_raw.timestamp / 1000,
|
||||
i,
|
||||
(rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
|
||||
(rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
|
||||
rc_raw.rssi);
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
|
||||
(l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
|
||||
l->listener->rc_raw.rssi);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -328,48 +335,48 @@ void
|
||||
MavlinkOrbListener::l_global_position(const struct listener *l)
|
||||
{
|
||||
/* copy global position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
|
||||
orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos);
|
||||
|
||||
mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(),
|
||||
global_pos.timestamp / 1000,
|
||||
global_pos.lat * 1e7,
|
||||
global_pos.lon * 1e7,
|
||||
global_pos.alt * 1000.0f,
|
||||
(global_pos.alt - home.alt) * 1000.0f,
|
||||
global_pos.vel_n * 100.0f,
|
||||
global_pos.vel_e * 100.0f,
|
||||
global_pos.vel_d * 100.0f,
|
||||
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
|
||||
mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
|
||||
l->listener->global_pos.timestamp / 1000,
|
||||
l->listener->global_pos.lat * 1e7,
|
||||
l->listener->global_pos.lon * 1e7,
|
||||
l->listener->global_pos.alt * 1000.0f,
|
||||
(l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f,
|
||||
l->listener->global_pos.vel_n * 100.0f,
|
||||
l->listener->global_pos.vel_e * 100.0f,
|
||||
l->listener->global_pos.vel_d * 100.0f,
|
||||
_wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_local_position(const struct listener *l)
|
||||
{
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
|
||||
orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(),
|
||||
local_pos.timestamp / 1000,
|
||||
local_pos.x,
|
||||
local_pos.y,
|
||||
local_pos.z,
|
||||
local_pos.vx,
|
||||
local_pos.vy,
|
||||
local_pos.vz);
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
|
||||
l->listener->local_pos.timestamp / 1000,
|
||||
l->listener->local_pos.x,
|
||||
l->listener->local_pos.y,
|
||||
l->listener->local_pos.z,
|
||||
l->listener->local_pos.vx,
|
||||
l->listener->local_pos.vy,
|
||||
l->listener->local_pos.vz);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
|
||||
{
|
||||
struct position_setpoint_triplet_s triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet);
|
||||
|
||||
if (!triplet.current.valid)
|
||||
return;
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(),
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(),
|
||||
MAV_FRAME_GLOBAL,
|
||||
(int32_t)(triplet.current.lat * 1e7d),
|
||||
(int32_t)(triplet.current.lon * 1e7d),
|
||||
@ -383,10 +390,10 @@ MavlinkOrbListener::l_local_position_setpoint(const struct listener *l)
|
||||
struct vehicle_local_position_setpoint_s local_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
|
||||
orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(),
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(),
|
||||
MAV_FRAME_LOCAL_NED,
|
||||
local_sp.x,
|
||||
local_sp.y,
|
||||
@ -400,10 +407,10 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
|
||||
att_sp.timestamp / 1000,
|
||||
att_sp.roll_body,
|
||||
att_sp.pitch_body,
|
||||
@ -417,10 +424,10 @@ MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l)
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(),
|
||||
rates_sp.timestamp / 1000,
|
||||
rates_sp.roll,
|
||||
rates_sp.pitch,
|
||||
@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
/* copy actuator data into local buffer */
|
||||
orb_copy(ids[l->arg], *l->subp, &act_outputs);
|
||||
|
||||
if (gcs_link) {
|
||||
mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000,
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
|
||||
mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
|
||||
l->arg /* port number - needs GCS support */,
|
||||
/* QGC has port number support already */
|
||||
act_outputs.output[0],
|
||||
@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
act_outputs.output[7]);
|
||||
|
||||
/* only send in HIL mode and only send first group for HIL */
|
||||
if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
|
||||
if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
/* scale / assign outputs depending on system type */
|
||||
|
||||
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
0);
|
||||
|
||||
} else {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
|
||||
hrt_absolute_time(),
|
||||
(act_outputs.output[0] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[1] - 1500.0f) / 500.0f,
|
||||
@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
|
||||
void
|
||||
MavlinkOrbListener::l_actuator_armed(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||
orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
|
||||
}
|
||||
|
||||
void
|
||||
@ -541,10 +548,10 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
|
||||
struct manual_control_setpoint_s man_control;
|
||||
|
||||
/* copy manual control data into local buffer */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
|
||||
orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(),
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
|
||||
mavlink_msg_manual_control_send(l->mavlink->get_chan(),
|
||||
mavlink_system.sysid,
|
||||
man_control.roll * 1000,
|
||||
man_control.pitch * 1000,
|
||||
@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
|
||||
void
|
||||
MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
|
||||
|
||||
if (gcs_link) {
|
||||
if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
|
||||
/* send, add spaces so that string buffer is at least 10 chars long */
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
"ctrl0 ",
|
||||
actuators_0.control[0]);
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
l->listener->actuators_0.control[0]);
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
"ctrl1 ",
|
||||
actuators_0.control[1]);
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
l->listener->actuators_0.control[1]);
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
"ctrl2 ",
|
||||
actuators_0.control[2]);
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
l->listener->actuators_0.control[2]);
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
"ctrl3 ",
|
||||
actuators_0.control[3]);
|
||||
l->listener->actuators_0.control[3]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -584,13 +591,13 @@ MavlinkOrbListener::l_debug_key_value(const struct listener *l)
|
||||
{
|
||||
struct debug_key_value_s debug;
|
||||
|
||||
orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
|
||||
orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
|
||||
|
||||
/* Enforce null termination */
|
||||
debug.key[sizeof(debug.key) - 1] = '\0';
|
||||
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
last_sensor_timestamp / 1000,
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
l->listener->last_sensor_timestamp / 1000,
|
||||
debug.key,
|
||||
debug.value);
|
||||
}
|
||||
@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
|
||||
{
|
||||
struct optical_flow_s flow;
|
||||
|
||||
orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
|
||||
orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow);
|
||||
|
||||
mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
|
||||
mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
|
||||
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_home(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
|
||||
orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home);
|
||||
|
||||
mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
|
||||
mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_airspeed(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
|
||||
orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed);
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_nav_cap(const struct listener *l)
|
||||
{
|
||||
|
||||
orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
|
||||
orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
|
||||
|
||||
mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
|
||||
mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
|
||||
hrt_absolute_time() / 1000,
|
||||
"turn dist",
|
||||
nav_cap.turn_distance);
|
||||
l->listener->nav_cap.turn_distance);
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkOrbListener::l_control_mode(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
|
||||
orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode);
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_base_mode = 0;
|
||||
uint32_t mavlink_custom_mode = 0;
|
||||
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan,
|
||||
mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
|
||||
mavlink_system.type,
|
||||
MAV_AUTOPILOT_PX4,
|
||||
mavlink_base_mode,
|
||||
@ -657,7 +664,9 @@ void *
|
||||
MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
|
||||
char buf[32];
|
||||
sprintf(buf, "mavlink rcv%d", Mavlink::instance_count());
|
||||
prctl(PR_SET_NAME, buf, getpid());
|
||||
|
||||
/*
|
||||
* set up poll to block for new data,
|
||||
@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
*
|
||||
* Might want to invoke each listener once to set initial state.
|
||||
*/
|
||||
struct pollfd fds[_n_listeners];
|
||||
struct pollfd fds[_max_listeners];
|
||||
|
||||
for (unsigned i = 0; i < _n_listeners; i++) {
|
||||
fds[i].fd = *listeners[i].subp;
|
||||
struct listener* next = _listeners;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
|
||||
fds[i].fd = *next->subp;
|
||||
fds[i].events = POLLIN;
|
||||
|
||||
next = next->next;
|
||||
i++;
|
||||
}
|
||||
/* Invoke callback to set initial state */
|
||||
//listeners[i].callback(&listener[i]);
|
||||
}
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
/* silent */
|
||||
|
||||
} else if (poll_ret < 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
|
||||
//mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
|
||||
|
||||
} else {
|
||||
|
||||
for (unsigned i = 0; i < _n_listeners; i++) {
|
||||
unsigned i = 0;
|
||||
struct listener* cb = _listeners;
|
||||
while (cb != nullptr) {
|
||||
|
||||
if (fds[i].revents & POLLIN)
|
||||
listeners[i].callback(&listeners[i]);
|
||||
cb->callback(cb);
|
||||
|
||||
cb = cb->next;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
MavlinkOrbListener::uorb_receive_start(void)
|
||||
void * MavlinkOrbListener::uorb_start_helper(void *context)
|
||||
{
|
||||
return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL);
|
||||
}
|
||||
|
||||
pthread_t
|
||||
MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
|
||||
{
|
||||
MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink);
|
||||
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* rate limit set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- ATTITUDE VALUE --- */
|
||||
mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
/* rate limit set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->att_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- GPS VALUE --- */
|
||||
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */
|
||||
mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- HOME POSITION --- */
|
||||
mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */
|
||||
mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */
|
||||
|
||||
/* --- SYSTEM STATE --- */
|
||||
status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
|
||||
mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */
|
||||
|
||||
/* --- CONTROL MODE --- */
|
||||
mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */
|
||||
mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */
|
||||
|
||||
/* --- RC CHANNELS VALUE --- */
|
||||
rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
orb_set_interval(rc_sub, 100); /* 10Hz updates */
|
||||
mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- RC RAW VALUE --- */
|
||||
mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
orb_set_interval(mavlink_subs.input_rc_sub, 100);
|
||||
mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
orb_set_interval(mavlink->get_subs()->input_rc_sub, 100);
|
||||
|
||||
/* --- GLOBAL POS VALUE --- */
|
||||
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
|
||||
mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */
|
||||
|
||||
/* --- LOCAL POS VALUE --- */
|
||||
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
|
||||
mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */
|
||||
|
||||
/* --- GLOBAL SETPOINT VALUE --- */
|
||||
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
|
||||
mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- LOCAL SETPOINT VALUE --- */
|
||||
mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
|
||||
orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */
|
||||
mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
|
||||
orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- ATTITUDE SETPOINT VALUE --- */
|
||||
mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */
|
||||
mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- RATES SETPOINT VALUE --- */
|
||||
mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */
|
||||
mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- ACTUATOR OUTPUTS --- */
|
||||
mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
|
||||
mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
|
||||
mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
|
||||
mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
|
||||
mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
|
||||
mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
|
||||
mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
|
||||
mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
|
||||
/* rate limits set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- ACTUATOR ARMED VALUE --- */
|
||||
mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
|
||||
mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- MAPPED MANUAL CONTROL INPUTS --- */
|
||||
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
/* rate limits set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- ACTUATOR CONTROL VALUE --- */
|
||||
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
|
||||
mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- DEBUG VALUE OUTPUT --- */
|
||||
mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
|
||||
orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */
|
||||
mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
|
||||
orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */
|
||||
|
||||
/* --- FLOW SENSOR --- */
|
||||
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
|
||||
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
|
||||
mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow));
|
||||
orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */
|
||||
|
||||
/* --- AIRSPEED --- */
|
||||
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
|
||||
mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- NAVIGATION CAPABILITIES --- */
|
||||
mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
|
||||
orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */
|
||||
nav_cap.turn_distance = 0.0f;
|
||||
mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
|
||||
orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */
|
||||
|
||||
/* start the listener loop */
|
||||
pthread_attr_t uorb_attr;
|
||||
@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void)
|
||||
pthread_attr_setstacksize(&uorb_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
|
||||
pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv);
|
||||
|
||||
pthread_attr_destroy(&uorb_attr);
|
||||
return thread;
|
||||
|
||||
@ -38,8 +38,40 @@
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkOrbListener
|
||||
@ -67,18 +99,32 @@ public:
|
||||
*/
|
||||
void status();
|
||||
|
||||
pthread_t uorb_receive_start(void);
|
||||
static pthread_t uorb_receive_start(Mavlink *mavlink);
|
||||
void * uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener {
|
||||
void (*callback)(const struct listener *l);
|
||||
int *subp;
|
||||
uintptr_t arg;
|
||||
struct listener* next;
|
||||
Mavlink *mavlink;
|
||||
MavlinkOrbListener* listener;
|
||||
};
|
||||
|
||||
void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg);
|
||||
static void * uorb_start_helper(void *context);
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
bool thread_should_exit; /**< if true, sensor task should exit */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
Mavlink* _mavlink;
|
||||
|
||||
struct listener *_listeners;
|
||||
unsigned _n_listeners;
|
||||
static const unsigned _max_listeners = 32;
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
@ -90,28 +136,28 @@ private:
|
||||
*/
|
||||
void task_main() __attribute__((noreturn));
|
||||
|
||||
void l_sensor_combined(const struct listener *l);
|
||||
void l_vehicle_attitude(const struct listener *l);
|
||||
void l_vehicle_gps_position(const struct listener *l);
|
||||
void l_vehicle_status(const struct listener *l);
|
||||
void l_rc_channels(const struct listener *l);
|
||||
void l_input_rc(const struct listener *l);
|
||||
void l_global_position(const struct listener *l);
|
||||
void l_local_position(const struct listener *l);
|
||||
void l_global_position_setpoint(const struct listener *l);
|
||||
void l_local_position_setpoint(const struct listener *l);
|
||||
void l_attitude_setpoint(const struct listener *l);
|
||||
void l_actuator_outputs(const struct listener *l);
|
||||
void l_actuator_armed(const struct listener *l);
|
||||
void l_manual_control_setpoint(const struct listener *l);
|
||||
void l_vehicle_attitude_controls(const struct listener *l);
|
||||
void l_debug_key_value(const struct listener *l);
|
||||
void l_optical_flow(const struct listener *l);
|
||||
void l_vehicle_rates_setpoint(const struct listener *l);
|
||||
void l_home(const struct listener *l);
|
||||
void l_airspeed(const struct listener *l);
|
||||
void l_nav_cap(const struct listener *l);
|
||||
void l_control_mode(const struct listener *l);
|
||||
static void l_sensor_combined(const struct listener *l);
|
||||
static void l_vehicle_attitude(const struct listener *l);
|
||||
static void l_vehicle_gps_position(const struct listener *l);
|
||||
static void l_vehicle_status(const struct listener *l);
|
||||
static void l_rc_channels(const struct listener *l);
|
||||
static void l_input_rc(const struct listener *l);
|
||||
static void l_global_position(const struct listener *l);
|
||||
static void l_local_position(const struct listener *l);
|
||||
static void l_global_position_setpoint(const struct listener *l);
|
||||
static void l_local_position_setpoint(const struct listener *l);
|
||||
static void l_attitude_setpoint(const struct listener *l);
|
||||
static void l_actuator_outputs(const struct listener *l);
|
||||
static void l_actuator_armed(const struct listener *l);
|
||||
static void l_manual_control_setpoint(const struct listener *l);
|
||||
static void l_vehicle_attitude_controls(const struct listener *l);
|
||||
static void l_debug_key_value(const struct listener *l);
|
||||
static void l_optical_flow(const struct listener *l);
|
||||
static void l_vehicle_rates_setpoint(const struct listener *l);
|
||||
static void l_home(const struct listener *l);
|
||||
static void l_airspeed(const struct listener *l);
|
||||
static void l_nav_cap(const struct listener *l);
|
||||
static void l_control_mode(const struct listener *l);
|
||||
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct vehicle_local_position_s local_pos;
|
||||
|
||||
@ -83,7 +83,8 @@ __BEGIN_DECLS
|
||||
|
||||
__END_DECLS
|
||||
|
||||
void MavlinkReceiver::MavlinkReceiver() :
|
||||
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_mavlink(parent),
|
||||
pub_hil_global_pos(-1),
|
||||
pub_hil_local_pos(-1),
|
||||
pub_hil_attitude(-1),
|
||||
@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
void *
|
||||
MavlinkReceiver::receive_thread(void *arg)
|
||||
{
|
||||
int uart_fd = *((int *)arg);
|
||||
int uart_fd = _mavlink->get_uart_fd();
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t buf[32];
|
||||
@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg)
|
||||
handle_message(&msg);
|
||||
|
||||
/* handle packet with waypoint component */
|
||||
mavlink_wpm_message_handler(&msg);
|
||||
_mavlink->mavlink_wpm_message_handler(&msg);
|
||||
|
||||
/* handle packet with parameter component */
|
||||
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
_mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -839,15 +840,22 @@ void MavlinkReceiver::print_status()
|
||||
|
||||
}
|
||||
|
||||
pthread_t
|
||||
MavlinkReceiver::receive_start(int uart)
|
||||
void * MavlinkReceiver::start_helper(void *context)
|
||||
{
|
||||
return ((MavlinkReceiver *)context)->receive_thread(NULL);
|
||||
}
|
||||
|
||||
pthread_t
|
||||
MavlinkReceiver::receive_start(Mavlink *mavlink)
|
||||
{
|
||||
MavlinkReceiver *rcv = new MavlinkReceiver(mavlink);
|
||||
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
// set to non-blocking read
|
||||
int flags = fcntl(uart, F_GETFL, 0);
|
||||
fcntl(uart, F_SETFL, flags | O_NONBLOCK);
|
||||
int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0);
|
||||
fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&receiveloop_attr, ¶m);
|
||||
@ -857,7 +865,7 @@ MavlinkReceiver::receive_start(int uart)
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 3000);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
|
||||
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv);
|
||||
|
||||
pthread_attr_destroy(&receiveloop_attr);
|
||||
return thread;
|
||||
|
||||
@ -96,7 +96,9 @@ public:
|
||||
*/
|
||||
void print_status();
|
||||
|
||||
pthread_t receive_start(int uart);
|
||||
static pthread_t receive_start(Mavlink* mavlink);
|
||||
|
||||
static void * start_helper(void *context);
|
||||
|
||||
private:
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user