fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 17:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers: re-use calibration topic

Browse Source 
E.g. instead of defining a gyro_scale struct in drv_gyro.h, use the
gyro_calibration message.
This commit is contained in:
Julian Oes
2016-03-07 23:06:06 +01:00
parent 98e407696e
commit b9cc0b74e2
16 changed files with 343 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+7 -8
View File
@@ -181,14 +181,13 @@ int do_accel_calibration(int mavlink_fd)
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = OK;
src/modules/commander/gyro_calibration.cpp
+9 -10
View File
@@ -73,7 +73,7 @@ typedef struct {
int mavlink_fd;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
struct gyro_scale gyro_scale[max_gyros];
struct gyro_calibration_s gyro_scale[max_gyros];
struct gyro_report gyro_report_0;
} gyro_worker_data_t;
@@ -159,14 +159,13 @@ int do_gyro_calibration(int mavlink_fd)
worker_data.mavlink_fd = mavlink_fd;
struct gyro_scale gyro_scale_zero = {
0.0f, // x offset
1.0f, // x scale
0.0f, // y offset
1.0f, // y scale
0.0f, // z offset
1.0f, // z scale
};
struct gyro_calibration_s gyro_scale_zero;
gyro_scale_zero.x_offset = 0.0f;
gyro_scale_zero.x_scale = 1.0f;
gyro_scale_zero.y_offset = 0.0f;
gyro_scale_zero.y_scale = 1.0f;
gyro_scale_zero.z_offset = 0.0f;
gyro_scale_zero.z_scale = 1.0f;
int device_prio_max = 0;
int32_t device_id_primary = 0;
@@ -192,7 +191,7 @@ int do_gyro_calibration(int mavlink_fd)
#endif
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
#ifndef __PX4_QURT
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
src/modules/commander/mag_calibration.cpp
+8 -9
View File
@@ -103,14 +103,13 @@ int do_mag_calibration(int mavlink_fd)
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
#ifndef __PX4_QURT
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
mscale_null.x_scale = 1.0f;
mscale_null.y_offset = 0.0f;
mscale_null.y_scale = 1.0f;
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
#endif
int result = OK;
@@ -595,7 +594,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
struct mag_scale mscale;
struct mag_calibration_s mscale;
// Set new scale
src/modules/sensors/sensors.cpp
+18 -18
View File
@@ -464,7 +464,7 @@ private:
* @param device: the device id of the sensor.
* @return: true if config is ok
*/
bool apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id);
bool apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id);
/**
* Apply a accel calibration.
@@ -474,7 +474,7 @@ private:
* @param device: the device id of the sensor.
* @return: true if config is ok
*/
bool apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id);
bool apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id);
/**
* Apply a mag calibration.
@@ -484,7 +484,7 @@ private:
* @param device: the device id of the sensor.
* @return: true if config is ok
*/
bool apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id);
bool apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id);
/**
* Check for changes in rc_parameter_map
@@ -1255,7 +1255,7 @@ Sensors::parameter_update_poll(bool forced)
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
struct gyro_scale gscale = {};
struct gyro_calibration_s gscale = {};
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
@@ -1323,7 +1323,7 @@ Sensors::parameter_update_poll(bool forced)
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
struct accel_scale ascale = {};
struct accel_calibration_s ascale = {};
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &ascale.x_offset));
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
@@ -1400,7 +1400,7 @@ Sensors::parameter_update_poll(bool forced)
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
struct mag_scale mscale = {};
struct mag_calibration_s mscale = {};
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
@@ -1515,12 +1515,12 @@ Sensors::parameter_update_poll(bool forced)
}
bool
Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id)
Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id)
{
#ifndef __PX4_QURT
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gscale);
const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
if (res) {
return false;
@@ -1535,10 +1535,10 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, c
static orb_advert_t gyro_calibration_pub = nullptr;
if (gyro_calibration_pub != nullptr) {
orb_publish(ORB_ID(gyro_calibration), gyro_calibration_pub, gscale);
orb_publish(ORB_ID(gyro_calibration), gyro_calibration_pub, gcal);
} else {
gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gscale);
gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gcal);
}
return true;
@@ -1546,12 +1546,12 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, c
}
bool
Sensors::apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id)
Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
{
#ifndef __PX4_QURT
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)ascale);
const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
if (res) {
return false;
@@ -1566,10 +1566,10 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale,
static orb_advert_t accel_calibration_pub = nullptr;
if (accel_calibration_pub != nullptr) {
orb_publish(ORB_ID(accel_calibration), accel_calibration_pub, ascale);
orb_publish(ORB_ID(accel_calibration), accel_calibration_pub, acal);
} else {
accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), ascale);
accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), acal);
}
return true;
@@ -1577,12 +1577,12 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale,
}
bool
Sensors::apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id)
Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
{
#ifndef __PX4_QURT
/* On most systems, we can just use the IOCTL call to set the calibration params. */
const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mscale);
const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
if (res) {
return false;
@@ -1597,10 +1597,10 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, con
static orb_advert_t mag_calibration_pub = nullptr;
if (mag_calibration_pub != nullptr) {
orb_publish(ORB_ID(mag_calibration), mag_calibration_pub, mscale);
orb_publish(ORB_ID(mag_calibration), mag_calibration_pub, mcal);
} else {
mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mscale);
mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mcal);
}
return true;