diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:48:09 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:48:09 +0100 |
commit | b52402dbe2d3447b25a46070bdd771c12fd4c55a (patch) | |
tree | 825b9a3349169ce933906685f05ada037fd29570 /apps/mavlink/mavlink.c | |
parent | fdf1c712b163deace14dfd8c74a6b1afac6262fb (diff) | |
download | px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.tar.gz px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.tar.bz2 px4-firmware-b52402dbe2d3447b25a46070bdd771c12fd4c55a.zip |
Fixed code style for mavlink app
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 117 |
1 files changed, 73 insertions, 44 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ccc9d6d7d..2ec459ddc 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -156,12 +156,15 @@ set_hil_on_off(bool hil_enabled) 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 { /* 200 Hz */ hil_rate_interval = 5; @@ -202,13 +205,13 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* attitude or rate control */ if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { + v_status.flag_control_rates_enabled) { *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; } /* vector control */ if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { + v_status.flag_control_position_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -220,6 +223,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* set arming state */ if (armed.armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } @@ -230,9 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) v_status.flag_preflight_mag_calibration || v_status.flag_preflight_accel_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; + } else { *mavlink_state = MAV_STATE_UNINIT; } + break; case SYSTEM_STATE_STANDBY: @@ -284,41 +290,48 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m int ret = OK; switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_RAW_IMU: - /* sensor sub triggers RAW IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - case MAVLINK_MSG_ID_ATTITUDE: - /* attitude sub triggers attitude */ - orb_set_interval(subs->att_sub, min_interval); - break; - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: - /* actuator_outputs triggers this message */ - orb_set_interval(subs->act_0_sub, min_interval); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - orb_set_interval(subs->act_3_sub, min_interval); - orb_set_interval(subs->actuators_sub, min_interval); - break; - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); - break; - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); - break; - default: - /* not found */ - ret = ERROR; - break; + case MAVLINK_MSG_ID_SCALED_IMU: + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_HIGHRES_IMU: + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_RAW_IMU: + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(subs->att_sub, min_interval); + break; + + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + orb_set_interval(subs->debug_key_value, min_interval); + break; + + default: + /* not found */ + ret = ERROR; + break; } return ret; @@ -460,7 +473,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } -void +void mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) { write(uart, ch, (size_t)(sizeof(uint8_t) * length)); @@ -469,7 +482,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t* mavlink_get_channel_status(uint8_t channel) +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_status[channel]; @@ -478,7 +491,7 @@ mavlink_status_t* mavlink_get_channel_status(uint8_t channel) /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_buffer[channel]; @@ -501,18 +514,21 @@ void mavlink_update_system(void) /* update system and component id */ int32_t system_id; param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } - + int32_t system_type; param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } @@ -538,8 +554,10 @@ int mavlink_thread_main(int argc, char *argv[]) switch (ch) { case 'b': baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) errx(1, "invalid baud rate '%s'", optarg); + break; case 'd': @@ -560,6 +578,7 @@ int mavlink_thread_main(int argc, char *argv[]) } struct termios uart_config_original; + bool usb_uart; /* print welcome text */ @@ -573,6 +592,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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); @@ -603,6 +623,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 2 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); @@ -613,6 +634,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 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); @@ -625,6 +647,7 @@ int mavlink_thread_main(int argc, char *argv[]) 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); + } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); @@ -677,6 +700,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.errors_count4); lowspeed_counter = 0; } + lowspeed_counter++; /* sleep quarter the time */ @@ -693,6 +717,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* sleep quarter the time */ usleep(25000); + if (baudrate > 57600) { mavlink_pm_queued_send(); } @@ -704,6 +729,7 @@ int mavlink_thread_main(int argc, char *argv[]) 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); } @@ -730,8 +756,8 @@ static void usage() { fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" - " mavlink stop\n" - " mavlink status\n"); + " mavlink stop\n" + " mavlink status\n"); exit(1); } @@ -755,16 +781,18 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, mavlink_thread_main, - (const char**)argv); + (const char **)argv); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; + while (thread_running) { usleep(200000); printf("."); } + warnx("terminated."); exit(0); } @@ -772,6 +800,7 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { errx(0, "running"); + } else { errx(1, "not running"); } |