static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); DevHandle h; DevMgr::getHandle(s, h); if (!h.isValid()) { if (!optional) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); } } return false; } device_id = -1000; // TODO: There is no baro calibration yet, since no external baros exist // int ret = check_calibration(fd, "CAL_BARO%u_ID"); // if (ret) { // mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); // success = false; // goto out; // } //out: DevMgr::releaseHandle(h); return success; }
static bool gnssCheck(int mavlink_fd) { 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) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } px4_close(gpsSub); return success; }
void BlockLocalPositionEstimator::flowCheckTimeout() { if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) { if (_flowInitialized) { flowDeinit(); mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout "); } } }
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 = 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 = 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 = 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: close(fd); return success; }
void BlockLocalPositionEstimator::visionCheckTimeout() { if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) { if (_visionInitialized) { _visionInitialized = false; _visionStats.reset(); mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] vision position timeout "); } } }
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; char s[30]; sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); DevHandle h; DevMgr::getHandle(s, h); if (!h.isValid()) { if (!optional) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); } } return false; } int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id); if (ret) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); } success = false; goto out; } ret = h.ioctl(GYROIOCSELFTEST, 0); if (ret != OK) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); } success = false; goto out; } out: DevMgr::releaseHandle(h); return success; }
void Mission::reset_offboard_mission(struct mission_s &mission) { dm_lock(DM_KEY_MISSION_STATE); if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { /* set current item to 0 */ mission.current_seq = 0; /* reset jump counters */ if (mission.count > 0) { dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); for (unsigned index = 0; index < mission.count; index++) { struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, index, &item, len) != len) { PX4_WARN("could not read mission item during reset"); break; } if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET, &item, len) != len) { PX4_WARN("could not save mission item during reset"); break; } } } } } else { mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: could not read mission"); /* initialize mission state in dataman */ mission.dataman_id = 0; mission.count = 0; mission.current_seq = 0; } dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } dm_unlock(DM_KEY_MISSION_STATE); }
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; }
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 = open(s, 0); if (fd < 0) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); } return false; } close(fd); return success; }
int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ bool reportFailures = force_report || (!status->data_link_lost && !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; } bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } return !preflight_ok; }
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed) { int return_code = OK; int fd = -1; struct battery_status_s battery; int batt_sub = -1; bool batt_updated = false; bool batt_connected = false; hrt_abstime battery_connect_wait_timeout = 30000000; hrt_abstime pwm_high_timeout = 10000000; hrt_abstime timeout_start; mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); batt_sub = orb_subscribe(ORB_ID(battery_status)); if (batt_sub < 0) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery"); goto Error; } // Make sure battery is disconnected orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); goto Error; } armed->in_esc_calibration_mode = true; fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); if (fd < 0) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device"); goto Error; } /* tell IO/FMU that its ok to disable its safety with the switch */ if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch"); goto Error; } /* tell IO/FMU that the system is armed (it will output values if safety is off) */ if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system"); goto Error; } /* tell IO to switch off safety without using the safety switch */ if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off"); goto Error; } mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now"); timeout_start = hrt_absolute_time(); while (true) { // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM // sit high. hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; if (hrt_absolute_time() - timeout_start > timeout_wait) { if (!batt_connected) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); goto Error; } // PWM was high long enough break; } if (!batt_connected) { orb_check(batt_sub, &batt_updated); if (batt_updated) { orb_copy(ORB_ID(battery_status), batt_sub, &battery); if (battery.voltage_filtered_v > 3.0f) { // Battery is connected, signal to user and start waiting again batt_connected = true; timeout_start = hrt_absolute_time(); mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected"); } } } usleep(50000); } Out: if (batt_sub != -1) { orb_unsubscribe(batt_sub); } if (fd != -1) { if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off"); } if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed"); } if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated"); } px4_close(fd); } armed->in_esc_calibration_mode = false; if (return_code == OK) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); } return return_code; Error: return_code = ERROR; goto Out; }
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) { const unsigned samples_num = 3000; *active_sensors = 0; float accel_ref[max_sens][6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int subs[max_sens]; uint64_t timestamps[max_sens]; for (unsigned i = 0; i < max_sens; i++) { subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); /* store initial timestamp - used to infer which sensors are active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); timestamps[i] = arp.timestamp; } unsigned done_count = 0; int res = OK; while (true) { bool done = true; unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { if (data_collected[i]) { done_count++; } else { done = false; } } if (old_done_count != done_count) { mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); } if (done) { break; } /* inform user which axes are still needed */ mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); int orient = detect_orientation(mavlink_fd, subs); if (orient < 0) { mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still..."); sleep(3); continue; } /* inform user about already handled side */ if (data_collected[orient]) { mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); sleep(3); continue; } mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); sleep(1); read_accelerometer_avg(subs, accel_ref, orient, samples_num); mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); data_collected[orient] = true; tune_neutral(true); } /* close all subscriptions */ for (unsigned i = 0; i < max_sens; i++) { /* figure out which sensors were active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } close(subs[i]); } if (res == OK) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); /* verbose output on the console */ printf("accel_T transformation matrix:\n"); for (unsigned j = 0; j < 3; j++) { printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); } printf("\n"); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } } return res; }
/** * Wait for vehicle become still and detect it's orientation. * * @param mavlink_fd the MAVLink file descriptor to print output to * @param subs the accelerometer subscriptions. Only the first one will be used. * * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) { const unsigned ndim = 3; struct accel_report sensor; /* exponential moving average of accel */ float accel_ema[ndim] = { 0.0f }; /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = powf(0.25f, 2); /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ hrt_abstime still_time = 2000000; struct pollfd fds[1]; fds[0].fd = subs[0]; fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ hrt_abstime timeout = 30000000; hrt_abstime t_timeout = t_start + timeout; hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; unsigned poll_errcount = 0; while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(sensor_accel), subs[0], &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; for (unsigned i = 0; i < ndim; i++) { float di = 0.0f; switch (i) { case 0: di = sensor.x; break; case 1: di = sensor.y; break; case 2: di = sensor.z; break; } float d = di - accel_ema[i]; accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; } if (d > accel_disp[i]) { accel_disp[i] = d; } } /* still detector with hysteresis */ if (accel_disp[0] < still_thr2 && accel_disp[1] < still_thr2 && accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; } else { /* still since t_still */ if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } } } else if (accel_disp[0] > still_thr2 * 4.0f || accel_disp[1] > still_thr2 * 4.0f || accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); sleep(3); t_still = 0; } } } else if (poll_ret == 0) { poll_errcount++; } if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } } 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 ] } 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 ] } 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 ] } 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 ] } 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 ] } 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 ] } mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); return ERROR; // Can't detect orientation }
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) { bool success = true; char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); int fd = open(s, O_RDONLY); if (fd < 0) { if (!optional) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } return false; } int calibration_devid; int ret; int devid = ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_ACC%u_ID", instance); param_get(param_find(s), &(calibration_devid)); if (devid != calibration_devid) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; goto out; } ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); success = false; goto out; } if (dynamic) { /* check measurement result range */ struct accel_report acc; ret = read(fd, &acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); /* this is frickin' fatal */ success = false; goto out; } } else { mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); /* this is frickin' fatal */ success = false; goto out; } } out: close(fd); return success; }
int do_accel_calibration(int mavlink_fd) { int fd; int32_t device_id[max_sens]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); sleep(3); mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); sleep(5); struct accel_scale accel_scale = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, }; int res = OK; char str[30]; /* reset all sensors */ for (unsigned s = 0; s < max_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); if (fd < 0) { continue; } device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } } float accel_offs[max_sens][3]; float accel_T[max_sens][3][3]; unsigned active_sensors; if (res == OK) { /* measure and calculate offsets & scales */ res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); } if (res != OK || active_sensors == 0) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); 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; get_rot_matrix(board_rotation_id, &board_rotation); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); for (unsigned i = 0; i < active_sensors; i++) { /* handle individual sensors, one by one */ math::Vector<3> accel_offs_vec(accel_offs[i]); math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix<3, 3> accel_T_mat(accel_T[i]); 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); accel_scale.y_offset = accel_offs_rotated(1); accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", i); failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set(param_find(str), &(device_id[i]))); if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); return ERROR; } sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = 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); } if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; }
int do_mag_calibration(int mavlink_fd) { const unsigned max_mags = 3; int32_t device_id[max_mags]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); sleep(1); struct mag_scale mscale_null[max_mags] = { { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, } } ; int res = ERROR; char str[30]; unsigned calibrated_ok = 0; for (unsigned s = 0; s < max_mags; s++) { /* erase old calibration */ (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); int fd = open(str, O_RDONLY); if (fd < 0) { continue; } mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s); sleep(3); device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); /* ensure all scale fields are initialized tha same as the first struct */ (void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0])); res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } if (res == OK) { /* calibrate range */ res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration"); /* this is non-fatal - mark it accordingly */ res = OK; } } close(fd); if (res == OK) { res = calibrate_instance(mavlink_fd, s, device_id[s]); if (res == OK) { calibrated_ok++; } } } if (calibrated_ok) { mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); usleep(100000); mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); /* auto-save to EEPROM */ res = param_save_default(); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } } else { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; }
int rc_calibration_check(int mavlink_fd) { char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, _parameter_handles_rev, _parameter_handles_dz; unsigned map_fail_count = 0; const char *rc_map_mandatory[] = { "RC_MAP_MODE_SW", /* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/ 0 /* end marker */ }; unsigned j = 0; /* first check channel mappings */ while (rc_map_mandatory[j] != 0) { param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; j++; continue; } int32_t mapping; param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } if (mapping == 0) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } j++; } unsigned total_fail_count = 0; unsigned channels_failed = 0; for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { /* should the channel be enabled? */ uint8_t count = 0; /* initialize values to values failing the check */ float param_min = 0.0f; float param_max = 0.0f; float param_trim = 0.0f; float param_rev = 0.0f; float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f; /* min values */ sprintf(nbuf, "RC%d_MIN", i + 1); _parameter_handles_min = param_find(nbuf); param_get(_parameter_handles_min, ¶m_min); /* trim values */ sprintf(nbuf, "RC%d_TRIM", i + 1); _parameter_handles_trim = param_find(nbuf); param_get(_parameter_handles_trim, ¶m_trim); /* max values */ sprintf(nbuf, "RC%d_MAX", i + 1); _parameter_handles_max = param_find(nbuf); param_get(_parameter_handles_max, ¶m_max); /* channel reverse */ sprintf(nbuf, "RC%d_REV", i + 1); _parameter_handles_rev = param_find(nbuf); param_get(_parameter_handles_rev, ¶m_rev); /* channel deadzone */ sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles_dz = param_find(nbuf); param_get(_parameter_handles_dz, ¶m_dz); /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); /* give system time to flush error message in case there are more */ usleep(100000); count++; } total_fail_count += count; if (count) { channels_failed++; } } if (channels_failed) { sleep(2); mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); usleep(100000); } return total_fail_count + map_fail_count; }
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) { calibrate_return result = calibrate_return_ok; *active_sensors = 0; accel_worker_data_t worker_data; worker_data.mavlink_log_pub = mavlink_log_pub; worker_data.done_count = 0; bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; // Initialize subs to error condition so we know which ones are open and which are not for (size_t i=0; i<max_accel_sens; i++) { worker_data.subs[i] = -1; } uint64_t timestamps[max_accel_sens]; // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned accel_count = orb_group_count(ORB_ID(sensor_accel)); for (unsigned i = 0; i < accel_count; i++) { worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); if (worker_data.subs[i] < 0) { result = calibrate_return_error; break; } #ifdef __PX4_QURT // For QURT respectively the driver framework, we need to get the device ID by copying one report. struct accel_report accel_report; orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report); device_id[i] = accel_report.device_id; #endif /* store initial timestamp - used to infer which sensors are active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; if (device_id[i] != 0) { // Get priority int32_t prio; orb_priority(worker_data.subs[i], &prio); if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_id[i]; } } else { mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Accel #%u no device id, abort", i); result = calibrate_return_error; break; } } if (result == calibrate_return_ok) { int cancel_sub = calibrate_cancel_subscribe(); result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); calibrate_cancel_unsubscribe(cancel_sub); } /* close all subscriptions */ for (unsigned i = 0; i < max_accel_sens; i++) { if (worker_data.subs[i] >= 0) { /* figure out which sensors were active */ struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } px4_close(worker_data.subs[i]); } } if (result == calibrate_return_ok) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (result != calibrate_return_ok) { mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error"); break; } } } return result; }
int do_level_calibration(orb_advert_t *mavlink_log_pub) { const unsigned cal_time = 5; const unsigned cal_hz = 100; unsigned settle_time = 30; bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level"); param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); param_t board_rot_handle = param_find("SENS_BOARD_ROT"); // save old values if calibration fails float roll_offset_current; float pitch_offset_current; int32_t board_rot_current = 0; param_get(roll_offset_handle, &roll_offset_current); param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); // give attitude some time to settle if there have been changes to the board rotation parameters if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { settle_time = 0; } float zero = 0.0f; param_set(roll_offset_handle, &zero); param_set(pitch_offset_handle, &zero); px4_pollfd_struct_t fds[1]; fds[0].fd = att_sub; fds[0].events = POLLIN; float roll_mean = 0.0f; float pitch_mean = 0.0f; unsigned counter = 0; // sleep for some time hrt_abstime start = hrt_absolute_time(); while(hrt_elapsed_time(&start) < settle_time * 1000000) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); sleep(settle_time / 10); } start = hrt_absolute_time(); // average attitude for 5 seconds while(hrt_elapsed_time(&start) < cal_time * 1000000) { int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pollret <= 0) { // attitude estimator is not running mavlink_and_console_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot"); mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); goto out; } orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); roll_mean += att.roll; pitch_mean += att.pitch; counter++; } mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_log_pub, "not enough measurements taken"); success = false; goto out; } if (fabsf(roll_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_log_pub, "excess roll angle"); } else if (fabsf(pitch_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_log_pub, "excess pitch angle"); } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; param_set(roll_offset_handle, &roll_mean); param_set(pitch_offset_handle, &pitch_mean); success = true; } out: if (success) { mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); return 0; } else { // set old parameters param_set(roll_offset_handle, &roll_offset_current); param_set(pitch_offset_handle, &pitch_offset_current); mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); return 1; } }
calibrate_return calibrate_from_orientation(int mavlink_fd, int cancel_sub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void* worker_data, bool lenient_still_position) { calibrate_return result = calibrate_return_ok; // Setup subscriptions to onboard accel sensor int sub_accel = orb_subscribe(ORB_ID(sensor_combined)); if (sub_accel < 0) { mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel"); return calibrate_return_error; } unsigned orientation_failures = 0; // Rotate through all requested orientation while (true) { if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; break; } if (orientation_failures > 4) { result = calibrate_return_error; mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion"); break; } unsigned int side_complete_count = 0; // Update the number of completed sides for (unsigned i = 0; i < detect_orientation_side_count; i++) { if (side_data_collected[i]) { side_complete_count++; } } if (side_complete_count == detect_orientation_side_count) { // We have completed all sides, move on break; } /* inform user which orientations are still needed */ char pendingStr[256]; pendingStr[0] = 0; for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) { if (!side_data_collected[cur_orientation]) { strcat(pendingStr, " "); strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation)); } } mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr); mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side"); enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position); if (orient == DETECT_ORIENTATION_ERROR) { orientation_failures++; mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); continue; } /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient)); mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side"); continue; } mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient)); orientation_failures = 0; // Call worker routine result = calibration_worker(orient, cancel_sub, worker_data); if (result != calibrate_return_ok ) { break; } mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient)); // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); usleep(200000); } if (sub_accel >= 0) { px4_close(sub_accel); } return result; }
int do_accel_calibration(orb_advert_t *mavlink_log_pub) { #ifndef __PX4_QURT int fd; #endif mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); struct accel_calibration_s accel_scale; accel_scale.x_offset = 0.0f; accel_scale.x_scale = 1.0f; accel_scale.y_offset = 0.0f; accel_scale.y_scale = 1.0f; accel_scale.z_offset = 0.0f; accel_scale.z_scale = 1.0f; int res = OK; char str[30]; /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { #ifndef __PX4_QURT sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); if (fd < 0) { continue; } device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); if (res != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); } #else (void)sprintf(str, "CAL_ACC%u_XOFF", s); res = param_set(param_find(str), &accel_scale.x_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YOFF", s); res = param_set(param_find(str), &accel_scale.y_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZOFF", s); res = param_set(param_find(str), &accel_scale.z_offset); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_XSCALE", s); res = param_set(param_find(str), &accel_scale.x_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_YSCALE", s); res = param_set(param_find(str), &accel_scale.y_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); res = param_set(param_find(str), &accel_scale.z_scale); if (res != OK) { PX4_ERR("unable to reset %s", str); } #endif } float accel_offs[max_accel_sens][3]; float accel_T[max_accel_sens][3][3]; unsigned active_sensors; /* measure and calculate offsets & scales */ if (res == OK) { calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors); if (cal_return == calibrate_return_cancelled) { // Cancel message already displayed, nothing left to do return ERROR; } else if (cal_return == calibrate_return_ok) { res = OK; } else { res = ERROR; } } if (res != OK) { if (active_sensors == 0) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); } return ERROR; } /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); 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; get_rot_matrix(board_rotation_id, &board_rotation); math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); for (unsigned i = 0; i < active_sensors; i++) { /* handle individual sensors, one by one */ math::Vector<3> accel_offs_vec(accel_offs[i]); math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix<3, 3> accel_T_mat(accel_T[i]); 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); accel_scale.y_offset = accel_offs_rotated(1); accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); bool failed = false; failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i, (double)accel_scale.x_offset, (double)accel_scale.y_offset, (double)accel_scale.z_offset); PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i, (double)accel_scale.x_scale, (double)accel_scale.y_scale, (double)accel_scale.z_scale); /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; } #ifndef __PX4_QURT sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); fd = px4_open(str, 0); if (fd < 0) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist"); res = ERROR; } else { res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); px4_close(fd); } if (res != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i); } #endif } if (res == OK) { /* auto-save to EEPROM */ res = param_save_default(); if (res != OK) { mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG); } /* if there is a any preflight-check system response, let the barrage of messages through */ usleep(200000); mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); } else { mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ usleep(600000); return res; }
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; }
transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings arming_state_t new_arming_state, ///< arming state requested struct actuator_armed_s *armed, ///< current armed status bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { /* * Get sensing state if necessary */ int prearm_ret = OK; /* only perform the check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = prearm_check(status, mavlink_fd); } /* * Perform an atomic state update */ #ifdef __PX4_NUTTX irqstate_t flags = irqsave(); #endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; prearm_ret = OK; status->condition_system_sensors_initialized = true; /* recover from a prearm fail */ if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } else { armed->lockdown = false; } // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } // Perform power checks only if circuit breaker is not // engaged for these checks if (!status->circuit_breaker_engaged_power_check) { // Fail transition if power is not good if (!status->condition_power_input_valid) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); feedback_provided = true; valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages if (status->avionics_power_rail_voltage < 4.75f) { mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } } } } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } feedback_provided = true; } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { // Silent ignore feedback_provided = true; } // Sensors need to be initialized for STANDBY state, except for HIL } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { if (!fRunPreArmChecks) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); } feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition if (valid_transition) { armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } /* end of atomic state update */ #ifdef __PX4_NUTTX irqrestore(flags); #endif } if (ret == TRANSITION_DENIED) { #define WARNSTR "INVAL: %s - %s" /* only print to console here by default as this is too technical to be useful during operation */ warnx(WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); /* print to MAVLink if we didn't provide any feedback yet */ if (!feedback_provided) { mavlink_log_critical(mavlink_fd, WARNSTR, state_names[status->arming_state], state_names[new_arming_state]); } #undef WARNSTR } return ret; }
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); DevHandle h; DevMgr::getHandle(s, h); if (!h.isValid()) { if (!optional) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } } return false; } int ret = check_calibration(h, "CAL_ACC%u_ID", device_id); if (ret) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); } success = false; goto out; } ret = h.ioctl(ACCELIOCSELFTEST, 0); if (ret != OK) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret); } success = false; goto out; } #ifdef __PX4_NUTTX if (dynamic) { /* check measurement result range */ struct accel_report acc; ret = h.read(&acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); } /* this is frickin' fatal */ success = false; goto out; } } else { if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ"); } /* this is frickin' fatal */ success = false; goto out; } } #endif out: DevMgr::releaseHandle(h); return success; }
bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { /* select onboard/offboard mission */ int *mission_index_ptr; dm_item_t dm_item; struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; /* do not work on empty missions */ if (mission->count == 0) { return false; } /* move to next item if there is one */ if (mission_index_next < ((int)mission->count - 1)) { mission_index_next++; } if (onboard) { /* onboard mission */ mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds - if they are equal, we just reached the end */ if (*mission_index_ptr != (int)mission->count) { mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); } return false; } const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ struct mission_item_s mission_item_tmp; /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { /* do DO_JUMP as many times as requested */ if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) { /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR DO JUMP waypoint could not be written"); return false; } report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { if (is_current) { mavlink_log_critical(_navigator->get_mavlink_fd(), "DO JUMP repetitions completed"); } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } } else { /* if it's not a DO_JUMP, then we were successful */ memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); return true; } } /* we have given up, we don't want to cycle forever */ mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR DO JUMP is cycling, giving up"); return false; }
enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position) { const unsigned ndim = 3; struct sensor_combined_s sensor; float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel float ema_len = 0.5f; // EMA time constant in seconds const float normal_still_thr = 0.25; // normal still threshold float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us px4_pollfd_struct_t fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ hrt_abstime timeout = 30000000; hrt_abstime t_timeout = t_start + timeout; hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; unsigned poll_errcount = 0; while (true) { /* wait blocking for new data */ int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; for (unsigned i = 0; i < ndim; i++) { float di = 0.0f; switch (i) { case 0: di = sensor.accelerometer_m_s2[0]; break; case 1: di = sensor.accelerometer_m_s2[1]; break; case 2: di = sensor.accelerometer_m_s2[2]; break; } float d = di - accel_ema[i]; accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; } if (d > accel_disp[i]) { accel_disp[i] = d; } } /* still detector with hysteresis */ if (accel_disp[0] < still_thr2 && accel_disp[1] < still_thr2 && accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; } else { /* still since t_still */ if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } } } else if (accel_disp[0] > still_thr2 * 4.0f || accel_disp[1] > still_thr2 * 4.0f || accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); usleep(200000); t_still = 0; } } } else if (poll_ret == 0) { poll_errcount++; } if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); return DETECT_ORIENTATION_ERROR; } } 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 DETECT_ORIENTATION_TAIL_DOWN; // [ 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 DETECT_ORIENTATION_NOSE_DOWN; // [ -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 DETECT_ORIENTATION_LEFT; // [ 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 DETECT_ORIENTATION_RIGHT; // [ 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 DETECT_ORIENTATION_UPSIDE_DOWN; // [ 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 DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation"); return DETECT_ORIENTATION_ERROR; // Can't detect orientation }
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation"); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); /* * Detect if the system is rotating. * * We're detecting this as a general rotation on any axis, not necessary on the one we * asked the user for. This is because we really just need two roughly orthogonal axes * for a good result, so we're not constraining the user more than we have to. */ hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; const float gyro_int_thresh_rad = 0.5f; int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && fabsf(gyro_y_integral) < gyro_int_thresh_rad && fabsf(gyro_z_integral) < gyro_int_thresh_rad) { /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; close(sub_gyro); return result; } /* abort with timeout */ if (hrt_absolute_time() > detection_deadline) { result = calibrate_return_error; warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation."); break; } /* Wait clocking for new data on all gyro */ struct pollfd fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; int poll_ret = poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); /* ensure we have a valid first timestamp */ if (last_gyro > 0) { /* integrate */ float delta_t = (gyro.timestamp - last_gyro) / 1e6f; gyro_x_integral += gyro.x * delta_t; gyro_y_integral += gyro.y * delta_t; gyro_z_integral += gyro.z * delta_t; } last_gyro = gyro.timestamp; } } close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; calibration_counter_side = 0; while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; break; } // Wait clocking for new data on all 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) { fds[fd_count].fd = worker_data->sub_mag[cur_mag]; fds[fd_count].events = POLLIN; fd_count++; } } int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { int prev_count[max_mags]; bool rejected = false; for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag]; if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); // Check if this measurement is good to go in rejected = rejected || reject_sample(mag.x, mag.y, mag.z, worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], worker_data->calibration_counter_total[cur_mag], calibration_sides * worker_data->calibration_points_perside); worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; worker_data->calibration_counter_total[cur_mag]++; } } // Keep calibration of all mags in lockstep if (rejected) { // Reset counts, since one of the mags rejected the measurement for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; } } else { calibration_counter_side++; // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } } else { poll_errcount++; } if (poll_errcount > worker_data->calibration_points_perside * 3) { result = calibrate_return_error; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); break; } } if (result == calibrate_return_ok) { mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); } return result; }
int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) { /* 45 seconds */ uint64_t calibration_interval = 25 * 1000 * 1000; /* maximum 500 values */ const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; float *x = NULL; float *y = NULL; float *z = NULL; char str[30]; int res = OK; /* allocate memory */ mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); /* clean up */ if (x != NULL) { free(x); } if (y != NULL) { free(y); } if (z != NULL) { free(z); } res = ERROR; return res; } if (res == OK) { int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s); if (sub_mag < 0) { mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort"); res = ERROR; } else { struct mag_report mag; /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); /* calibrate offsets */ uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0U; while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = sub_mag; fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; z[calibration_counter] = mag.z; calibration_counter++; if (calibration_counter % (calibration_maxcount / 20) == 0) { mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); } } else { poll_errcount++; } if (poll_errcount > 1000) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); res = ERROR; break; } } close(sub_mag); } } float sphere_x; float sphere_y; float sphere_z; float sphere_radius; if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); res = ERROR; } } if (x != NULL) { free(x); } if (y != NULL) { free(y); } if (z != NULL) { free(z); } if (res == OK) { /* apply calibration and set parameters */ struct mag_scale mscale; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); int fd = open(str, 0); res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); } if (res == OK) { mscale.x_offset = sphere_x; mscale.y_offset = sphere_y; mscale.z_offset = sphere_z; res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } close(fd); if (res == OK) { bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_MAG%u_ID", s); failed |= (OK != param_set(param_find(str), &(device_id))); (void)sprintf(str, "CAL_MAG%u_XOFF", s); failed |= (OK != param_set(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", s); failed |= (OK != param_set(param_find(str), &(mscale.y_offset))); (void)sprintf(str, "CAL_MAG%u_ZOFF", s); failed |= (OK != param_set(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", s); failed |= (OK != param_set(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", s); failed |= (OK != param_set(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", s); failed |= (OK != param_set(param_find(str), &(mscale.z_scale))); if (failed) { res = ERROR; mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); } mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); } return res; }
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; }
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures) { bool failed = false; /* ---- MAG ---- */ if (checkMag) { bool prime_found = false; int32_t prime_id = 0; param_get(param_find("CAL_MAG_PRIME"), &prime_id); /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); int device_id = -1; if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) { failed = true; } if (device_id == prime_id) { prime_found = true; } } /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found"); } failed = true; } } /* ---- ACCEL ---- */ if (checkAcc) { bool prime_found = false; int32_t prime_id = 0; param_get(param_find("CAL_ACC_PRIME"), &prime_id); /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); int device_id = -1; if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) { failed = true; } if (device_id == prime_id) { prime_found = true; } } /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found"); } failed = true; } } /* ---- GYRO ---- */ if (checkGyro) { bool prime_found = false; int32_t prime_id = 0; param_get(param_find("CAL_GYRO_PRIME"), &prime_id); /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); int device_id = -1; if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) { failed = true; } if (device_id == prime_id) { prime_found = true; } } /* check if the primary device is present */ if (!prime_found && prime_id != 0) { if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found"); } failed = true; } } /* ---- BARO ---- */ if (checkBaro) { bool prime_found = false; int32_t prime_id = 0; param_get(param_find("CAL_BARO_PRIME"), &prime_id); /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); int device_id = -1; if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) { failed = true; } if (device_id == prime_id) { prime_found = true; } } // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational"); } failed = true; } } /* ---- AIRSPEED ---- */ if (checkAirspeed) { if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) { if (reportFailures) { mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed"); } failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { if (!gnssCheck(mavlink_log_pub, reportFailures)) { failed = true; } } #ifdef __PX4_QURT // WARNING: Preflight checks are important and should be added back when // all the sensors are supported PX4_WARN("WARNING: Preflight checks PASS always."); failed = false; #endif /* Report status */ return !failed; }