fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink: hearbeat sending works now

Browse Source
This commit is contained in:
Julian Oes
2014-02-11 16:16:57 +01:00
parent a5045ccee6
commit 19105bebc5
2 changed files with 73 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+70 -66
View File
@@ -135,9 +135,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
#endif #endif
} }
warnx("uart: %d", uart);
warnx("channel: %d", channel);
ssize_t desired = (sizeof(uint8_t) * length); ssize_t desired = (sizeof(uint8_t) * length);
ssize_t ret = write(uart, ch, desired); ssize_t ret = write(uart, ch, desired);
@@ -155,7 +152,7 @@ namespace mavlink
} }
Mavlink::Mavlink() : Mavlink::Mavlink() :
_mavlink_fd(-1), // _mavlink_fd(-1),
_task_should_exit(false), _task_should_exit(false),
thread_running(false), thread_running(false),
_mavlink_task(-1), _mavlink_task(-1),
@@ -217,12 +214,18 @@ Mavlink* Mavlink::new_instance()
{ {
Mavlink* inst = new Mavlink(); Mavlink* inst = new Mavlink();
Mavlink* next = ::_head; Mavlink* next = ::_head;
while (next != nullptr)
next = next->_next;
/* now parent has a null pointer, fill it */
next = inst;
/* create the first instance at _head */
if (::_head == nullptr) {
::_head = inst;
/* afterwards follow the next and append the instance */
} else {
while (next != nullptr) {
next = next->_next;
}
/* now parent has a null pointer, fill it */
next = inst;
}
return inst; return inst;
} }
@@ -246,7 +249,7 @@ int Mavlink::get_uart_fd(unsigned index)
{ {
Mavlink* inst = get_instance(index); Mavlink* inst = get_instance(index);
if (inst) if (inst)
return inst->_mavlink_fd; return inst->get_uart_fd();
return -1; return -1;
} }
@@ -379,7 +382,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* open uart */ /* open uart */
warnx("UART is %s, baudrate is %d\n", uart_name, baud); warnx("UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY); _uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */ /* Try to set baud rate */
struct termios uart_config; struct termios uart_config;
@@ -387,14 +390,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
*is_usb = false; *is_usb = false;
/* Back up the original uart configuration to restore it after exit */ /* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
close(uart); close(_uart);
return -1; return -1;
} }
/* Fill the struct for the new configuration */ /* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config); tcgetattr(_uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */ /* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR; uart_config.c_oflag &= ~ONLCR;
@@ -405,19 +408,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* Set baud rate */ /* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart); close(_uart);
return -1; return -1;
} }
} }
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart); close(_uart);
return -1; return -1;
} }
return uart; return _uart;
} }
int int
@@ -451,7 +454,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
} }
orb_set_interval(subs.spa_sub, hil_rate_interval); orb_set_interval(subs.spa_sub, hil_rate_interval);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
} }
if (!hil_enabled && _mavlink_hil_enabled) { if (!hil_enabled && _mavlink_hil_enabled) {
@@ -541,50 +544,50 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas
} }
int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
{ {
int ret = OK; int ret = OK;
switch (mavlink_msg_id) { switch (mavlink_msg_id) {
case MAVLINK_MSG_ID_SCALED_IMU: case MAVLINK_MSG_ID_SCALED_IMU:
/* sensor sub triggers scaled IMU */ /* sensor sub triggers scaled IMU */
orb_set_interval(subs->sensor_sub, min_interval); orb_set_interval(subs.sensor_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_HIGHRES_IMU: case MAVLINK_MSG_ID_HIGHRES_IMU:
/* sensor sub triggers highres IMU */ /* sensor sub triggers highres IMU */
orb_set_interval(subs->sensor_sub, min_interval); orb_set_interval(subs.sensor_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_RAW_IMU: case MAVLINK_MSG_ID_RAW_IMU:
/* sensor sub triggers RAW IMU */ /* sensor sub triggers RAW IMU */
orb_set_interval(subs->sensor_sub, min_interval); orb_set_interval(subs.sensor_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
/* attitude sub triggers attitude */ /* attitude sub triggers attitude */
orb_set_interval(subs->att_sub, min_interval); orb_set_interval(subs.att_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
/* actuator_outputs triggers this message */ /* actuator_outputs triggers this message */
orb_set_interval(subs->act_0_sub, min_interval); orb_set_interval(subs.act_0_sub, min_interval);
orb_set_interval(subs->act_1_sub, min_interval); orb_set_interval(subs.act_1_sub, min_interval);
orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs.act_2_sub, min_interval);
orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs.act_3_sub, min_interval);
orb_set_interval(subs->actuators_sub, min_interval); orb_set_interval(subs.actuators_sub, min_interval);
orb_set_interval(subs->actuators_effective_sub, min_interval); orb_set_interval(subs.actuators_effective_sub, min_interval);
orb_set_interval(subs->spa_sub, min_interval); orb_set_interval(subs.spa_sub, min_interval);
orb_set_interval(subs->rates_setpoint_sub, min_interval); orb_set_interval(subs.rates_setpoint_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_MANUAL_CONTROL: case MAVLINK_MSG_ID_MANUAL_CONTROL:
/* manual_control_setpoint triggers this message */ /* manual_control_setpoint triggers this message */
orb_set_interval(subs->man_control_sp_sub, min_interval); orb_set_interval(subs.man_control_sp_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
orb_set_interval(subs->debug_key_value, min_interval); orb_set_interval(subs.debug_key_value, min_interval);
break; break;
default: default:
@@ -1433,7 +1436,7 @@ Mavlink::task_main(int argc, char *argv[])
/* initialize logging device */ /* initialize logging device */
// TODO // TODO
_mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); // _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0);
//mavlink_log_info(_mavlink_fd, "[mavlink] started"); //mavlink_log_info(_mavlink_fd, "[mavlink] started");
@@ -1491,9 +1494,9 @@ Mavlink::task_main(int argc, char *argv[])
fflush(stdout); fflush(stdout);
/* default values for arguments */ /* default values for arguments */
uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0) if (_uart < 0)
err(1, "could not open %s", device_name); err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */ /* create the device node that's used for sending text log messages, etc. */
@@ -1516,57 +1519,57 @@ Mavlink::task_main(int argc, char *argv[])
/* all subscriptions are now active, set up initial guess about rate limits */ /* all subscriptions are now active, set up initial guess about rate limits */
if (_baudrate >= 230400) { if (_baudrate >= 230400) {
/* 200 Hz / 5 ms */ /* 200 Hz / 5 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20);
/* 50 Hz / 20 ms */ /* 50 Hz / 20 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30);
/* 20 Hz / 50 ms */ /* 20 Hz / 50 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 10 Hz */ /* 10 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100);
/* 10 Hz */ /* 10 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (_baudrate >= 115200) { } else if (_baudrate >= 115200) {
/* 20 Hz / 50 ms */ /* 20 Hz / 50 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 5 Hz / 200 ms */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 5 Hz / 200 ms */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (_baudrate >= 57600) { } else if (_baudrate >= 57600) {
/* 10 Hz / 100 ms */ /* 10 Hz / 100 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */ /* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */ /* 5 Hz / 200 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500);
} else { } else {
/* very low baud rate, limit to 1 Hz / 1000 ms */ /* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */ /* 1 Hz / 1000 ms */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
/* 0.5 Hz */ /* 0.5 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */ /* 0.1 Hz */
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
} }
int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -1581,6 +1584,8 @@ Mavlink::task_main(int argc, char *argv[])
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[1]; struct pollfd fds[1];
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */ /* Setup of loop */
fds[0].fd = _params_sub; fds[0].fd = _params_sub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
@@ -1614,7 +1619,6 @@ Mavlink::task_main(int argc, char *argv[])
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */ /* send heartbeat */
warnx("send heartbeat, chan: %d", _chan);
mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
/* switch HIL mode if required */ /* switch HIL mode if required */
@@ -1688,7 +1692,7 @@ Mavlink::task_main(int argc, char *argv[])
pthread_join(uorb_receive_thread, NULL); pthread_join(uorb_receive_thread, NULL);
/* Reset the UART flags to original state */ /* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original); tcsetattr(_uart, TCSANOW, &uart_config_original);
/* destroy log buffer */ /* destroy log buffer */
//mavlink_logbuffer_destroy(&lb); //mavlink_logbuffer_destroy(&lb);
src/modules/mavlink/mavlink_main.h
+3 -4
View File
@@ -170,7 +170,7 @@ public:
static int get_uart_fd(unsigned index); static int get_uart_fd(unsigned index);
int get_uart_fd() { return uart; } int get_uart_fd() { return _uart; }
enum MAVLINK_MODE { enum MAVLINK_MODE {
MODE_TX_HEARTBEAT_ONLY=0, MODE_TX_HEARTBEAT_ONLY=0,
@@ -265,7 +265,6 @@ protected:
* registering clone devices etc. * registering clone devices etc.
*/ */
struct file_operations fops; struct file_operations fops;
int _mavlink_fd;
Mavlink* _next; Mavlink* _next;
private: private:
@@ -302,7 +301,7 @@ private:
mavlink_wpm_storage *wpm; mavlink_wpm_storage *wpm;
bool verbose; bool verbose;
int uart; int _uart;
int _baudrate; int _baudrate;
bool gcs_link; bool gcs_link;
/** /**
@@ -390,7 +389,7 @@ private:
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval);
/** /**
* Callback for param interface. * Callback for param interface.