|
Firmware
|
Calibration routines implementations. More...
#include <px4_defines.h>#include <px4_posix.h>#include <px4_time.h>#include <stdio.h>#include <unistd.h>#include <math.h>#include <float.h>#include <poll.h>#include <drivers/drv_hrt.h>#include <systemlib/mavlink_log.h>#include <lib/ecl/geo/geo.h>#include <string.h>#include <mathlib/mathlib.h>#include <matrix/math.hpp>#include <uORB/topics/vehicle_command.h>#include <uORB/topics/sensor_combined.h>#include <drivers/drv_tone_alarm.h>#include "calibration_routines.h"#include "calibration_messages.h"#include "commander_helper.h"Functions | |
| int | sphere_fit_least_squares (const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) |
| Least-squares fit of a sphere to a set of points. More... | |
| int | ellipsoid_fit_least_squares (const float x[], const float y[], const float z[], unsigned int size, int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z, float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z) |
| int | run_lm_sphere_fit (const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, unsigned int size, float *offset_x, float *offset_y, float *offset_z, float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z) |
| int | run_lm_ellipsoid_fit (const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, unsigned int size, float *offset_x, float *offset_y, float *offset_z, float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z) |
| enum detect_orientation_return | detect_orientation (orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position) |
| Wait for vehicle to become still and detect it's orientation. More... | |
| const char * | detect_orientation_str (enum detect_orientation_return orientation) |
| Returns the human readable string representation of the orientation. More... | |
| 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. More... | |
| int | calibrate_cancel_subscribe () |
| Called at the beginning of calibration in order to subscribe to the cancel command. More... | |
| void | calibrate_cancel_unsubscribe (int cmd_sub) |
| Called to cancel the subscription to the cancel command. More... | |
| bool | calibrate_cancel_check (orb_advert_t *mavlink_log_pub, int cancel_sub) |
| Used to periodically check for a cancel command. More... | |
Calibration routines implementations.
| bool calibrate_cancel_check | ( | orb_advert_t * | mavlink_log_pub, |
| int | cancel_sub | ||
| ) |
Used to periodically check for a cancel command.
Cancel subcription fromcalibration_cancel_subscribe
| mavlink_log_pub | uORB handle to write output to |
| int calibrate_cancel_subscribe | ( | void | ) |
Called at the beginning of calibration in order to subscribe to the cancel command.
| void calibrate_cancel_unsubscribe | ( | int | cancel_sub | ) |
Called to cancel the subscription to the cancel command.
| cancel_sub | Cancel subcription from calibration_cancel_subscribe |
| 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_detection | ||
| ) |
Perform calibration sequence which require a rest orientation detection prior to calibration.
| mavlink_log_pub | uORB handle to write output to |
| cancel_sub | Cancel subscription from calibration_cancel_subscribe |
| side_data_collected | Sides for which data still needs calibration |
| calibration_worker | Worker routine which performs the actual calibration |
| worker_data | Opaque data passed to worker routine |
| enum detect_orientation_return detect_orientation | ( | orb_advert_t * | mavlink_log_pub, |
| int | cancel_sub, | ||
| int | accel_sub, | ||
| bool | lenient_still_detection | ||
| ) |
Wait for vehicle to become still and detect it's orientation.
| mavlink_log_pub | uORB handle to write output to |
| cancel_sub | Cancel subscription from calibration_cancel_subscribe |
| accel_sub | Orb subcription to accel sensor |
| const char* detect_orientation_str | ( | enum detect_orientation_return | orientation | ) |
Returns the human readable string representation of the orientation.
| orientation | Orientation to return string for, "error" if buffer is too small |
| int sphere_fit_least_squares | ( | const float | x[], |
| const float | y[], | ||
| const float | z[], | ||
| unsigned int | size, | ||
| unsigned int | max_iterations, | ||
| float | delta, | ||
| float * | sphere_x, | ||
| float * | sphere_y, | ||
| float * | sphere_z, | ||
| float * | sphere_radius | ||
| ) |
Least-squares fit of a sphere to a set of points.
Fits a sphere to a set of points on the sphere surface.
| x | point coordinates on the X axis |
| y | point coordinates on the Y axis |
| z | point coordinates on the Z axis |
| size | number of points |
| max_iterations | abort if maximum number of iterations have been reached. If unsure, set to 100. |
| delta | abort if error is below delta. If unsure, set to 0 to run max_iterations times. |
| sphere_x | coordinate of the sphere center on the X axis |
| sphere_y | coordinate of the sphere center on the Y axis |
| sphere_z | coordinate of the sphere center on the Z axis |
| sphere_radius | sphere radius |
1.8.12