PX4 Firmware
PX4 Autopilot Software http://px4.io
accel.cpp
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 /**
35  * @file accel.cpp
36  * Implementation of the Accel Temperature Calibration for onboard sensors.
37  *
38  * @author Siddharth Bharat Purohit
39  * @author Paul Riseborough
40  * @author Beat Küng <beat-kueng@gmx.net>
41  */
42 
43 #include "accel.h"
45 #include <mathlib/mathlib.h>
46 #include <drivers/drv_hrt.h>
47 
48 TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature,
49  float max_start_temperature)
50  : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
51 {
52 
53  //init subscriptions
55 
58  }
59 
60  for (unsigned i = 0; i < _num_sensor_instances; i++) {
61  _sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
62  }
63 }
64 
66 {
67  for (unsigned i = 0; i < _num_sensor_instances; i++) {
69  }
70 }
71 
73 {
74  /* reset all driver level calibrations */
75  float offset = 0.0f;
76  float scale = 1.0f;
77 
78  for (unsigned s = 0; s < 3; s++) {
79  set_parameter("CAL_ACC%u_XOFF", s, &offset);
80  set_parameter("CAL_ACC%u_YOFF", s, &offset);
81  set_parameter("CAL_ACC%u_ZOFF", s, &offset);
82  set_parameter("CAL_ACC%u_XSCALE", s, &scale);
83  set_parameter("CAL_ACC%u_YSCALE", s, &scale);
84  set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
85  }
86 }
87 
88 int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
89 {
90  bool finished = data.hot_soaked;
91 
92  bool updated;
93  orb_check(sensor_sub, &updated);
94 
95  if (!updated) {
96  return finished ? 0 : 1;
97  }
98 
99  sensor_accel_s accel_data;
100  orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
101 
102  if (finished) {
103  // if we're done, return, but we need to return after orb_copy because of poll()
104  return 0;
105  }
106 
107  data.device_id = accel_data.device_id;
108 
109  data.sensor_sample_filt[0] = accel_data.x;
110  data.sensor_sample_filt[1] = accel_data.y;
111  data.sensor_sample_filt[2] = accel_data.z;
112  data.sensor_sample_filt[3] = accel_data.temperature;
113 
114  // wait for min start temp to be reached before starting calibration
115  if (data.sensor_sample_filt[3] < _min_start_temperature) {
116  return 1;
117  }
118 
119  if (!data.cold_soaked) {
120  // allow time for sensors and filters to settle
121  if (hrt_absolute_time() > 10E6) {
122  // If intial temperature exceeds maximum declare an error condition and exit
123  if (data.sensor_sample_filt[3] > _max_start_temperature) {
125 
126  } else {
127  data.cold_soaked = true;
128  data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
129  data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
130  data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
131  return 1;
132  }
133 
134  } else {
135  return 1;
136  }
137  }
138 
139  // check if temperature increased
140  if (data.sensor_sample_filt[3] > data.high_temp) {
141  data.high_temp = data.sensor_sample_filt[3];
142  data.hot_soak_sat = 0;
143 
144  } else {
145  return 1;
146  }
147 
148  //TODO: Detect when temperature has stopped rising for more than TBD seconds
149  if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
150  data.hot_soaked = true;
151  }
152 
153  if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
154  TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
155  (double)data.sensor_sample_filt[1],
156  (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
157  (double)(data.high_temp - data.low_temp));
158  }
159 
160  //update linear fit matrices
161  double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
162  data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
163  data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
164  data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
165 
166  return 1;
167 }
168 
170 {
171  for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
172  finish_sensor_instance(_data[uorb_index], uorb_index);
173  }
174 
175  int32_t enabled = 1;
176  int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
177 
178  if (result != PX4_OK) {
179  PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
180  }
181 
182  return result;
183 }
184 
185 int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
186 {
187  if (!data.hot_soaked || data.tempcal_complete) {
188  return 0;
189  }
190 
191  double res[3][4] = {};
192  data.P[0].fit(res[0]);
193  res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
194  PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
195  (double)res[0][2],
196  (double)res[0][3]);
197  data.P[1].fit(res[1]);
198  res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
199  PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
200  (double)res[1][2],
201  (double)res[1][3]);
202  data.P[2].fit(res[2]);
203  res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
204  PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
205  (double)res[2][2],
206  (double)res[2][3]);
207  data.tempcal_complete = true;
208 
209  char str[30];
210  float param = 0.0f;
211  int result = PX4_OK;
212 
213  set_parameter("TC_A%d_ID", sensor_index, &data.device_id);
214 
215  for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
216  for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
217  sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
218  param = (float)res[axis_index][coef_index];
219  result = param_set_no_notification(param_find(str), &param);
220 
221  if (result != PX4_OK) {
222  PX4_ERR("unable to reset %s", str);
223  }
224  }
225  }
226 
227  set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
228  set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
229  set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
230  return 0;
231 }
int _sensor_subs[SENSOR_COUNT_MAX]
Definition: common.h:194
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub)
update a single sensor instance
Definition: accel.cpp:88
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
__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
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH
starting temperature was above the configured allowed temperature
Definition: common.h:52
class TemperatureCalibrationCommon Common base class for all sensor types, contains shared code & dat...
Definition: common.h:120
High-resolution timer with callouts and timekeeping.
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
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
Definition: accel.cpp:48
uint8_t * data
Definition: dataman.cpp:149
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
Definition: uORB.cpp:110
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
virtual ~TemperatureCalibrationAccel()
Definition: accel.cpp:65
#define SENSOR_COUNT_MAX
Definition: common.h:49
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition: uORB.cpp:80
float _max_start_temperature
maximum temperature above which the process does not start and an error is declared ...
Definition: common.h:97
uint32_t device_id
Definition: sensor_accel.h:55
float _min_start_temperature
minimum temperature before the process starts
Definition: common.h:96
int finish_sensor_instance(PerSensorData &data, int sensor_index)
Definition: accel.cpp:185
#define TC_DEBUG(fmt,...)
Definition: common.h:40
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
PerSensorData _data[SENSOR_COUNT_MAX]
Definition: common.h:185