dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: -r (datarate) parameter implemented, minor fixes

This commit is contained in:
Anton Babushkin 2014-02-27 18:31:41 +04:00
parent 141982a3ac
commit 2159f948ea
5 changed files with 65 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -5,7 +5,7 @@
echo "Starting MAVLink on this USB console"
mavlink start -b 230400 -d /dev/ttyACM0
mavlink start -r 10000 -d /dev/ttyACM0
# Exit shell to make it available to MAVLink
exit

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -390,14 +390,14 @@ then
if [ $TTYS1_BUSY == yes ]
then
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
mavlink start -d /dev/ttyS0
mavlink start -r 1000 -d /dev/ttyS0
usleep 5000
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Start MAVLink on default port: ttyS1
mavlink start
mavlink start -r 1000
usleep 5000
fi
fi

81
src/modules/mavlink/mavlink_main.cpp
View File

@ -87,6 +87,7 @@
#endif
static const int ERROR = -1;
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz
static Mavlink* _head = nullptr;
@ -449,7 +450,6 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
}
/* open uart */
warnx("UART is %s, baudrate is %d\n", uart_name, baud);
_uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
@ -1407,6 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
int ch;
_baudrate = 57600;
_datarate = 0;
_channel = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
@ -1415,7 +1416,7 @@ Mavlink::task_main(int argc, char *argv[])
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) {
while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
@ -1425,6 +1426,14 @@ Mavlink::task_main(int argc, char *argv[])
break;
case 'r':
_datarate = strtoul(optarg, NULL, 10);
if (_datarate < 10 || _datarate > MAX_DATA_RATE)
errx(1, "invalid data rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
@ -1433,8 +1442,19 @@ Mavlink::task_main(int argc, char *argv[])
// mavlink_link_termination_allowed = true;
// break;
case 'o':
case 'm':
if (strcmp(optarg, "offboard") == 0) {
_mode = MODE_OFFBOARD;
} else if (strcmp(optarg, "onboard") == 0) {
_mode = MODE_ONBOARD;
} else if (strcmp(optarg, "hil") == 0) {
_mode = MODE_HIL;
} else if (strcmp(optarg, "custom") == 0) {
_mode = MODE_CUSTOM;
}
break;
case 'v':
@ -1447,51 +1467,61 @@ Mavlink::task_main(int argc, char *argv[])
}
}
if (_datarate == 0) {
/* convert bits to bytes and use 1/2 of bandwidth by default */
_datarate = _baudrate / 20;
}
if (_datarate > MAX_DATA_RATE) {
_datarate = MAX_DATA_RATE;
}
if (Mavlink::instance_exists(device_name, this)) {
errx(1, "mavlink instance for %s already running", device_name);
}
struct termios uart_config_original;
bool usb_uart;
/* inform about mode */
switch (_mode) {
case MODE_TX_HEARTBEAT_ONLY:
warnx("MODE_TX_HEARTBEAT_ONLY");
case MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MODE_OFFBOARD:
warnx("MODE_OFFBOARD");
warnx("mode: OFFBOARD");
break;
case MODE_ONBOARD:
warnx("MODE_ONBOARD");
warnx("mode: ONBOARD");
break;
case MODE_HIL:
warnx("MODE_HIL");
warnx("mode: HIL");
break;
default:
warnx("Error: Unknown mode");
warnx("ERROR: Unknown mode");
break;
}
switch(_mode) {
case MODE_OFFBOARD:
case MODE_HIL:
case MODE_CUSTOM:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break;
case MODE_TX_HEARTBEAT_ONLY:
default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
warnx("Error: Unknown mode");
break;
}
/* Flush stdout in case MAVLink is about to take it over */
warnx("data rate: %d bytes/s", _datarate);
warnx("port: %s, baudrate: %d", device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
struct termios uart_config_original;
bool usb_uart;
/* default values for arguments */
_uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart);
@ -1529,17 +1559,14 @@ Mavlink::task_main(int argc, char *argv[])
warnx("started");
mavlink_log_info(_mavlink_fd, "[mavlink] started");
/* add default streams depending on mode, intervals depend on baud rate */
float rate_mult = _baudrate / 57600.0f;
if (rate_mult > 4.0f) {
rate_mult = 4.0f;
}
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
add_stream("HEARTBEAT", 1.0f);
switch(_mode) {
case MODE_OFFBOARD:
add_stream("SYS_STATUS", 1.0f * rate_mult);
add_stream("SYS_STATUS", 1.0f);
add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
add_stream("HIGHRES_IMU", 1.0f * rate_mult);
add_stream("ATTITUDE", 10.0f * rate_mult);
@ -1550,13 +1577,14 @@ Mavlink::task_main(int argc, char *argv[])
break;
case MODE_HIL:
add_stream("SYS_STATUS", 1.0f * rate_mult);
add_stream("SYS_STATUS", 1.0f);
add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
add_stream("HIGHRES_IMU", 1.0f * rate_mult);
add_stream("ATTITUDE", 10.0f * rate_mult);
add_stream("GPS_RAW_INT", 1.0f * rate_mult);
add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
add_stream("HIL_CONTROLS", 20.0f * rate_mult);
break;
default:
@ -1703,7 +1731,12 @@ Mavlink::status()
static void usage()
{
errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]");
errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n"
"Supported modes (-m):\n"
"\toffboard\tSend standard telemetry data to ground station (default)\n"
"\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n"
"\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n"
"\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n");
}
int mavlink_main(int argc, char *argv[])

3
src/modules/mavlink/mavlink_main.h
View File

@ -166,7 +166,7 @@ public:
const char *device_name;
enum MAVLINK_MODE {
MODE_TX_HEARTBEAT_ONLY=0,
MODE_CUSTOM=0,
MODE_OFFBOARD,
MODE_ONBOARD,
MODE_HIL
@ -245,6 +245,7 @@ private:
bool _verbose;
int _uart;
int _baudrate;
int _datarate;
/**
* If the queue index is not at 0, the queue sending

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

@ -250,7 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY);
//TODO why do we need this?
//_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY);
/*
* rate control mode - defined by MAVLink