forked from Archive/PX4-Autopilot
Merge pull request #822 from helenol/helen_mavlink_pull
Added a -w option to mavlink, added vicon messages to stream.
This commit is contained in:
commit
df987f3577
|
@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
int buf_free = 0;
|
||||
|
||||
if (instance->get_flow_control_enabled()
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
|
||||
|
||||
if (buf_free == 0) {
|
||||
|
||||
if (last_write_times[(unsigned)channel] != 0 &&
|
||||
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
|
||||
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
|
||||
|
||||
warnx("DISABLING HARDWARE FLOW CONTROL");
|
||||
instance->enable_flow_control(false);
|
||||
|
@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|||
}
|
||||
}
|
||||
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
|
||||
if (ret != desired) {
|
||||
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
|
||||
/* If the wait until transmit flag is on, only transmit after we've received messages.
|
||||
Otherwise, transmit all the time. */
|
||||
if (instance->should_transmit()) {
|
||||
ssize_t ret = write(uart, ch, desired);
|
||||
if (ret != desired) {
|
||||
// XXX do something here, but change to using FIONWRITE and OS buf size for detection
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void usage(void);
|
||||
|
@ -204,6 +209,8 @@ Mavlink::Mavlink() :
|
|||
_task_running(false),
|
||||
_hil_enabled(false),
|
||||
_is_usb_uart(false),
|
||||
_wait_to_transmit(false),
|
||||
_received_messages(false),
|
||||
_main_loop_delay(1000),
|
||||
_subscriptions(nullptr),
|
||||
_streams(nullptr),
|
||||
|
@ -1768,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
* set error flag instead */
|
||||
bool err_flag = false;
|
||||
|
||||
while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) {
|
||||
while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'b':
|
||||
_baudrate = strtoul(optarg, NULL, 10);
|
||||
|
@ -1820,6 +1827,10 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
_verbose = true;
|
||||
break;
|
||||
|
||||
case 'w':
|
||||
_wait_to_transmit = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
err_flag = true;
|
||||
break;
|
||||
|
@ -2264,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[])
|
|||
|
||||
static void usage()
|
||||
{
|
||||
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]");
|
||||
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
|
||||
}
|
||||
|
||||
int mavlink_main(int argc, char *argv[])
|
||||
|
|
|
@ -202,6 +202,14 @@ public:
|
|||
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
|
||||
/* Functions for waiting to start transmission until message received. */
|
||||
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
|
||||
bool get_has_received_messages() { return _received_messages; }
|
||||
void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
|
||||
bool get_wait_to_transmit() { return _wait_to_transmit; }
|
||||
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
|
||||
|
||||
protected:
|
||||
Mavlink *next;
|
||||
|
||||
|
@ -216,6 +224,8 @@ private:
|
|||
/* states */
|
||||
bool _hil_enabled; /**< Hardware In the Loop 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. */
|
||||
|
||||
unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */
|
||||
|
||||
|
@ -270,6 +280,8 @@ private:
|
|||
|
||||
pthread_mutex_t _message_buffer_mutex;
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Send one parameter.
|
||||
*
|
||||
|
|
|
@ -640,6 +640,47 @@ protected:
|
|||
};
|
||||
|
||||
|
||||
|
||||
class MavlinkStreamViconPositionEstimate : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name()
|
||||
{
|
||||
return "VICON_POSITION_ESTIMATE";
|
||||
}
|
||||
|
||||
MavlinkStream *new_instance()
|
||||
{
|
||||
return new MavlinkStreamViconPositionEstimate();
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *pos_sub;
|
||||
struct vehicle_vicon_position_s *pos;
|
||||
|
||||
protected:
|
||||
void subscribe(Mavlink *mavlink)
|
||||
{
|
||||
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
|
||||
pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
|
||||
}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
if (pos_sub->update(t)) {
|
||||
mavlink_msg_vicon_position_estimate_send(_channel,
|
||||
pos->timestamp / 1000,
|
||||
pos->x,
|
||||
pos->y,
|
||||
pos->z,
|
||||
pos->roll,
|
||||
pos->pitch,
|
||||
pos->yaw);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
|
@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = {
|
|||
new MavlinkStreamAttitudeControls(),
|
||||
new MavlinkStreamNamedValueFloat(),
|
||||
new MavlinkStreamCameraCapture(),
|
||||
new MavlinkStreamViconPositionEstimate(),
|
||||
nullptr
|
||||
};
|
||||
|
|
|
@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* If we've received a valid message, mark the flag indicating so.
|
||||
This is used in the '-w' command-line flag. */
|
||||
_mavlink->set_has_received_messages(true);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
Loading…
Reference in New Issue