fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 22:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander fix and enforce code style

Browse Source
This commit is contained in:
Daniel Agar
2018-11-28 17:11:15 -05:00
parent 91721f2060
commit 48d9484ceb
16 changed files with 855 additions and 481 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/gyro_calibration.cpp
+87 -47
View File
@@ -1,35 +1,35 @@
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* 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.
*
****************************************************************************/
/****************************************************************************
*
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* 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.
*
****************************************************************************/
/**
* @file gyro_calibration.cpp
@@ -74,18 +74,20 @@ typedef struct {
sensor_gyro_s gyro_report_0;
} gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
{
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data);
unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0;
const unsigned calibration_count = 5000;
sensor_gyro_s gyro_report;
unsigned poll_errcount = 0;
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) {
sensor_correction.gyro_scale_0[i] = 1.0f;
sensor_correction.gyro_scale_1[i] = 1.0f;
@@ -94,6 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
@@ -120,6 +123,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (poll_ret > 0) {
unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) {
// Skip if instance has enough samples
@@ -134,9 +138,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (s == 0) {
// take a working copy
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) *
sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) *
sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) *
sensor_correction.gyro_scale_0[2];
// take a reference copy of the primary sensor including correction for thermal drift
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
@@ -145,14 +152,20 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
} else if (s == 1) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) *
sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) *
sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) *
sensor_correction.gyro_scale_1[2];
} else if (s == 2) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2];
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) *
sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) *
sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) *
sensor_correction.gyro_scale_2[2];
} else {
worker_data->gyro_scale[s].x_offset += gyro_report.x;
@@ -165,7 +178,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
// Maintain the sample count of the slowest sensor
// Maintain the sample count of the slowest sensor
if (calibration_counter[s] && calibration_counter[s] < update_count) {
update_count = calibration_counter[s];
}
@@ -236,6 +249,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
return PX4_ERROR;
@@ -246,6 +260,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
@@ -256,37 +271,50 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
}
#else
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
@@ -304,7 +332,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// Lock in to correct ORB instance
bool found_cur_gyro = false;
for(unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
sensor_gyro_s report{};
@@ -319,6 +348,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
// Device IDs match, correct ORB instance for this gyro
found_cur_gyro = true;
} else {
orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
}
@@ -332,8 +362,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#endif
}
if(!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro, worker_data.device_id[cur_gyro]);
if (!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
worker_data.device_id[cur_gyro]);
res = calibrate_return_error;
break;
}
@@ -347,6 +378,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
device_prio_max = prio;
device_id_primary = worker_data.device_id[cur_gyro];
}
} else {
calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
}
@@ -393,6 +425,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = PX4_OK;
}
}
try_count++;
} while (res == PX4_ERROR && try_count <= max_tries);
@@ -424,6 +457,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
@@ -437,11 +471,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += worker_data.gyro_scale[uorb_index].x_offset;
@@ -452,8 +488,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
val += worker_data.gyro_scale[uorb_index].z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
@@ -489,6 +527,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}
@@ -504,6 +543,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}