forked from Archive/PX4-Autopilot
Converted commander to use px4_posix calls
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
parent
bba26c3430
commit
694427e4ba
|
@ -127,6 +127,7 @@
|
|||
#include "calibration_routines.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
|
@ -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) {
|
||||
|
||||
|
|
|
@ -40,6 +40,7 @@
|
|||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -42,6 +42,7 @@
|
|||
*/
|
||||
|
||||
#include <px4_defines.h>
|
||||
#include <px4_posix.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
|
@ -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)
|
||||
|
|
|
@ -41,6 +41,7 @@
|
|||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
@ -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);
|
||||
|
|
|
@ -42,7 +42,9 @@
|
|||
#include "calibration_routines.h"
|
||||
#include "calibration_messages.h"
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
|
@ -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_mag<max_mags; cur_mag++) {
|
||||
if (worker_data->sub_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<max_mags; cur_mag++) {
|
||||
|
@ -350,7 +352,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
|
|||
// Close subscriptions
|
||||
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||
if (worker_data.sub_mag[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) {
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue