dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

drivers: re-use calibration topic 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/drivers/bma180/bma180.cpp
View File

@ -149,7 +149,7 @@ private:
RingBuffer *_reports;
struct accel_scale _accel_scale;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
@ -496,12 +496,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:

13
src/drivers/drv_accel.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 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
@ -42,6 +42,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include <uORB/topics/accel_calibration.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@ -54,16 +55,6 @@
#include <uORB/topics/sensor_accel.h>
#define accel_report sensor_accel_s
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
struct accel_scale {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
* ioctl() definitions
*

12
src/drivers/drv_gyro.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 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
@ -42,6 +42,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include <uORB/topics/gyro_calibration.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@ -54,15 +55,6 @@
#include <uORB/topics/sensor_gyro.h>
#define gyro_report sensor_gyro_s
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
struct gyro_scale {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
* ioctl() definitions

12
src/drivers/drv_mag.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 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
@ -40,6 +40,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include <uORB/topics/mag_calibration.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@ -52,15 +53,6 @@
#include <uORB/topics/sensor_mag.h>
#define mag_report sensor_mag_s
/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
struct mag_scale {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
};
/*
* ioctl() definitions

30
src/drivers/hmc5883/hmc5883.cpp
View File

@ -1075,23 +1075,21 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
// XXX do something smarter here
int fd = (int)enable;
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_calibration_s mscale_previous;
mscale_previous.x_offset = 0.0f,
mscale_previous.x_scale = 1.0f,
mscale_previous.y_offset = 0.0f,
mscale_previous.y_scale = 1.0f,
mscale_previous.z_offset = 0.0f,
mscale_previous.z_scale = 1.0f,
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,
float sum_excited[3] = {0.0f, 0.0f, 0.0f};

6
src/drivers/l3gd20/l3gd20.cpp
View File

@ -235,7 +235,7 @@ private:
ringbuffer::RingBuffer *_reports;
struct gyro_scale _gyro_scale;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
@ -704,12 +704,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:

12
src/drivers/lsm303d/lsm303d.cpp
View File

@ -279,13 +279,13 @@ private:
ringbuffer::RingBuffer *_accel_reports;
ringbuffer::RingBuffer *_mag_reports;
struct accel_scale _accel_scale;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
struct mag_scale _mag_scale;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
@ -962,7 +962,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
@ -984,7 +984,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
@ -1095,12 +1095,12 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
case MAGIOCSSCALE:
/* copy scale in */
memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale));
return OK;
case MAGIOCGSCALE:
/* copy scale out */
memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale));
return OK;
case MAGIOCSRANGE:

12
src/drivers/mpu6000/mpu6000.cpp
View File

@ -267,7 +267,7 @@ private:
ringbuffer::RingBuffer *_accel_reports;
struct accel_scale _accel_scale;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
@ -276,7 +276,7 @@ private:
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
@ -1400,7 +1400,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
@ -1414,7 +1414,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
@ -1484,12 +1484,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:

252
src/drivers/mpu9250/mpu9250.cpp
View File

@ -194,6 +194,250 @@
*/
#define MPU9250_TIMER_REDUCTION 200
class MPU9250_gyro;
class MPU9250 : public device::SPI
{
public:
MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
virtual ~MPU9250();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
friend class MPU9250_gyro;
virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen);
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
MPU9250_gyro *_gyro;
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
unsigned _dlpf_freq;
unsigned _sample_rate;
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
perf_counter_t _controller_latency_perf;
uint8_t _register_wait;
uint64_t _reset_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _accel_int;
Integrator _gyro_int;
enum Rotation _rotation;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define MPU9250_NUM_CHECKED_REGISTERS 11
static const uint8_t _checked_registers[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// last temperature reading for print_info()
float _last_temperature;
// keep last accel reading for duplicate detection
uint16_t _last_accel[3];
bool _got_duplicate;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Read a register from the MPU9250
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the MPU9250
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the MPU9250
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the MPU9250, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the MPU9250 measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Swap a 16-bit value read from the MPU9250 to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
MPU9250(const MPU9250 &);
MPU9250 operator=(const MPU9250 &);
#pragma pack(push, 1)
/**
* Report conversation within the MPU9250, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
};
#pragma pack(pop)
};
/*
list of registers that will be checked in check_registers(). Note
@ -910,7 +1154,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
@ -924,7 +1168,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
@ -1006,12 +1250,12 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:

15
src/modules/commander/accelerometer_calibration.cpp
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;

19
src/modules/commander/gyro_calibration.cpp
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);

17
src/modules/commander/mag_calibration.cpp
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

36
src/modules/sensors/sensors.cpp
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;

12
src/platforms/posix/drivers/accelsim/accelsim.cpp
View File

@ -114,13 +114,13 @@ private:
ringbuffer::RingBuffer *_accel_reports;
ringbuffer::RingBuffer *_mag_reports;
struct accel_scale _accel_scale;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
struct mag_scale _mag_scale;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
@ -584,7 +584,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg)
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
@ -606,7 +606,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg)
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &(_accel_scale), sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &(_accel_scale), sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
@ -712,12 +712,12 @@ ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg)
case MAGIOCSSCALE:
/* copy scale in */
memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale));
return OK;
case MAGIOCGSCALE:
/* copy scale out */
memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale));
return OK;
case MAGIOCSRANGE:

12
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -154,7 +154,7 @@ private:
ringbuffer::RingBuffer *_accel_reports;
struct accel_scale _accel_scale;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
@ -163,7 +163,7 @@ private:
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
@ -839,7 +839,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
@ -853,7 +853,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
@ -910,12 +910,12 @@ GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg)
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:

6
src/systemcmds/config/config.c
View File

@ -180,7 +180,7 @@ do_gyro(int argc, char *argv[])
if (ret) {
warnx("gyro self test FAILED! Check calibration:");
struct gyro_scale scale;
struct gyro_calibration_s scale;
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
if (ret) {
@ -262,7 +262,7 @@ do_mag(int argc, char *argv[])
if (ret) {
warnx("mag self test FAILED! Check calibration:");
struct mag_scale scale;
struct mag_calibration_s scale;
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
if (ret) {
@ -344,7 +344,7 @@ do_accel(int argc, char *argv[])
if (ret) {
warnx("accel self test FAILED! Check calibration:");
struct accel_scale scale;
struct accel_calibration_s scale;
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
if (ret) {