PX4 Firmware
PX4 Autopilot Software http://px4.io
common.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #pragma once
35 
36 #define TC_PRINT_DEBUG 0
37 #if TC_PRINT_DEBUG
38 #define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
39 #else
40 #define TC_DEBUG(fmt, ...)
41 #endif
42 
43 #include <px4_platform_common/log.h>
44 #include <mathlib/mathlib.h>
45 #include <lib/parameters/param.h>
46 
47 #include "polyfit.hpp"
48 
49 #define SENSOR_COUNT_MAX 3
50 
51 
52 #define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature
53 #define TC_ERROR_COMMUNICATION 112 ///< no sensors found
54 
55 /**
56  * Base class for temperature calibration types with abstract methods (for all different sensor types)
57  */
59 {
60 public:
61  TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
62  : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature),
63  _max_start_temperature(max_start_temperature) {}
64 
66 
67  /**
68  * check & update new sensor data.
69  * @return progress in range [0, 100], 110 when finished, <0 on error,
70  * -TC_ERROR_INITIAL_TEMP_TOO_HIGH if starting temperature is too hot
71  * -TC_ERROR_COMMUNICATION if no sensors found
72  */
73  virtual int update() = 0;
74 
75  /**
76  * do final fitting & write the parameters. Call this exactly once after update() returned 110
77  * @return 0 on success, <0 otherwise
78  */
79  virtual int finish() = 0;
80 
81  /** reset all driver-level calibration parameters */
82  virtual void reset_calibration() = 0;
83 
84 protected:
85 
86  /**
87  * set a system parameter (without system notification) and print an error if it fails
88  * @param format_str for example "CAL_GYRO%u_XOFF"
89  * @param index which index (will replace %u in format_str)
90  * @param value
91  * @return 0 on success
92  */
93  inline int set_parameter(const char *format_str, unsigned index, const void *value);
94 
95  float _min_temperature_rise; ///< minimum difference in temperature before the process finishes
96  float _min_start_temperature; ///< minimum temperature before the process starts
97  float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared
98 };
99 
100 
101 
102 int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
103 {
104  char param_str[30];
105  (void)sprintf(param_str, format_str, index);
106  int result = param_set_no_notification(param_find(param_str), value);
107 
108  if (result != 0) {
109  PX4_ERR("unable to reset %s (%i)", param_str, result);
110  }
111 
112  return result;
113 }
114 
115 /**
116  ** class TemperatureCalibrationCommon
117  * Common base class for all sensor types, contains shared code & data.
118  */
119 template <int Dim, int PolyfitOrder>
121 {
122 public:
123  TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
124  : TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {}
125 
126  virtual ~TemperatureCalibrationCommon() = default;
127 
128  /**
129  * @see TemperatureCalibrationBase::update()
130  */
131  int update()
132  {
133  int num_not_complete = 0;
134 
135  if (_num_sensor_instances == 0) {
136  return -TC_ERROR_COMMUNICATION;
137  }
138 
139  for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
140  int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
141 
142  if (status < 0) {
143  return status;
144  }
145 
146  num_not_complete += status;
147  }
148 
149  if (num_not_complete > 0) {
150  // calculate progress
151  float min_diff = _min_temperature_rise;
152 
153  for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
154  float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
155 
156  if (cur_diff < min_diff) {
157  min_diff = cur_diff;
158  }
159  }
160 
161  return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
162  }
163 
164  return 110;
165  }
166 
167 protected:
168 
169  struct PerSensorData {
170  float sensor_sample_filt[Dim + 1]; ///< last value is the temperature
172  unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces
173  from the last reading */
174  uint32_t device_id = 0; ///< ID for the sensor being calibrated
175  bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been
176  /// verified and the starting temperature set
177  bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase
178  bool tempcal_complete = false; ///< true when the calibration has been completed
179  float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C)
180  float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C)
181  float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the
182  calibration temperature range (deg C) */
183  };
184 
185  PerSensorData _data[SENSOR_COUNT_MAX];
186 
187  /**
188  * update a single sensor instance
189  * @return 0 when done, 1 not finished yet, <0 for an error
190  */
191  virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0;
192 
193  unsigned _num_sensor_instances{0};
194  int _sensor_subs[SENSOR_COUNT_MAX];
195 };
Base class for temperature calibration types with abstract methods (for all different sensor types) ...
Definition: common.h:58
static struct vehicle_status_s status
Definition: Commander.cpp:138
#define TC_ERROR_COMMUNICATION
no sensors found
Definition: common.h:53
__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.
Definition: parameters.cpp:820
float _min_temperature_rise
minimum difference in temperature before the process finishes
Definition: common.h:95
class TemperatureCalibrationCommon Common base class for all sensor types, contains shared code & dat...
Definition: common.h:120
virtual void reset_calibration()=0
reset all driver-level calibration parameters
static int32_t device_id[max_accel_sens]
int set_parameter(const char *format_str, unsigned index, const void *value)
set a system parameter (without system notification) and print an error if it fails ...
Definition: common.h:102
Global flash based parameter store.
TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
Definition: common.h:61
virtual int finish()=0
do final fitting & write the parameters.
uint8_t * data
Definition: dataman.cpp:149
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
Definition: common.h:123
virtual ~TemperatureCalibrationBase()
Definition: common.h:65
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#define SENSOR_COUNT_MAX
Definition: common.h:49
float _max_start_temperature
maximum temperature above which the process does not start and an error is declared ...
Definition: common.h:97
virtual int update()=0
check & update new sensor data.
P[0][0]
Definition: quatCovMat.c:44
float _min_start_temperature
minimum temperature before the process starts
Definition: common.h:96
static struct mpu9x50_data _data
IMU measurement data.