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_attitude_controls(struct listener *l)
{
struct actuator_controls_s actuators;
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl0 ",
actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl1 ",
actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl2 ",
actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl3 ",
actuators.control[3]);
}
/* Only send in HIL mode */
if (mavlink_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);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
actuators.control[0],
actuators.control[1],
actuators.control[2],
actuators.control[3],
0,
0,
0,
0,
mavlink_mode,
0);
}
}
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);
}
void
l_actuator_outputs(const struct listener *l)
{
struct actuator_outputs_s act_outputs;
orb_id_t ids[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
/* 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_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
act_outputs.output[0],
act_outputs.output[1],
act_outputs.output[2],
act_outputs.output[3],
act_outputs.output[4],
act_outputs.output[5],
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) {
/* 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);
/* HIL message as per MAVLink spec */
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode,
0);
} else {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 500.0f,
(act_outputs.output[1] - 1500.0f) / 500.0f,
(act_outputs.output[2] - 1500.0f) / 500.0f,
(act_outputs.output[3] - 1000.0f) / 1000.0f,
(act_outputs.output[4] - 1500.0f) / 500.0f,
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
mavlink_mode,
0);
}
}
}
}
/**
* MAVLink Protocol main function.
*/
int mavlink_thread_main(int argc, char *argv[])
{
int ch;
char *device_name = "/dev/ttyS1";
baudrate = 57600;
struct vehicle_status_s v_status;
struct actuator_armed_s armed;
/* 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);
/* Initialize system properties */
mavlink_update_system();
/* start the MAVLink receiver */
receive_thread = receive_start(uart);
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(&v_status, &armed, &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);
/* 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 1000 ms */
usleep(1000000);
}
/* wait for threads to complete */
pthread_join(receive_thread, NULL);
/* Reset the UART flags to original state */
if (!usb_uart)
tcsetattr(uart, TCSANOW, &uart_config_original);
thread_running = false;
exit(0);
}
/**
* 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);
}
void
l_actuator_outputs(struct listener *l)
{
struct actuator_outputs_s act_outputs;
orb_id_t ids[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
/* 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_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
act_outputs.output[0],
act_outputs.output[1],
act_outputs.output[2],
act_outputs.output[3],
act_outputs.output[4],
act_outputs.output[5],
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_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);
float rudder, throttle;
// XXX very ugly, needs rework
if (isfinite(act_outputs.output[3])
&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
/* throttle is fourth output */
rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f;
throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f;
} else {
/* only three outputs, put throttle on position 4 / index 3 */
rudder = 0;
throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f;
}
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 1000.0f,
(act_outputs.output[1] - 1500.0f) / 1000.0f,
rudder,
throttle,
(act_outputs.output[4] - 1500.0f) / 1000.0f,
(act_outputs.output[5] - 1500.0f) / 1000.0f,
(act_outputs.output[6] - 1500.0f) / 1000.0f,
(act_outputs.output[7] - 1500.0f) / 1000.0f,
mavlink_mode,
0);
}
}
}
