130 #include <px4_platform_common/defines.h> 131 #include <px4_platform_common/posix.h> 132 #include <px4_platform_common/time.h> 140 #include <mathlib/mathlib.h> 160 float (&accel_offs)[
max_accel_sens][3],
float (&accel_T)[max_accel_sens][3][3],
unsigned *active_sensors);
166 float (&accel_offs)[max_accel_sens][3],
float g);
218 (void)sprintf(str,
"CAL_ACC%u_XOFF", s);
222 PX4_ERR(
"unable to reset %s", str);
225 (void)sprintf(str,
"CAL_ACC%u_YOFF", s);
229 PX4_ERR(
"unable to reset %s", str);
232 (void)sprintf(str,
"CAL_ACC%u_ZOFF", s);
236 PX4_ERR(
"unable to reset %s", str);
239 (void)sprintf(str,
"CAL_ACC%u_XSCALE", s);
243 PX4_ERR(
"unable to reset %s", str);
246 (void)sprintf(str,
"CAL_ACC%u_YSCALE", s);
250 PX4_ERR(
"unable to reset %s", str);
253 (void)sprintf(str,
"CAL_ACC%u_ZSCALE", s);
257 PX4_ERR(
"unable to reset %s", str);
266 unsigned active_sensors = 0;
285 if (active_sensors == 0) {
294 int32_t board_rotation_int;
295 param_get(board_rotation_h, &(board_rotation_int));
301 bool tc_locked[3] = {
false};
303 for (
unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
309 matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
311 accel_scale.
x_offset = accel_offs_rotated(0);
312 accel_scale.
x_scale = accel_T_rotated(0, 0);
313 accel_scale.
y_offset = accel_offs_rotated(1);
314 accel_scale.
y_scale = accel_T_rotated(1, 1);
315 accel_scale.
z_offset = accel_offs_rotated(2);
316 accel_scale.
z_scale = accel_T_rotated(2, 2);
323 PX4_INFO(
"found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
327 PX4_INFO(
"found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
333 int32_t tc_enabled_int;
336 if (tc_enabled_int == 1) {
340 sensor_correction_sub.copy(&sensor_correction);
343 if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
344 tc_locked[sensor_correction.accel_mapping[uorb_index]] =
true;
350 for (
unsigned axis_index = 0; axis_index < 3; axis_index++) {
352 (void)sprintf(str,
"TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
356 if (axis_index == 0) {
359 }
else if (axis_index == 1) {
362 }
else if (axis_index == 2) {
370 for (
unsigned axis_index = 0; axis_index < 3; axis_index++) {
372 (void)sprintf(str,
"TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
375 if (axis_index == 0) {
378 }
else if (axis_index == 1) {
381 }
else if (axis_index == 2) {
401 (void)sprintf(str,
"CAL_ACC%u_XOFF", uorb_index);
403 (void)sprintf(str,
"CAL_ACC%u_YOFF", uorb_index);
405 (void)sprintf(str,
"CAL_ACC%u_ZOFF", uorb_index);
407 (void)sprintf(str,
"CAL_ACC%u_XSCALE", uorb_index);
409 (void)sprintf(str,
"CAL_ACC%u_YSCALE", uorb_index);
411 (void)sprintf(str,
"CAL_ACC%u_ZSCALE", uorb_index);
413 (void)sprintf(str,
"CAL_ACC%u_ID", uorb_index);
459 const unsigned samples_num = 750;
470 (
double)worker_data->
accel_ref[0][orientation][0],
471 (
double)worker_data->
accel_ref[0][orientation][1],
472 (
double)worker_data->
accel_ref[0][orientation][2]);
481 float (&accel_offs)[max_accel_sens][3],
float (&accel_T)[max_accel_sens][3][3],
unsigned *active_sensors)
499 worker_data.
subs[i] = -1;
508 if (orb_accel_count > max_accel_sens) {
513 for (
unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel <
max_accel_sens; cur_accel++) {
516 bool found_cur_accel =
false;
518 for (
unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
532 found_cur_accel =
true;
534 timestamps[cur_accel] = report.
timestamp;
544 found_cur_accel =
true;
549 if (!found_cur_accel) {
581 if (worker_data.
subs[i] >= 0) {
598 for (
unsigned i = 0; i < (*active_sensors); i++) {
615 float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3],
unsigned orient,
unsigned samples_num)
623 float board_offset[3];
624 param_get(board_offset_x, &board_offset[0]);
625 param_get(board_offset_y, &board_offset[1]);
626 param_get(board_offset_z, &board_offset[2]);
629 M_DEG_TO_RAD_F * board_offset[0],
630 M_DEG_TO_RAD_F * board_offset[1],
631 M_DEG_TO_RAD_F * board_offset[2]);
633 int32_t board_rotation_int;
634 param_get(board_rotation_h, &(board_rotation_int));
642 fds[i].events = POLLIN;
648 unsigned errcount = 0;
652 if (
orb_copy(
ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
654 memset(&sensor_correction, 0,
sizeof(sensor_correction));
656 for (
unsigned i = 0; i < 3; i++) {
664 while (counts[0] < samples_num) {
665 int poll_ret =
px4_poll(&fds[0], max_accel_sens, 1000);
695 accel_sum[s][0] += arp.
x;
696 accel_sum[s][1] += arp.
y;
697 accel_sum[s][2] += arp.
z;
709 if (errcount > samples_num / 10) {
717 accel_sum_vec = board_rotation * accel_sum_vec;
719 for (
size_t j = 0; j < 3; j++) {
720 accel_sum[i][j] = accel_sum_vec(j);
725 for (
unsigned i = 0; i < 3; i++) {
726 accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
735 float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
736 src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
737 src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
743 dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
744 dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
745 dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
746 dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
747 dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
748 dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
749 dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
750 dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
751 dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
757 float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3],
float (&accel_T)[max_accel_sens][3][3],
758 float (&accel_offs)[max_accel_sens][3],
float g)
761 for (
unsigned i = 0; i < 3; i++) {
762 accel_offs[sensor][i] = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
767 memset(mat_A, 0,
sizeof(mat_A));
769 for (
unsigned i = 0; i < 3; i++) {
770 for (
unsigned j = 0; j < 3; j++) {
771 float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
777 float mat_A_inv[3][3];
784 for (
unsigned i = 0; i < 3; i++) {
785 for (
unsigned j = 0; j < 3; j++) {
787 accel_T[sensor][j][i] = mat_A_inv[j][i] * g;
796 const unsigned cal_time = 5;
797 const unsigned cal_hz = 100;
798 unsigned settle_time = 30;
800 bool success =
false;
803 memset(&att, 0,
sizeof(att));
812 float roll_offset_current;
813 float pitch_offset_current;
814 int32_t board_rot_current = 0;
815 param_get(roll_offset_handle, &roll_offset_current);
816 param_get(pitch_offset_handle, &pitch_offset_current);
817 param_get(board_rot_handle, &board_rot_current);
820 if (board_rot_current == 0 && fabsf(roll_offset_current) <
FLT_EPSILON && fabsf(pitch_offset_current) <
FLT_EPSILON) {
829 px4_pollfd_struct_t fds[1];
832 fds[0].events = POLLIN;
834 float roll_mean = 0.0f;
835 float pitch_mean = 0.0f;
844 px4_sleep(settle_time / 10);
851 int pollret =
px4_poll(&fds[0], (
sizeof(fds) /
sizeof(fds[0])), 100);
862 roll_mean += euler.
phi();
863 pitch_mean += euler.
theta();
869 if (counter > (cal_time * cal_hz / 2)) {
879 if (fabsf(roll_mean) > 0.8
f) {
882 }
else if (fabsf(pitch_mean) > 0.8
f) {
886 roll_mean *= (float)M_RAD_TO_DEG;
887 pitch_mean *= (float)M_RAD_TO_DEG;
#define CAL_ERROR_SENSOR_MSG
static orb_advert_t * mavlink_log_pub
calibrate_return calculate_calibration_values(unsigned sensor, float(&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float(&accel_T)[max_accel_sens][3][3], float(&accel_offs)[max_accel_sens][3], float g)
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
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)
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition of geo / math functions to perform geodesic calculations.
__EXPORT int param_set_no_notification(param_t param, const void *val)
Set the value of a parameter, but do not notify the system about the change.
Common calibration messages.
int do_level_calibration(orb_advert_t *mavlink_log_pub)
detect_orientation_return
int orb_priority(int handle, int32_t *priority)
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, 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)
Perform calibration sequence which require a rest orientation detection prior to calibration.
float accel_ref[max_accel_sens][detect_orientation_side_count][3]
#define CAL_ERROR_RESET_CAL_MSG
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
orb_advert_t * mavlink_log_pub
static const unsigned max_accel_sens
static const char * sensor_name
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
Matrix< Type, N, M > transpose() const
static constexpr float CONSTANTS_ONE_G
accel scaling factors; Vout = Vscale * (Vin + Voffset)
Global flash based parameter store.
#define ACCEL_BASE_DEVICE_PATH
void calibrate_cancel_unsubscribe(int cmd_sub)
Called to cancel the subscription to the cancel command.
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
const char * detect_orientation_str(enum detect_orientation_return orientation)
Returns the human readable string representation of the orientation.
Commander helper functions definitions.
int sensor_correction_sub
Rotation
Enum for board and external compass rotations.
#define CAL_ERROR_APPLY_CAL_MSG
int orb_unsubscribe(int handle)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
#define CAL_QGC_STARTED_MSG
#define CAL_ERROR_SET_PARAMS_MSG
static int device_prio_max
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
static int32_t device_id_primary
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
int mat_invert3(float src[3][3], float dst[3][3])
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int(&subs)[max_accel_sens], float(&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
#define ACCELIOCSSCALE
set the accel scaling constants to the structure pointed to by (arg)
Quaternion< float > Quatf
__EXPORT void param_notify_changes(void)
Notify the system about parameter changes.
int orb_check(int handle, bool *updated)
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition of accelerometer calibration.
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
#define calibration_log_critical(_pub, _text,...)
Data passed to calibration worker routine.
static const unsigned detect_orientation_side_count
#define CAL_QGC_PROGRESS_MSG
#define CAL_QGC_FAILED_MSG
int calibrate_cancel_subscribe()
Called at the beginning of calibration in order to subscribe to the cancel command.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
#define calibration_log_info(_pub, _text,...)