diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-18 16:48:43 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-18 16:48:43 +0200 |
commit | dc484c1d21fcb7bc4f4be97853647321e8a147e1 (patch) | |
tree | 4b51a2ff911967318d725bf58bed77b1247468b5 /apps/mavlink/mavlink.c | |
parent | e707574f73b8ed0e9103fd999bd43847c657b5e1 (diff) | |
download | px4-firmware-dc484c1d21fcb7bc4f4be97853647321e8a147e1.tar.gz px4-firmware-dc484c1d21fcb7bc4f4be97853647321e8a147e1.tar.bz2 px4-firmware-dc484c1d21fcb7bc4f4be97853647321e8a147e1.zip |
State machine cleanup, introduced variable rates for MAVLink depending on the baud rate
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 76 |
1 files changed, 54 insertions, 22 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9fc987910..026af14ae 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -92,7 +92,6 @@ static mavlink_status_t status; static pthread_t receive_thread; static pthread_t uorb_receive_thread; -static uint16_t mavlink_message_intervals[256]; /**< intervals at which to send MAVLink packets */ /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -125,6 +124,7 @@ static struct vehicle_command_s vcmd; static int pub_hil_global_pos = -1; static int ardrone_motors_pub = -1; static int cmd_pub = -1; +static int sensor_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; static int flow_pub = -1; @@ -154,7 +154,7 @@ void handleMessage(mavlink_message_t *msg); /** * Enable / disable Hardware in the Loop simulation mode. */ -int set_hil_on_off(uint8_t vehicle_mode); +int set_hil_on_off(bool hil_enabled); /** * Translate the custom state into standard mavlink modes and state. @@ -281,12 +281,12 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa -int set_hil_on_off(uint8_t vehicle_mode) +int set_hil_on_off(bool hil_enabled) { int ret = OK; /* Enable HIL */ - if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) { + if (hil_enabled && !mavlink_hil_enabled) { //printf("\n HIL ON \n"); @@ -308,7 +308,7 @@ int set_hil_on_off(uint8_t vehicle_mode) } } - if (!(vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && mavlink_hil_enabled) { + if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; (void)close(pub_hil_attitude); (void)close(pub_hil_global_pos); @@ -326,7 +326,7 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_mode = 0; /* set mode flags independent of system state */ - if (c_status->control_manual_enabled) { + if (c_status->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } @@ -432,6 +432,28 @@ static void *receiveloop(void *arg) return NULL; } +static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) +{ + int ret = OK; + + switch (mavlink_msg_id) { + case MAVLINK_MSG_ID_SCALED_IMU: + /* senser sub triggers scaled IMU */ + orb_set_interval(sensor_sub, min_interval); + break; + case MAVLINK_MSG_ID_RAW_IMU: + /* senser sub triggers RAW IMU */ + orb_set_interval(sensor_sub, min_interval); + break; + default: + /* not found */ + ret = ERROR; + break; + } + + return ret; +} + /** * Listen for uORB topics and send via MAVLink. * @@ -466,7 +488,7 @@ static void *uorb_receiveloop(void *arg) /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); orb_set_interval(sensor_sub, 100); /* 10Hz updates */ fds[fdsc_count].fd = sensor_sub; fds[fdsc_count].events = POLLIN; @@ -662,11 +684,11 @@ static void *uorb_receiveloop(void *arg) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); /* enable or disable HIL */ - set_hil_on_off(v_status.mode); + 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 = v_status.mode; + uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ @@ -691,7 +713,7 @@ static void *uorb_receiveloop(void *arg) fw_control.attitude_control_output[3]); /* Only send in HIL mode */ - if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) { + 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 @@ -898,10 +920,11 @@ void handleMessage(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); } - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &flow); } if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { @@ -1192,11 +1215,6 @@ int mavlink_main(int argc, char *argv[]) /* reate the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); - /* Send attitude at 10 Hz / every 100 ms */ - mavlink_message_intervals[MAVLINK_MSG_ID_ATTITUDE] = 100; - /* Send raw sensor values at 10 Hz / every 100 ms */ - mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100; - /* default values for arguments */ char *uart_name = "/dev/ttyS0"; int baudrate = 57600; @@ -1283,9 +1301,23 @@ int mavlink_main(int argc, char *argv[]) uint16_t counter = 0; int lowspeed_counter = 0; - /**< Subscribe to system state and RC channels */ - // int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - // int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + /* all subscriptions are now active, set up initial guess about rate limits */ + if (baudrate >= 921600) { + /* 1000 Hz / 1 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1); + } else if (baudrate >= 460800) { + /* 250 Hz / 4 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4); + } else if (baudrate >= 115200) { + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20); + } else if (baudrate >= 57600) { + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); + } else { + /* very low baud rate, limit to 1 Hz / 1000 ms */ + set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000); + } while (1) { @@ -1305,7 +1337,7 @@ int mavlink_main(int argc, char *argv[]) if (lowspeed_counter == 10) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = v_status.mode; + uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); /* send heartbeat */ |