From 60dabef75616c7db2e48ce8103dc565ea2327596 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Sep 2012 08:24:08 +0200 Subject: [PATCH] Cleaned up HIL interface --- apps/mavlink/mavlink.c | 222 +++++++++++++++++++---------------------- 1 file changed, 104 insertions(+), 118 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index d9a201d4b0..aa4b39e50d 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -62,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -127,8 +126,6 @@ static struct vehicle_attitude_s hil_attitude; static struct vehicle_global_position_s hil_global_pos; -static struct fixedwing_control_s fw_control; - static struct ardrone_motors_setpoint_s ardrone_motors; static struct vehicle_command_s vcmd; @@ -138,7 +135,6 @@ static struct actuator_armed_s armed; static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; -static int local_pos_sub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; @@ -146,6 +142,8 @@ static bool mavlink_hil_enabled = false; static char mavlink_message_string[51] = {0}; +static int baudrate = 57600; + /* interface mode */ static enum { MAVLINK_INTERFACE_MODE_OFFBOARD, @@ -164,6 +162,10 @@ static struct mavlink_subscriptions { int man_control_sp_sub; int armed_sub; int actuators_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int spg_sub; bool initialized; } mavlink_subs = { .sensor_sub = 0, @@ -177,6 +179,10 @@ static struct mavlink_subscriptions { .man_control_sp_sub = 0, .armed_sub = 0, .actuators_sub = 0, + .local_pos_sub = 0, + .spa_sub = 0, + .spl_sub = 0, + .spg_sub = 0, .initialized = false }; @@ -352,16 +358,34 @@ int set_hil_on_off(bool hil_enabled) printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); - if (pub_hil_attitude > 0 && pub_hil_global_pos > 0) { - mavlink_hil_enabled = true; + mavlink_hil_enabled = true; + /* ramp up some HIL-related subscriptions */ + unsigned hil_rate_interval; + + if (baudrate < 19200) { + /* 10 Hz */ + hil_rate_interval = 100; + } else if (baudrate < 38400) { + /* 10 Hz */ + hil_rate_interval = 100; + } else if (baudrate < 115200) { + /* 20 Hz */ + hil_rate_interval = 50; + } else if (baudrate < 460800) { + /* 50 Hz */ + hil_rate_interval = 20; } else { - ret = ERROR; + /* 100 Hz */ + hil_rate_interval = 10; } + + orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; + orb_set_interval(mavlink_subs.spa_sub, 200); (void)close(pub_hil_attitude); (void)close(pub_hil_global_pos); @@ -585,14 +609,6 @@ static void *uorb_receiveloop(void *arg) fds[fdsc_count].events = POLLIN; fdsc_count++; - // /* --- ARDRONE CONTROL --- */ - // /* subscribe to ORB for AR.Drone controller outputs */ - // int ar_sub = orb_subscribe(ORB_ID(ardrone_control)); - // orb_set_interval(ar_sub, 200); /* 5Hz updates */ - // fds[fdsc_count].fd = ar_sub; - // fds[fdsc_count].events = POLLIN; - // fdsc_count++; - /* --- SYSTEM STATE --- */ /* struct already globally allocated */ /* subscribe to topic */ @@ -611,15 +627,6 @@ static void *uorb_receiveloop(void *arg) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- FIXED WING CONTROL VALUE --- */ - /* struct already globally allocated */ - /* subscribe to ORB for fixed wing control */ - int fw_sub = orb_subscribe(ORB_ID(fixedwing_control)); - orb_set_interval(fw_sub, 50); /* 20 Hz updates */ - fds[fdsc_count].fd = fw_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- GLOBAL POS VALUE --- */ /* struct already globally allocated and topic already subscribed */ fds[fdsc_count].fd = subs->global_pos_sub; @@ -628,34 +635,34 @@ static void *uorb_receiveloop(void *arg) /* --- LOCAL POS VALUE --- */ /* struct and topic already globally subscribed */ - fds[fdsc_count].fd = local_pos_sub; + fds[fdsc_count].fd = subs->local_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL SETPOINT VALUE --- */ /* subscribe to ORB for local setpoint */ /* struct already allocated */ - int spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(spg_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = spg_sub; + subs->spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + orb_set_interval(subs->spg_sub, 2000); /* 0.5 Hz updates */ + fds[fdsc_count].fd = subs->spg_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL SETPOINT VALUE --- */ /* subscribe to ORB for local setpoint */ /* struct already allocated */ - int spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(spl_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = spl_sub; + subs->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(subs->spl_sub, 2000); /* 0.5 Hz updates */ + fds[fdsc_count].fd = subs->spl_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE SETPOINT VALUE --- */ /* subscribe to ORB for attitude setpoint */ /* struct already allocated */ - int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = spa_sub; + subs->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(subs->spa_sub, 2000); /* 0.5 Hz updates */ + fds[fdsc_count].fd = subs->spa_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -820,71 +827,6 @@ static void *uorb_receiveloop(void *arg) // TODO decide where to send channels } - /* --- FIXED WING CONTROL CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy fixed wing control into local buffer */ - orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control); - /* send control output via MAVLink */ - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0], - fw_control.attitude_control_output[1], fw_control.attitude_control_output[2], - fw_control.attitude_control_output[3]); - - /* Only send in HIL mode */ - if (v_status.flag_hil_enabled) { - /* Send the desired attitude from RC or from the autonomous controller */ - // XXX it should not depend on a RC setting, but on a system_state value - - float roll_ail, pitch_elev, throttle, yaw_rudd; - - if (rc.chan[rc.function[OVERRIDE]].scale < 2000) { - - //orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control); - roll_ail = fw_control.attitude_control_output[ROLL]; - pitch_elev = fw_control.attitude_control_output[PITCH]; - throttle = fw_control.attitude_control_output[THROTTLE]; - yaw_rudd = fw_control.attitude_control_output[YAW]; - - } else { - - roll_ail = rc.chan[rc.function[ROLL]].scale; - pitch_elev = rc.chan[rc.function[PITCH]].scale; - throttle = rc.chan[rc.function[THROTTLE]].scale; - yaw_rudd = rc.chan[rc.function[YAW]].scale; - } - - /* hacked HIL implementation in order for the APM Planner to work - * (correct cmd: mavlink_msg_hil_controls_send()) - */ - - mavlink_msg_rc_channels_scaled_send(chan, - hrt_absolute_time(), - 0, // port 0 - roll_ail, - pitch_elev, - throttle, - yaw_rudd, - 0, - 0, - 0, - 0, - 1 /*rssi=1*/); - - /* correct command duplicate */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - roll_ail, - pitch_elev, - yaw_rudd, - throttle, - 0, - 0, - 0, - 0, - 32, /* HIL_MODE */ - 0); - } - } - /* --- VEHICLE GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { /* copy global position data into local buffer */ @@ -900,37 +842,82 @@ static void *uorb_receiveloop(void *arg) /* heading in degrees * 10, from 0 to 36.000) */ uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg); + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, + relative_alt, vx, vy, vz, hdg); } /* --- VEHICLE LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); + orb_copy(ORB_ID(vehicle_local_position), subs->local_pos_sub, &local_pos); + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, + local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); } /* --- VEHICLE GLOBAL SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp); + orb_copy(ORB_ID(vehicle_global_position_setpoint), subs->spg_sub, &buf.global_sp); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw); + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, + buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw); } /* --- VEHICLE LOCAL SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); + orb_copy(ORB_ID(vehicle_local_position_setpoint), subs->spl_sub, &buf.local_sp); + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, + buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); } /* --- VEHICLE ATTITUDE SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, + buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); + + /* Only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* hacked HIL implementation in order for the APM Planner to work + * (correct cmd: mavlink_msg_hil_controls_send()) + */ + + mavlink_msg_rc_channels_scaled_send(chan, + hrt_absolute_time(), + 0, // port 0 + buf.att_sp.roll_body, + buf.att_sp.pitch_body, + buf.att_sp.thrust, + buf.att_sp.yaw_body, + 0, + 0, + 0, + 0, + 1 /*rssi=1*/); + + /* 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(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* correct HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + buf.att_sp.roll_body, + buf.att_sp.pitch_body, + buf.att_sp.yaw_body, + buf.att_sp.thrust, + 0, + 0, + 0, + 0, + mavlink_mode, + 0); + } } /* --- ACTUATOR OUTPUTS 0 --- */ @@ -1284,12 +1271,12 @@ void handleMessage(mavlink_message_t *msg) } } -int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ int speed; - switch (baudrate) { + switch (baud) { case 0: speed = B0; break; case 50: speed = B50; break; @@ -1333,12 +1320,12 @@ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_ case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baudrate); + fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baudrate); + printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -1395,7 +1382,6 @@ int mavlink_thread_main(int argc, char *argv[]) memset(&rc, 0, sizeof(rc)); memset(&hil_attitude, 0, sizeof(hil_attitude)); memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&fw_control, 0, sizeof(fw_control)); memset(&ardrone_motors, 0, sizeof(ardrone_motors)); memset(&vcmd, 0, sizeof(vcmd)); @@ -1407,7 +1393,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* default values for arguments */ char *uart_name = "/dev/ttyS1"; - int baudrate = 57600; + baudrate = 57600; /* read program arguments */ int i; @@ -1467,8 +1453,8 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ /* subscribe to ORB for local position */ - local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ pthread_attr_t receiveloop_attr; @@ -1543,7 +1529,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* get local and global position */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* 1 Hz */ @@ -1640,7 +1626,7 @@ exit_cleanup: /* close subscriptions */ close(mavlink_subs.global_pos_sub); - close(local_pos_sub); + close(mavlink_subs.local_pos_sub); fflush(stdout); fflush(stderr);