diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 409c2ea003..4248ad282b 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -127,6 +127,7 @@ #include "calibration_routines.h" #include "commander_helper.h" +#include #include #include #include @@ -190,16 +191,16 @@ int do_accel_calibration(int mavlink_fd) for (unsigned s = 0; s < max_accel_sens; s++) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ - fd = open(str, 0); + fd = px4_open(str, 0); if (fd < 0) { continue; } - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - close(fd); + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + px4_close(fd); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); @@ -268,14 +269,14 @@ int do_accel_calibration(int mavlink_fd) } sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); - fd = open(str, 0); + fd = px4_open(str, 0); if (fd < 0) { mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist"); res = ERROR; } else { - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - close(fd); + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + px4_close(fd); } if (res != OK) { @@ -366,7 +367,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } - close(worker_data.subs[i]); + px4_close(worker_data.subs[i]); } } @@ -390,7 +391,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac */ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - struct pollfd fds[max_accel_sens]; + px4_pollfd_struct_t fds[max_accel_sens]; for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; @@ -405,7 +406,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { - int poll_ret = poll(&fds[0], max_accel_sens, 1000); + int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000); if (poll_ret > 0) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 773c62b3e1..dd78676af6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -40,6 +40,7 @@ #include "calibration_messages.h" #include "commander_helper.h" +#include #include #include #include @@ -90,17 +91,17 @@ int do_airspeed_calibration(int mavlink_fd) }; bool paramreset_successful = false; - int fd = open(AIRSPEED0_DEVICE_PATH, 0); + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); if (fd > 0) { - if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } - close(fd); + px4_close(fd); } if (!paramreset_successful) { @@ -110,14 +111,14 @@ int do_airspeed_calibration(int mavlink_fd) param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } @@ -130,11 +131,11 @@ int do_airspeed_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); @@ -149,7 +150,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } @@ -158,19 +159,19 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { - int fd_scale = open(AIRSPEED0_DEVICE_PATH, 0); + int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { - if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); } - close(fd_scale); + px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -180,13 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } else { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -204,11 +205,11 @@ int do_airspeed_calibration(int mavlink_fd) while (calibration_counter < maxcount) { /* wait blocking for new data */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); @@ -228,14 +229,14 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); - close(diff_pres_sub); + px4_close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -243,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); - close(diff_pres_sub); + px4_close(diff_pres_sub); feedback_calibration_failed(mavlink_fd); return ERROR; @@ -256,14 +257,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -271,6 +272,6 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); - close(diff_pres_sub); + px4_close(diff_pres_sub); return OK; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 31baedb2fc..97873e462c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -264,6 +264,7 @@ int commander_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -271,7 +272,7 @@ int commander_main(int argc, char *argv[]) if (thread_running) { warnx("commander already running"); /* this is not an error */ - exit(0); + return 0; } thread_should_exit = false; @@ -286,13 +287,14 @@ int commander_main(int argc, char *argv[]) usleep(200); } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (!thread_running) { - errx(0, "commander already stopped"); + warnx("commander already stopped"); + return 0; } thread_should_exit = true; @@ -304,18 +306,18 @@ int commander_main(int argc, char *argv[]) warnx("terminated."); - exit(0); + return 0; } /* commands needing the app to run below */ if (!thread_running) { warnx("\tcommander not started"); - exit(1); + return 1; } if (!strcmp(argv[1], "status")) { print_status(); - exit(0); + return 0; } if (!strcmp(argv[1], "calibrate")) { @@ -332,9 +334,10 @@ int commander_main(int argc, char *argv[]) } if (calib_ret) { - errx(1, "calibration failed, exiting."); + warnx("calibration failed, exiting."); + return 0; } else { - exit(0); + return 0; } } else { warnx("missing argument"); @@ -346,25 +349,25 @@ int commander_main(int argc, char *argv[]) int checkres = prearm_check(&status, mavlink_fd_local); close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); - exit(0); + return 0; } if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); close(mavlink_fd_local); - exit(0); + return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); close(mavlink_fd_local); - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } void usage(const char *reason) @@ -374,7 +377,6 @@ void usage(const char *reason) } fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); - exit(1); } void print_status() @@ -935,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); - exit(ERROR); + px4_task_exit(ERROR); } /* armed topic */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c06523ce44..e2680bf9ad 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include #include @@ -124,10 +125,10 @@ int buzzer_init() tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { - warnx("Buzzer: open fail\n"); + warnx("Buzzer: px4_open fail\n"); return ERROR; } @@ -136,12 +137,12 @@ int buzzer_init() void buzzer_deinit() { - close(buzzer); + px4_close(buzzer); } void set_tune_override(int tune) { - ioctl(buzzer, TONE_SET_ALARM, tune); + px4_ioctl(buzzer, TONE_SET_ALARM, tune); } void set_tune(int tune) @@ -152,7 +153,7 @@ void set_tune(int tune) 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); + px4_ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; @@ -230,22 +231,22 @@ int led_init() blink_msg_end = 0; /* first open normal LEDs */ - leds = open(LED0_DEVICE_PATH, 0); + leds = px4_open(LED0_DEVICE_PATH, 0); if (leds < 0) { - warnx("LED: open fail\n"); + warnx("LED: px4_open fail\n"); return ERROR; } /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ - (void)ioctl(leds, LED_ON, LED_BLUE); + (void)px4_ioctl(leds, LED_ON, LED_BLUE); /* switch blue off */ led_off(LED_BLUE); /* we consider the amber led mandatory */ - if (ioctl(leds, LED_ON, LED_AMBER)) { - warnx("Amber LED: ioctl fail\n"); + if (px4_ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: px4_ioctl fail\n"); return ERROR; } @@ -253,7 +254,7 @@ int led_init() led_off(LED_AMBER); /* then try RGB LEDs, this can fail on FMUv1*/ - rgbleds = open(RGBLED0_DEVICE_PATH, 0); + rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0); if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); @@ -265,11 +266,11 @@ int led_init() void led_deinit() { if (leds >= 0) { - close(leds); + px4_close(leds); } if (rgbleds >= 0) { - close(rgbleds); + px4_close(rgbleds); } } @@ -278,7 +279,7 @@ int led_toggle(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_TOGGLE, led); + return px4_ioctl(leds, LED_TOGGLE, led); } int led_on(int led) @@ -286,7 +287,7 @@ int led_on(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_ON, led); + return px4_ioctl(leds, LED_ON, led); } int led_off(int led) @@ -294,7 +295,7 @@ int led_off(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_OFF, led); + return px4_ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) @@ -303,7 +304,7 @@ void rgbled_set_color(rgbled_color_t color) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) @@ -312,7 +313,7 @@ void rgbled_set_mode(rgbled_mode_t mode) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) @@ -321,7 +322,7 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a2b0900ef6..b5d5bb23c7 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -41,6 +41,7 @@ #include "calibration_messages.h" #include "commander_helper.h" +#include #include #include #include @@ -96,16 +97,16 @@ int do_gyro_calibration(int mavlink_fd) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { continue; } - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + px4_close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); @@ -123,7 +124,7 @@ int do_gyro_calibration(int mavlink_fd) /* subscribe to gyro sensor topic */ int sub_sensor_gyro[max_gyros]; - struct pollfd fds[max_gyros]; + px4_pollfd_struct_t fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); @@ -137,7 +138,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(&fds[0], max_gyros, 1000); + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -175,7 +176,7 @@ int do_gyro_calibration(int mavlink_fd) } for (unsigned s = 0; s < max_gyros; s++) { - close(sub_sensor_gyro[s]); + px4_close(sub_sensor_gyro[s]); gyro_scale[s].x_offset /= calibration_counter[s]; gyro_scale[s].y_offset /= calibration_counter[s]; @@ -225,15 +226,15 @@ int do_gyro_calibration(int mavlink_fd) /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { failed = true; continue; } - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - close(fd); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); + px4_close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 198acb0272..49ccbf3ef1 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -42,7 +42,9 @@ #include "calibration_routines.h" #include "calibration_messages.h" +#include #include +#include #include #include #include @@ -119,16 +121,16 @@ int do_mag_calibration(int mavlink_fd) // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - int fd = open(str, O_RDONLY); + int fd = px4_open(str, O_RDONLY); if (fd < 0) { continue; } // Get device id for this mag - device_ids[cur_mag] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); // Reset mag scale - result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag); @@ -136,7 +138,7 @@ int do_mag_calibration(int mavlink_fd) if (result == OK) { /* calibrate range */ - result = ioctl(fd, MAGIOCCALIBRATE, fd); + result = px4_ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag); @@ -145,7 +147,7 @@ int do_mag_calibration(int mavlink_fd) } } - close(fd); + px4_close(fd); } if (result == OK) { @@ -192,7 +194,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) calibration_counter_side < worker_data->calibration_points_perside) { // Wait clocking for new data on all mags - struct pollfd fds[max_mags]; + px4_pollfd_struct_t fds[max_mags]; size_t fd_count = 0; for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { @@ -201,7 +203,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) fd_count++; } } - int poll_ret = poll(fds, fd_count, 1000); + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { for (size_t cur_mag=0; cur_mag= 0) { - close(worker_data.sub_mag[cur_mag]); + px4_close(worker_data.sub_mag[cur_mag]); } } @@ -398,14 +400,14 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - fd_mag = open(str, 0); + fd_mag = px4_open(str, 0); if (fd_mag < 0) { mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag); result = ERROR; } if (result == OK) { - result = ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale); + result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag); result = ERROR; @@ -417,7 +419,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; - result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale); + result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag); result = ERROR; @@ -426,7 +428,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) // Mag device no longer needed if (fd_mag >= 0) { - close(fd_mag); + px4_close(fd_mag); } if (result == OK) { diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index c9de1ffa2f..4ef361a826 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -40,6 +40,8 @@ SRCS = commander.cpp \ commander_params.c \ commander_helper.cpp \ calibration_routines.cpp \ + mag_calibration.cpp \ + gyro_calibration.cpp \ baro_calibration.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ @@ -48,13 +50,9 @@ SRCS = commander.cpp \ ifdef ($(PX4_TARGET_OS),nuttx) SRCS += - state_machine_helper.cpp \ - gyro_calibration.cpp \ - mag_calibration.cpp + state_machine_helper.cpp else -SRCS += state_machine_helper_linux.cpp \ - gyro_calibration_linux.cpp \ - mag_calibration_linux.cpp +SRCS += state_machine_helper_linux.cpp endif MODULE_STACKSIZE = 5000