Commit 349809f5 authored by Anton Babushkin's avatar Anton Babushkin
Browse files

sensors, commander: code style fixed

parent 88b18bba
......@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3,3> board_rotation;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
math::Vector<3> accel_offs_vec(&accel_offs[0]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
......@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
}
}
if (old_done_count != done_count)
if (old_done_count != done_count) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
}
if (done)
if (done) {
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
......@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f)
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i])
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
......@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 0; // [ g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 0; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 1; // [ -g, 0, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 1; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 2; // [ 0, g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 2; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr)
return 3; // [ 0, -g, 0 ]
fabsf(accel_ema[2]) < accel_err_thr) {
return 3; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
return 4; // [ 0, 0, g ]
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return 4; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
return 5; // [ 0, 0, -g ]
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return 5; // [ 0, 0, -g ]
}
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
......@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
for (int i = 0; i < 3; i++) {
accel_sum[i] += sensor.accelerometer_m_s2[i];
}
count++;
......@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
continue;
}
if (errcount > samples_num / 10)
if (errcount > samples_num / 10) {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
......@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f)
return ERROR; // Singular matrix
if (det == 0.0f) {
return ERROR; // Singular matrix
}
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
......@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
}
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {
......
......@@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd)
bool paramreset_successful = false;
int fd = open(AIRSPEED_DEVICE_PATH, 0);
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
......@@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
......
......@@ -221,7 +221,7 @@ void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
......@@ -233,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
int commander_main(int argc, char *argv[])
{
if (argc < 1)
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
......@@ -261,8 +262,9 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "stop")) {
if (!thread_running)
if (!thread_running) {
errx(0, "commander already stopped");
}
thread_should_exit = true;
......@@ -304,8 +306,9 @@ int commander_main(int argc, char *argv[])
void usage(const char *reason)
{
if (reason)
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
......@@ -364,20 +367,22 @@ void print_status()
static orb_advert_t status_pub;
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
}
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
......@@ -417,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (hil_ret == OK)
if (hil_ret == OK) {
ret = true;
}
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED)
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
......@@ -466,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
if (main_res == TRANSITION_CHANGED)
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
......@@ -480,24 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
// We use an float epsilon delta to test float equality.
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
} else {
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
} else {
// Flick to inair restore first if this comes from an onboard system
if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
}
transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
break;
......@@ -525,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
//XXX: to enable the parachute, a param needs to be set
......@@ -545,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
if (use_current) {
/* use current position */
if (status->condition_global_position_valid) {
......@@ -588,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
......@@ -601,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
......@@ -883,6 +898,7 @@ int commander_thread_main(int argc, char *argv[])
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
......@@ -923,6 +939,7 @@ int commander_thread_main(int argc, char *argv[])
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
}
......@@ -940,9 +957,11 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
} else {
eph_epv_good = true;
}
......@@ -950,17 +969,19 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
} else {
eph_epv_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
......@@ -995,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
......@@ -1008,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
......@@ -1077,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[])
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
if (last_idle_time > 0)
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
if (last_idle_time > 0) {
status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle
}
last_idle_time = system_load.tasks[0].total_runtime;
......@@ -1255,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
......@@ -1320,6 +1344,7 @@ int commander_thread_main(int argc, char *argv[])
if (res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
} else if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
}
......@@ -1350,8 +1375,9 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
status_changed = true;
}
}
/* check which state machines for changes, clear "changed" flag */
......@@ -1368,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;
......@@ -1390,6 +1416,7 @@ int commander_thread_main(int argc, char *argv[])
status.condition_home_position_valid = true;
}
}
was_armed = armed.armed;
if (main_state_changed) {
......@@ -1553,21 +1580,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
if (leds_counter % 20 == 0) {
led_toggle(LED_BLUE);
}
} else {
/* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_BLUE);
}
}
#endif
/* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) {
if (leds_counter % 2 == 0)
if (leds_counter % 2 == 0) {
led_toggle(LED_AMBER);
}
} else {
led_off(LED_AMBER);
......@@ -1781,7 +1811,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
......@@ -1831,8 +1861,9 @@ void *commander_low_prio_loop(void *arg)
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for thread_should_exit, etc. */
if (pret == 0)
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
......@@ -1847,8 +1878,9 @@ void *commander_low_prio_loop(void *arg)
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO)
cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
continue;
}
/* only handle low-priority commands here */
switch (cmd.command) {
......@@ -1926,6 +1958,7 @@ void *commander_low_prio_loop(void *arg)
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
......@@ -1940,10 +1973,12 @@ void *commander_low_prio_loop(void *arg)
}
if (calib_ret == OK)
if (calib_ret == OK) {
tune_positive(true);
else
} else {
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
......@@ -1963,11 +1998,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
......@@ -1983,11 +2020,13 @@ void *commander_low_prio_loop(void *arg)
mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
if (ret < 0) {
ret = -ret;
}
if (ret < 1000)
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
}
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
......@@ -1997,8 +2036,8 @@ void *commander_low_prio_loop(void *arg)
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
/* handled in the IO driver */
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
......
......@@ -113,17 +113,22 @@ void buzzer_deinit()
close(buzzer);
}
void set_tune(int tune) {
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
ioctl(buzzer, TONE_SET_ALARM, tune);