void
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);
/* 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);
/* 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);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void
l_vehicle_status(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);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_mode,
v_status.state_machine,
mavlink_state);
}
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
/* initialize mavlink text message buffering */
mavlink_logbuffer_init(&lb, 5);
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
switch (ch) {
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg);
break;
case 'd':
device_name = optarg;
break;
case 'e':
mavlink_link_termination_allowed = true;
break;
case 'o':
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
break;
default:
usage();
}
}
struct termios uart_config_original;
bool usb_uart;
/* print welcome text */
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0)
err(1, "could not open %s", device_name);
/* create the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
/* start the ORB receiver */
uorb_receive_thread = uorb_receive_start();
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
/* 10 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
/* 0.5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
thread_running = true;
/* arm counter to go off immediately */
unsigned lowspeed_counter = 10;
while (!thread_should_exit) {
/* 1 Hz */
if (lowspeed_counter == 10) {
mavlink_update_system();
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
v_status.errors_count2,
v_status.errors_count3,
v_status.errors_count4);
lowspeed_counter = 0;
}
lowspeed_counter++;
/* sleep quarter the time */
usleep(25000);
/* check if waypoint has been reached against the last positions */
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
/* sleep quarter the time */
usleep(25000);
/* send parameters at 20 Hz (if queued for sending) */
mavlink_pm_queued_send();
/* sleep quarter the time */
usleep(25000);
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
/* sleep 10 ms */
usleep(10000);
/* send one string at 10 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
}
}
/* sleep 15 ms */
usleep(15000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
pthread_join(uorb_receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
