44 #include <mathlib/mathlib.h> 45 #include <px4_platform_common/log.h> 46 #include <px4_platform_common/posix.h> 47 #include <px4_platform_common/tasks.h> 87 static int do_temperature_calibration(
int argc,
char *argv[]);
98 bool _force_task_exit =
false;
99 int _control_task = -1;
122 for (
unsigned i = 0; i < num_gyro; i++) {
124 fds[i].fd = gyro_sub[i];
125 fds[i].events = POLLIN;
128 int32_t min_temp_rise = 24;
130 PX4_INFO(
"Waiting for %i degrees difference in sensor temperature", min_temp_rise);
132 int32_t min_start_temp = 5;
135 int32_t max_start_temp = 10;
140 bool error_reported[3] = {};
141 int num_calibrators = 0;
146 if (calibrators[num_calibrators]) {
150 PX4_ERR(
"alloc failed");
157 if (calibrators[num_calibrators]) {
161 PX4_ERR(
"alloc failed");
169 if (calibrators[num_calibrators]) {
173 PX4_ERR(
"alloc failed");
178 for (
int i = 0; i < num_calibrators; ++i) {
192 led_control.
mode = led_control_s::MODE_BLINK_NORMAL;
193 led_control.
priority = led_control_s::MAX_PRIORITY;
194 led_control.
color = led_control_s::COLOR_YELLOW;
197 int leds_completed = 0;
199 bool abort_calibration =
false;
205 int ret =
px4_poll(fds, num_gyro, 1000);
212 }
else if (ret == 0) {
221 for (
unsigned i = 0; i < num_gyro; ++i) {
226 int min_progress = 110;
228 for (
int i = 0; i < num_calibrators; ++i) {
229 ret = calibrators[i]->
update();
232 abort_calibration =
true;
233 PX4_ERR(
"Calibration won't start - sensor bad or communication error");
238 abort_calibration =
true;
239 PX4_ERR(
"Calibration won't start - sensor temperature too high");
243 }
else if (ret < 0 && !error_reported[i]) {
245 error_reported[i] =
true;
246 PX4_ERR(
"Calibration update step failed (%i)", ret);
248 }
else if (ret < min_progress) {
254 if (min_progress == 110 || abort_calibration) {
260 for (; leds_completed < led_progress; ++leds_completed) {
261 led_control.
led_mask = 1 << leds_completed;
262 led_control.
mode = led_control_s::MODE_ON;
269 if (now > next_progress_output) {
270 PX4_INFO(
"Calibration progress: %i%%", min_progress);
271 next_progress_output = now + 1e6;
275 if (abort_calibration) {
276 led_control.
color = led_control_s::COLOR_RED;
279 PX4_INFO(
"Sensor Measurments completed");
285 for (
int i = 0; i < num_calibrators; ++i) {
286 int ret = calibrators[i]->
finish();
289 PX4_ERR(
"Failed to finish calibration process (%i)", ret);
297 PX4_ERR(
"Failed to save params (%i)", ret);
302 led_control.
color = led_control_s::COLOR_GREEN;
307 led_control.
mode = led_control_s::MODE_BLINK_FAST;
311 for (
int i = 0; i < num_calibrators; ++i) {
312 delete calibrators[i];
315 for (
unsigned i = 0; i < num_gyro; i++) {
321 PX4_INFO(
"Exiting temperature calibration task");
335 SCHED_PRIORITY_MAX - 5,
343 PX4_ERR(
"start failed");
358 PX4_INFO(
"Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (
int)accel, (
int)baro, (
int)gyro);
362 PX4_ERR(
"alloc failed");
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Base class for temperature calibration types with abstract methods (for all different sensor types) ...
TemperatureCalibration * instance
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
#define TC_ERROR_COMMUNICATION
no sensors found
static led_control_s led_control
static int do_temperature_calibration(int argc, char *argv[])
bool _baro
enable baro calibration?
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH
starting temperature was above the configured allowed temperature
bool publish(const T &data)
Publish the struct.
static void publish_led_control(led_control_s &led_control)
virtual void reset_calibration()=0
reset all driver-level calibration parameters
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Led device API to control the external LED(s) via uORB interface.
static sensor_accel_s _accel
accel report
int run_temperature_calibration(bool accel, bool baro, bool gyro)
start temperature calibration in a new task for one or multiple sensors
virtual int finish()=0
do final fitting & write the parameters.
int orb_unsubscribe(int handle)
__EXPORT int param_save_default(void)
Save parameters to the default file.
void task_main(int argc, char *argv[])
static sensor_gyro_s _gyro
gyro report
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
bool _accel
enable accel calibration?
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
bool _gyro
enable gyro calibration?
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
void publish_led_control(led_control_s &led_control)
__EXPORT void param_notify_changes(void)
Notify the system about parameter changes.
__EXPORT void param_control_autosave(bool enable)
Enable/disable the param autosaving.
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
virtual int update()=0
check & update new sensor data.
uORB::PublicationQueued< led_control_s > _led_control_pub
TemperatureCalibration(bool accel, bool baro, bool gyro)
Constructor.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).