From 5d92927991f9375dccc125ef229a1dec33bf6f55 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 10:25:40 +0200 Subject: [PATCH 01/15] make motors spin in POSCTRL and ATTCTRL when landed and throttle applied by user --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e4682689af..392b31cf42 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -98,7 +98,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode -#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -1606,8 +1607,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + if (_vehicle_status.condition_landed && + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) + { + // when we are landed in these modes we want the motor to spin + _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max); + } + + } /* During a takeoff waypoint while waiting for launch the pitch sp is set From 6a00fce009528a99512cd6c2d0b105a2a97bef3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:55:04 +0200 Subject: [PATCH 02/15] EKF: Publish global position also if GPS is not yet valid so that controllers can get a valid altitude --- .../ekf_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 84da033adb..4208ed0deb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + /* indicate consumers that the current position data is not valid */ + _gps.eph = 10000.0f; + _gps.epv = 10000.0f; + /* fetch initial parameter values */ parameters_update(); @@ -686,21 +690,21 @@ void AttitudePositionEstimatorEKF::task_main() continue; } - //Run EKF data fusion steps + // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - //Publish attitude estimations + // Publish attitude estimations publishAttitude(); - //Publish Local Position estimations + // Publish Local Position estimations publishLocalPosition(); - //Publish Global Position, but only if it's any good - if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { + // Publish Global Position, but only if it's any good + if (_gpsIsGood || _global_pos.dead_reckoning) { publishGlobalPosition(); } - //Publish wind estimates + // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } @@ -891,6 +895,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { + _global_pos.lat = 0.0; + _global_pos.lon = 0.0; + _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { @@ -907,6 +915,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { + _global_pos.vel_d = 0.0f; } /* terrain altitude */ From c46b4a29b8c0be96e40959305e52e6e753e555ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 20:00:38 +0200 Subject: [PATCH 03/15] EKF: Publish initial altitude estimate in any case --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4208ed0deb..877bff6585 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -699,10 +699,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish Local Position estimations publishLocalPosition(); - // Publish Global Position, but only if it's any good - if (_gpsIsGood || _global_pos.dead_reckoning) { - publishGlobalPosition(); - } + // Publish Global Position, it will have a large uncertainty + // set if only altitude is known + publishGlobalPosition(); // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { From f4845b2b8f8b65d756dc37d51514a4c58c9cdd05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:22:06 +0200 Subject: [PATCH 04/15] FW pos control: Guard against altitude estimate change --- .../fw_pos_control_l1_main.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 392b31cf42..79f25f3945 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -103,6 +103,7 @@ static int _control_task = -1; /**< task handle for sensor task */ static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** * L1 control app start / stop handling function @@ -174,7 +175,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ @@ -969,9 +970,24 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; + // XXX this should go into a manual stick mapper + // class + static float _althold_epv = 0.0f; static bool was_in_deadband = false; bool climbout_mode = false; + /* + * Reset the hold altitude to the current altitude if the uncertainty + * changes significantly. + * This is to guard against uncommanded altitude changes + * when the altitude certainty increases or decreases. + */ + + if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) { + _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; + } + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { @@ -988,6 +1004,7 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) * The aircraft should immediately try to fly at this altitude * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; was_in_deadband = true; } From f680bbed545eea3a23b174bac4ccda4bf96b027d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:23:17 +0200 Subject: [PATCH 05/15] FW pos control: Rename _ground_alt to _takeoff_ground_alt to make it less ambigious with the actual terrain altitude --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 79f25f3945..90e4da3479 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -175,10 +175,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _takeoff_ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ - bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1026,7 +1026,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc { /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ if (in_takeoff_situation()) { - *hold_altitude = _ground_alt + _parameters.climbout_diff; + *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); } else { *pitch_limit_min = _parameters.pitch_limit_min; @@ -1068,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_in_air && !_vehicle_status.condition_landed) { _was_in_air = true; _time_went_in_air = hrt_absolute_time(); - _ground_alt = _global_pos.alt; + _takeoff_ground_alt = _global_pos.alt; } /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { From 5cf20c8dcfeba450bcc926f4a73b81c382a9ad43 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:57:31 +0200 Subject: [PATCH 06/15] increase fw idle for ATTCTL and POSCTL to 0.2 --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 90e4da3479..95c8545e73 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -99,7 +99,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading -#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode From 4fadb65ac65852bdf78534f7c50d289a1338eba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 12:21:40 +0200 Subject: [PATCH 07/15] commander: Reject mag samples which are on top of each other --- src/modules/commander/mag_calibration.cpp | 109 ++++++++++++++++++---- 1 file changed, 92 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 04e66a5cb6..260316666c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,8 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; +static constexpr float mag_sphere_radius = 0.2f; +static const unsigned int calibration_sides = 3; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -76,7 +78,7 @@ typedef struct { unsigned int calibration_points_perside; unsigned int calibration_interval_perside_seconds; uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total; + unsigned int calibration_counter_total[max_mags]; bool side_data_collected[detect_orientation_side_count]; float* x[max_mags]; float* y[max_mags]; @@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd) return result; } +static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) +{ + float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; + //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; + + for (size_t i = 0; i < count; i++) { + float dx = sx - x[i]; + float dy = sy - y[i]; + float dz = sz - z[i]; + float dist = sqrtf(dx * dx + dy * dy + dz * dz); + + if (dist < min_sample_dist) { + return true; + } + } + + return false; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -286,27 +307,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = poll(fds, fd_count, 1000); if (poll_ret > 0) { + + int prev_count[max_mags]; + bool rejected = false; + for (size_t cur_mag=0; cur_magcalibration_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] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; - + 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]++; } } - - worker_data->calibration_counter_total++; - 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))); + + // 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++; } @@ -336,7 +377,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_counter_total = 0; worker_data.calibration_points_perside = 80; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; @@ -357,9 +397,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag 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_sides = 3; const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; @@ -438,7 +478,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // 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, + 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]); @@ -450,6 +490,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } } } + + // 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", cur_mag, 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", cur_mag, 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", (double)sphere_radius[cur_mag]); + printf(">>>>>>>\n"); + } + } // Data points are no longer needed for (size_t cur_mag=0; cur_mag Date: Wed, 24 Jun 2015 12:22:01 +0200 Subject: [PATCH 08/15] Tools: Add Matlab script to plot mag data --- Tools/Matlab/plot_mag.m | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 Tools/Matlab/plot_mag.m diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m new file mode 100644 index 0000000000..5fa4db4c23 --- /dev/null +++ b/Tools/Matlab/plot_mag.m @@ -0,0 +1,41 @@ +% +% Tool for plotting mag data +% +close all; +clear all; + +plot_scale = 0.8; + +xmax = plot_scale; +xmin = -xmax; +ymax = plot_scale; +ymin = -ymax; +zmax = plot_scale; +zmin = -zmax; + +mag0_raw = load('../../mag0_raw.csv'); +mag1_raw = load('../../mag1_raw.csv'); + +mag0_cal = load('../../mag0_cal.csv'); +mag1_cal = load('../../mag1_cal.csv'); + +fm0r = figure(); + +mag0_x_scale = 1.07; +mag0_y_scale = 0.95; +mag0_z_scale = 1.00; + +plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1r = figure(); +plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm0c = figure(); +plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1c = figure(); +plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) From a4a6e69521658bae87597819274bd417110a44cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:42:59 +0200 Subject: [PATCH 09/15] Matlab tools: Add ellipsoid fit --- Tools/Matlab/ellipsoid_fit.m | 174 +++++++++++++++++++++++++++++++++++ Tools/Matlab/plot_mag.m | 31 +++++-- 2 files changed, 199 insertions(+), 6 deletions(-) create mode 100644 Tools/Matlab/ellipsoid_fit.m diff --git a/Tools/Matlab/ellipsoid_fit.m b/Tools/Matlab/ellipsoid_fit.m new file mode 100644 index 0000000000..d288aa3821 --- /dev/null +++ b/Tools/Matlab/ellipsoid_fit.m @@ -0,0 +1,174 @@ +% Copyright (c) 2009, Yury Petrov +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * Redistributions in binary form must reproduce the above copyright +% notice, this list of conditions and the following disclaimer in +% the documentation and/or other materials provided with the distribution +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE. +% + +function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals ) +% +% Fit an ellispoid/sphere to a set of xyz data points: +% +% [center, radii, evecs, pars ] = ellipsoid_fit( X ) +% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 ); +% +% Parameters: +% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors +% * flag - 0 fits an arbitrary ellipsoid (default), +% - 1 fits an ellipsoid with its axes along [x y z] axes +% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad +% - 3 fits a sphere +% +% Output: +% * center - ellispoid center coordinates [xc; yc; zc] +% * ax - ellipsoid radii [a; b; c] +% * evecs - ellipsoid radii directions as columns of the 3x3 matrix +% * v - the 9 parameters describing the ellipsoid algebraically: +% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 +% +% Author: +% Yury Petrov, Northeastern University, Boston, MA +% + +error( nargchk( 1, 3, nargin ) ); % check input arguments +if nargin == 1 + flag = 0; % default to a free ellipsoid +end +if flag == 2 && nargin == 2 + equals = 'xy'; +end + +if size( X, 2 ) ~= 3 + error( 'Input data must have three columns!' ); +else + x = X( :, 1 ); + y = X( :, 2 ); + z = X( :, 3 ); +end + +% need nine or more data points +if length( x ) < 9 && flag == 0 + error( 'Must have at least 9 points to fit a unique ellipsoid' ); +end +if length( x ) < 6 && flag == 1 + error( 'Must have at least 6 points to fit a unique oriented ellipsoid' ); +end +if length( x ) < 5 && flag == 2 + error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' ); +end +if length( x ) < 3 && flag == 3 + error( 'Must have at least 4 points to fit a unique sphere' ); +end + +if flag == 0 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x .* y, ... + 2 * x .* z, ... + 2 * y .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 9 ellipsoid parameters +elseif flag == 1 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 6 ellipsoid parameters +elseif flag == 2 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1, + % where A = B or B = C or A = C + if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + D = [ y .* y + z .* z, ... + x .* x, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + D = [ x .* x + z .* z, ... + y .* y, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + else + D = [ x .* x + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + end +else + % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x + y .* y + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 4 sphere parameters +end + +% solve the normal system of equations +v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) ); + +% find the ellipsoid parameters +if flag == 0 + % form the algebraic form of the ellipsoid + A = [ v(1) v(4) v(5) v(7); ... + v(4) v(2) v(6) v(8); ... + v(5) v(6) v(3) v(9); ... + v(7) v(8) v(9) -1 ]; + % find the center of the ellipsoid + center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ]; + % form the corresponding translation matrix + T = eye( 4 ); + T( 4, 1:3 ) = center'; + % translate to the center + R = T * A * T'; + % solve the eigenproblem + [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) ); + radii = sqrt( 1 ./ diag( evals ) ); +else + if flag == 1 + v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; + elseif flag == 2 + if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; + elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; + else % xy + v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; + end + else + v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; + end + center = ( -v( 7:9 ) ./ v( 1:3 ) )'; + gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) ); + radii = ( sqrt( gam ./ v( 1:3 ) ) )'; + evecs = eye( 3 ); +end + + diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index 5fa4db4c23..f5dbfc5edd 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -1,6 +1,19 @@ % % Tool for plotting mag data % +% Reference values: +% telem> [cal] mag #0 off: x:0.15 y:0.07 z:0.14 Ga +% MATLAB: x:0.1581 y: 0.0701 z: 0.1439 Ga +% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02 +% MATLAB: 0.5499, 0.5190, 0.4907 +% +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% MATLAB: x:-0.1827 y:0.1147 z:-0.0848 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 +% MATLAB: 0.5122, 0.5065, 0.4915 +% +% + close all; clear all; @@ -13,11 +26,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw.csv'); -mag1_raw = load('../../mag1_raw.csv'); +mag0_raw = load('../../mag0_raw2.csv'); +mag1_raw = load('../../mag1_raw2.csv'); -mag0_cal = load('../../mag0_cal.csv'); -mag1_cal = load('../../mag1_cal.csv'); +mag0_cal = load('../../mag0_cal2.csv'); +mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); @@ -25,15 +38,21 @@ mag0_x_scale = 1.07; mag0_y_scale = 0.95; mag0_z_scale = 1.00; -plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); -plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From cae604ac1f8177775048dacdc899d4372efaf0ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:43:34 +0200 Subject: [PATCH 10/15] HMC5883: Increase the number of calibration cycles to ensure a stable result --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5bf88da3df..22c8c6359c 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } } - /* read the sensor up to 50x, stopping when we have 10 good values */ - for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + /* read the sensor up to 100x, stopping when we have 30 good values */ + for (uint8_t i = 0; i < 100 && good_count < 30; i++) { struct pollfd fds; /* wait for data to be ready */ From ef6092afd9c65d3937fe60e402f234eec5ffad38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:19 +0200 Subject: [PATCH 11/15] HMC5883: Calculate correct scaling to apply using multiplication --- src/drivers/hmc5883/hmc5883.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22c8c6359c..a9672dd7de 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = sum_excited[2] / good_count; /* set scaling in device */ - mscale_previous.x_scale = scaling[0]; - mscale_previous.y_scale = scaling[1]; - mscale_previous.z_scale = scaling[2]; + mscale_previous.x_scale = 1.0f / scaling[0]; + mscale_previous.y_scale = 1.0f / scaling[1]; + mscale_previous.z_scale = 1.0f / scaling[2]; ret = OK; From e28e4cb84cf387c5749fce8548954e82dcfc4761 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:48 +0200 Subject: [PATCH 12/15] Matlab mag: Update to real scaling, resulting fits confirm results --- Tools/Matlab/plot_mag.m | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index f5dbfc5edd..b344e41bc0 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -34,9 +34,9 @@ mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); -mag0_x_scale = 1.07; -mag0_y_scale = 0.95; -mag0_z_scale = 1.00; +mag0_x_scale = 0.88; +mag0_y_scale = 0.99; +mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); [center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); @@ -53,6 +53,9 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From 1fdc6a922115c235e1164aef81c2487750f9cf5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:51:59 +0200 Subject: [PATCH 13/15] commander: Remove unused min sample dist --- src/modules/commander/mag_calibration.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 260316666c..7af023fd82 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -189,7 +189,6 @@ int do_mag_calibration(int mavlink_fd) static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) { float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; - //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; for (size_t i = 0; i < count; i++) { float dx = sx - x[i]; From 338404b4b395c21d75c0c5599610d4cebbfb0ef1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 24 Jun 2015 13:19:46 -0700 Subject: [PATCH 14/15] Change mag cal to 6 orientations --- src/modules/commander/calibration_messages.h | 2 +- src/modules/commander/mag_calibration.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 53775ffe4f..1dbc5b6dbf 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -49,7 +49,7 @@ // instead of visual calibration until such a time as QGC is update to the new version. // The number in the cal started message is used to indicate the version stamp for the current calibration code. -#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" #define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7af023fd82..36cc0cdd5d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -66,7 +66,7 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static const unsigned int calibration_sides = 3; +static const unsigned int calibration_sides = 6; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -376,7 +376,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_points_perside = 80; + 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; @@ -384,9 +384,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag 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] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + 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>>>>>>\n"); } } From c402d0c2f7f856e8fd6b94be3ef2824a423f7843 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 13:20:43 +0200 Subject: [PATCH 15/15] Commander: updated mag calibration routine, matlab script updates --- Tools/Matlab/plot_mag.m | 34 +++++++++++++------ .../commander/calibration_routines.cpp | 6 ++-- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index b344e41bc0..c9f0c29925 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -13,6 +13,12 @@ % MATLAB: 0.5122, 0.5065, 0.4915 % % +% User-guided values: +% +% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga +% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95 +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 close all; clear all; @@ -26,11 +32,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw2.csv'); -mag1_raw = load('../../mag1_raw2.csv'); +mag0_raw = load('../../mag0_raw3.csv'); +mag1_raw = load('../../mag1_raw3.csv'); -mag0_cal = load('../../mag0_cal2.csv'); -mag1_cal = load('../../mag1_cal2.csv'); +mag0_cal = load('../../mag0_cal3.csv'); +mag1_cal = load('../../mag1_cal3.csv'); fm0r = figure(); @@ -39,10 +45,11 @@ mag0_y_scale = 0.99; mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); -center -radii +[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +mag0_raw_center +mag0_raw_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]); fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); @@ -53,11 +60,18 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); -center -radii +[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] ); +mag0_cal_center +mag0_cal_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([0, 0], [mag0_cal_radii(3)]); fm1c = figure(); plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); axis([xmin xmax ymin ymax zmin zmax]) +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +viscircles([0, 0], [radii(3)]); + +mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1)) +mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2)) +mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3)) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index e9f83775d7..045dbb032d 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -243,7 +243,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub 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 ? 1000000 : 1500000; // still time required in us + hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us struct pollfd fds[1]; fds[0].fd = accel_sub; @@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); - usleep(500000); + usleep(200000); t_still = 0; } } @@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); - usleep(500000); + usleep(200000); } if (sub_accel >= 0) {