bool VtolType::init() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { PX4_ERR("can't open %s", dev); return false; } int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values); if (ret != PX4_OK) { PX4_ERR("failed getting max values"); px4_close(fd); return false; } ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values); if (ret != PX4_OK) { PX4_ERR("failed getting disarmed values"); px4_close(fd); return false; } return true; }
/** * @brief Resets the driver. */ void reset() { if (!instance) { PX4_WARN("No ll40ls driver running"); return; } int fd = px4_open(instance->get_dev_name(), O_RDONLY); if (fd < 0) { PX4_ERR("Error opening fd"); return; } if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); px4_close(fd); return; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); px4_close(fd); return; } }
void led_deinit() { if (leds >= 0) { px4_close(leds); } if (rgbleds >= 0) { px4_close(rgbleds); } }
int uORB::Manager::node_advertise ( const struct orb_metadata *meta, int *instance, int priority ) { int fd = -1; int ret = ERROR; /* fill advertiser data */ const struct orb_advertdata adv = { meta, instance, priority }; /* open the control device */ fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); if (fd < 0) goto out; /* advertise the object */ ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's PX4_OK if it already exists */ if ((PX4_OK != ret) && (EEXIST == errno)) { ret = PX4_OK; } out: if (fd >= 0) px4_close(fd); return ret; }
/** * Adjust idle speed for fw mode. */ void VtolType::set_idle_fw() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { PX4_WARN("can't open %s", dev); } struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { pwm_values.values[i] = PWM_MOTOR_OFF; pwm_values.channel_count++; } int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); if (ret != OK) { PX4_WARN("failed setting min values"); } px4_close(fd); }
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { int result, fd; orb_advert_t advertiser; //warnx("orb_advertise_multi meta = %p\n", meta); /* open the node as an advertiser */ fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) { warnx("node_open as advertiser failed."); return nullptr; } /* get the advertiser handle and close the node */ result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == ERROR) { warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); return nullptr; } /* the advertiser must perform an initial publish to initialise the object */ result = orb_publish(meta, advertiser, data); if (result == ERROR) { warnx("orb_publish failed"); return nullptr; } return advertiser; }
/** * Adjust idle speed for mc mode. */ void VtolType::set_idle_mc() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { PX4_WARN("can't open %s", dev); } unsigned servo_count; int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); unsigned pwm_value = _params->idle_pwm_mc; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count++; } ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); if (ret != OK) { PX4_WARN("failed setting min values"); } px4_close(fd); flag_idle_mc = true; }
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type) { const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { PX4_WARN("can't open %s", dev); return false; } int ret; if (type == pwm_limit_type::TYPE_MINIMUM) { ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); } else { ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); } px4_close(fd); if (ret != OK) { PX4_ERR("failed setting max values"); return false; } return true; }
int test_gpio(int argc, char *argv[]) { int ret = 0; #ifdef PX4IO_DEVICE_PATH int fd = px4_open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); return ERROR; } /* set all GPIOs to default state */ px4_ioctl(fd, GPIO_RESET, ~0); /* XXX need to add some GPIO waving stuff here */ /* Go back to default */ px4_ioctl(fd, GPIO_RESET, ~0); px4_close(fd); printf("\t GPIO test successful.\n"); #endif return ret; }
static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) { bool success = true; int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); //Wait up to 2000ms to allow the driver to detect a GNSS receiver module px4_pollfd_struct_t fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; if(px4_poll(fds, 1, 2000) <= 0) { success = false; } else { struct vehicle_gps_position_s gps; if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) { success = false; } } //Report failure to detect module if (!success) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } } px4_close(gpsSub); return success; }
/** * Disable all multirotor motors when in fw mode. */ void Standard::set_max_mc(unsigned pwm_value) { int ret; unsigned servo_count; const char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) { PX4_WARN("can't open %s", dev); } ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count = _params->vtol_motor_count; } ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); if (ret != OK) { PX4_WARN("failed setting max values"); } px4_close(fd); }
/** * Start the driver on a specific bus. * * This function only returns if the sensor is up and running * or could not be detected successfully. */ int start_bus(uint8_t rotation, int i2c_bus) { int fd = -1; if (g_dev != nullptr) { PX4_ERR("already started"); return PX4_ERROR; } /* create the driver */ g_dev = new SF1XX(rotation, i2c_bus); if (g_dev == nullptr) { goto fail; } if (OK != g_dev->init()) { goto fail; } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { goto fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); goto fail; } px4_close(fd); return PX4_OK; fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } return PX4_ERROR; }
// 1M 8N1 serial connection to NRF51 int Syslink::open_serial(const char *dev) { #ifndef B1000000 #define B1000000 1000000 #endif int rate = B1000000; // open uart int fd = px4_open(dev, O_RDWR | O_NOCTTY); int termios_state = -1; if (fd < 0) { PX4_ERR("failed to open uart device!"); return -1; } // set baud rate struct termios config; tcgetattr(fd, &config); // clear ONLCR flag (which appends a CR for every LF) config.c_oflag = 0; config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); // Disable hardware flow control config.c_cflag &= ~CRTSCTS; /* Set baud rate */ if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) { warnx("ERR SET BAUD %s: %d\n", dev, termios_state); px4_close(fd); return -1; } if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) { PX4_WARN("ERR SET CONF %s\n", dev); px4_close(fd); return -1; } return fd; }
int test_tone(int argc, char *argv[]) { int fd, result; unsigned long tone; fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (fd < 0) { printf("failed opening " TONEALARM0_DEVICE_PATH "\n"); goto out; } tone = 1; if (argc == 2) { tone = atoi(argv[1]); } if (tone == 0) { result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); goto out; } else { printf("Alarm stopped.\n"); } } else { result = px4_ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); goto out; } result = px4_ioctl(fd, TONE_SET_ALARM, tone); if (result < 0) { printf("failed setting alarm %lu\n", tone); } else { printf("Alarm %lu (disable with: tests tone 0)\n", tone); } } out: if (fd >= 0) { px4_close(fd); } return 0; }
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { /* * Generate the path to the node and try to open it. */ char path[orb_maxpath]; int inst = instance; int ret = uORB::Utils::node_mkpath(path, meta, &inst); if (ret != OK) { errno = -ret; return PX4_ERROR; } #if defined(__PX4_NUTTX) struct stat buffer; ret = stat(path, &buffer); #else ret = px4_access(path, F_OK); #ifdef ORB_COMMUNICATOR if (ret == -1 && meta != nullptr && !_remote_topics.empty()) { ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR; } #endif /* ORB_COMMUNICATOR */ #endif if (ret == 0) { // we know the topic exists, but it's not necessarily advertised/published yet (for example // if there is only a subscriber) // The open() will not lead to memory allocations. int fd = px4_open(path, 0); if (fd >= 0) { unsigned long is_published; if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) { if (!is_published) { ret = PX4_ERROR; } } px4_close(fd); } } return ret; }
void Tiltrotor::set_rear_motor_state(rear_motor_state state) { int pwm_value = PWM_DEFAULT_MAX; // map desired rear rotor state to max allowed pwm signal switch (state) { case ENABLED: pwm_value = PWM_DEFAULT_MAX; _rear_motors = ENABLED; break; case DISABLED: pwm_value = PWM_LOWEST_MAX; _rear_motors = DISABLED; break; case IDLE: pwm_value = _params->idle_pwm_mc; _rear_motors = IDLE; break; } int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; int fd = px4_open(dev, 0); if (fd < 0) {PX4_WARN("can't open %s", dev);} ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; } else { pwm_values.values[i] = PWM_DEFAULT_MAX; } pwm_values.channel_count = _params->vtol_motor_count; } ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); if (ret != OK) {PX4_WARN("failed setting max values");} px4_close(fd); }
static int accel(int argc, char *argv[], const char *path) { printf("\tACCEL: test start\n"); fflush(stdout); int fd; struct accel_report buf; int ret; fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tACCEL: open fail, run <mpu6000 start> or <lsm303 start> or <bma180 start> first.\n"); return ERROR; } /* wait at least 100ms, sensor should have data after no more than 20ms */ usleep(100000); /* read data - expect samples */ ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tACCEL: read1 fail (%d)\n", ret); return ERROR; } else { printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { warnx("ACCEL acceleration values out of range!"); return ERROR; } float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); if (len < 8.0f || len > 12.0f) { warnx("ACCEL scale error!"); return ERROR; } /* Let user know everything is ok */ printf("\tOK: ACCEL passed all tests successfully\n"); px4_close(fd); return OK; }
static int gyro(int argc, char *argv[], const char *path) { printf("\tGYRO: test start\n"); fflush(stdout); int fd; struct gyro_report buf; int ret; fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tGYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n"); return ERROR; } /* wait at least 5 ms, sensor should have data after that */ usleep(5000); /* read data - expect samples */ ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tGYRO: read fail (%d)\n", ret); return ERROR; } else { printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); if (len > 0.3f) { warnx("GYRO scale error!"); return ERROR; } /* Let user know everything is ok */ printf("\tOK: GYRO passed all tests successfully\n"); px4_close(fd); return OK; }
static int mag(int argc, char *argv[], const char *path) { printf("\tMAG: test start\n"); fflush(stdout); int fd; struct mag_report buf; int ret; fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tMAG: open fail, run <hmc5883 start> or <lsm303 start> first.\n"); return ERROR; } /* wait at least 5 ms, sensor should have data after that */ usleep(5000); /* read data - expect samples */ ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tMAG: read fail (%d)\n", ret); return ERROR; } else { printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); } float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } /* Let user know everything is ok */ printf("\tOK: MAG passed all tests successfully\n"); px4_close(fd); return OK; }
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) { bool success = true; char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); } return false; } int calibration_devid; int ret; int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_MAG%u_ID", instance); param_get(param_find(s), &(calibration_devid)); if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; } ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); success = false; goto out; } out: px4_close(fd); return success; }
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) { bool success = true; char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); } return false; } px4_close(fd); return success; }
static int do_import(const char *param_file_name) { int fd = px4_open(param_file_name, O_RDONLY); if (fd < 0) { warn("open '%s'", param_file_name); return 1; } int result = param_import(fd); px4_close(fd); if (result < 0) { warnx("error importing from '%s'", param_file_name); return 1; } return 0; }
static int do_save(const char *param_file_name) { /* create the file */ int fd = px4_open(param_file_name, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("opening '%s' failed", param_file_name); return 1; } int result = param_export(fd, false); px4_close(fd); if (result < 0) { (void)unlink(param_file_name); warnx("error exporting to '%s'", param_file_name); return 1; } return 0; }
int initialise_uart() { // open uart _uart_fd = px4_open(_device, O_RDWR | O_NOCTTY); int termios_state = -1; if (_uart_fd < 0) { PX4_ERR("failed to open uart device!"); return -1; } // set baud rate int speed = B921600; struct termios uart_config; tcgetattr(_uart_fd, &uart_config); // clear ONLCR flag (which appends a CR for every LF) uart_config.c_oflag &= ~ONLCR; /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { warnx("ERR SET BAUD %s: %d\n", _device, termios_state); ::close(_uart_fd); return -1; } if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { PX4_WARN("ERR SET CONF %s\n", _device); px4_close(_uart_fd); return -1; } /* setup output flow control */ if (enable_flow_control(false)) { PX4_WARN("hardware flow disable failed"); } return _uart_fd; }
static int baro(int argc, char *argv[], const char *path) { printf("\tBARO: test start\n"); fflush(stdout); int fd; struct baro_report buf; int ret; fd = px4_open(path, O_RDONLY); if (fd < 0) { printf("\tBARO: open fail, run <ms5611 start> or <lps331 start> first.\n"); return ERROR; } /* wait at least 5 ms, sensor should have data after that */ usleep(5000); /* read data - expect samples */ ret = px4_read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { printf("\tBARO: read fail (%d)\n", ret); return ERROR; } else { printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); } /* Let user know everything is ok */ printf("\tOK: BARO passed all tests successfully\n"); px4_close(fd); return OK; }
int test_adc(int argc, char *argv[]) { int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { PX4_ERR("ERROR: can't open ADC device"); return 1; } for (unsigned i = 0; i < 5; i++) { /* make space for a maximum of twelve channels */ struct adc_msg_s data[12]; /* read all channels available */ ssize_t count = px4_read(fd, data, sizeof(data)); if (count < 0) { goto errout_with_dev; } unsigned channels = count / sizeof(data[0]); for (unsigned j = 0; j < channels; j++) { printf("%d: %u ", data[j].am_channel, data[j].am_data); } printf("\n"); usleep(150000); } printf("\t ADC test successful.\n"); errout_with_dev: if (fd != 0) { px4_close(fd); } return OK; }
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail) { bool success = true; int ret; int fd = orb_subscribe(ORB_ID(airspeed)); struct airspeed_s airspeed; if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); } success = false; goto out; } if (fabsf(airspeed.confidence) < 0.99f) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR"); } success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); } // XXX do not make this fatal yet } out: px4_close(fd); return success; }
/** * Reset the driver. */ int reset() { int fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { PX4_ERR("failed"); return PX4_ERROR; } if (ioctl(fd, SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); return PX4_ERROR; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); return PX4_ERROR; } px4_close(fd); return PX4_OK; }
int do_mag_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); struct mag_scale mscale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, }; int result = OK; // Determine which mags are available and reset each int32_t device_ids[max_mags]; char str[30]; for (size_t i=0; i<max_mags; i++) { device_ids[i] = 0; // signals no mag } for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); break; } // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = px4_open(str, O_RDONLY); if (fd < 0) { continue; } // Get device id for this mag device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); // Reset mag scale result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag); } /* calibrate range */ if (result == OK) { result = px4_ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag); /* this is non-fatal - mark it accordingly */ result = OK; } } px4_close(fd); } // Calibrate all mags at the same time if (result == OK) { switch (mag_calibrate_all(mavlink_fd, device_ids)) { case calibrate_return_cancelled: // Cancel message already displayed, we're done here result = ERROR; break; case calibrate_return_ok: /* auto-save to EEPROM */ result = param_save_default(); /* if there is a any preflight-check system response, let the barrage of messages through */ usleep(200000); if (result == OK) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); break; } else { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } // Fall through default: mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); break; } } /* give this message enough time to propagate */ usleep(600000); return result; }
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) { calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; worker_data.calibration_points_perside = 40; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { // Initialize to no subscription worker_data.sub_mag[cur_mag] = -1; // Initialize to no memory allocated worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; worker_data.calibration_counter_total[cur_mag] = 0; } const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory"); result = calibrate_return_error; } } // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); if (worker_data.sub_mag[cur_mag] < 0) { mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag); result = calibrate_return_error; break; } } } } // Limit update rate to get equally spaced measurements over time (in ms) if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; //mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs); orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); } } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output cancel_sub, // Subscription to vehicle_command for cancel support worker_data.side_data_collected, // Sides to calibrate mag_calibration_worker, // Calibration worker &worker_data, // Opaque data for calibration worked true); // true: lenient still detection calibrate_cancel_unsubscribe(cancel_sub); } // Close subscriptions for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (worker_data.sub_mag[cur_mag] >= 0) { px4_close(worker_data.sub_mag[cur_mag]); } } // Calculate calibration values for each mag float sphere_x[max_mags]; float sphere_y[max_mags]; float sphere_z[max_mags]; float sphere_radius[max_mags]; // Sphere fit the data to get calibration values if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag); result = calibrate_return_error; } } } } // Print uncalibrated data points if (result == calibrate_return_ok) { printf("RAW DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i]; float y = worker_data.y[cur_mag][i]; float z = worker_data.z[cur_mag][i]; printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); } printf(">>>>>>>\n"); } printf("CALIBRATED DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); } printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]); printf(">>>>>>>\n"); } } // Data points are no longer needed for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { free(worker_data.x[cur_mag]); free(worker_data.y[cur_mag]); free(worker_data.z[cur_mag]); } if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { int fd_mag = -1; struct mag_scale mscale; // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); fd_mag = px4_open(str, 0); if (fd_mag < 0) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } } if (result == calibrate_return_ok) { mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } } // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } if (result == calibrate_return_ok) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag); result = calibrate_return_error; } else { mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); } } } } } return result; }