static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
DevHandle h;
DevMgr::getHandle(s, h);
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
return false;
}
device_id = -1000;
// TODO: There is no baro calibration yet, since no external baros exist
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
//out:
DevMgr::releaseHandle(h);
return success;
}
int start()
{
PX4_ERR("start");
g_dev = new DfISL29501Wrapper();
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfISL29501Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfISL29501Wrapper start failed");
return ret;
}
// Open the range sensor
DevHandle h;
DevMgr::getHandle(ISL_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
ISL_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int start(enum Rotation rotation)
{
g_dev = new DfHmc9250Wrapper(rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfHmc9250Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfHmc9250Wrapper start failed");
return ret;
}
// Open the MAG sensor
DevHandle h;
DevMgr::getHandle(MAG_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
MAG_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
/**
* Start the driver.
*/
void
start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
DevHandle h;
/* create the driver */
g_dev = new GPSSIM(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
if (!h.isValid()) {
PX4_ERR("getHandle failed: %s", GPSSIM_DEVICE_PATH);
goto fail;
}
return;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("start failed");
}
int ImuTester::run()
{
// Default is fail unless pass critera met
m_pass = TEST_FAIL;
// Register the driver
int ret = m_sensor.init();
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_PATH, h.getError());
m_done = true;
} else {
m_done = false;
}
while (!m_done) {
++m_read_attempts;
struct imu_sensor_data data;
ret = ImuSensor::getSensorData(h, data, true);
if (ret == 0) {
uint32_t count = data.read_counter;
DF_LOG_INFO("count: %d", count);
if (m_read_counter != count) {
m_read_counter = count;
ImuSensor::printImuValues(h, data);
}
} else {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
}
if (m_read_counter >= num_read_attempts) {
// Done test - PASSED
m_pass = TEST_PASS;
m_done = true;
} else if (m_read_attempts > num_read_attempts) {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
m_done = true;
}
}
DevMgr::releaseHandle(h);
DF_LOG_INFO("Closing IMU sensor");
m_sensor.stop();
return m_pass;
}
int PressureTester::run()
{
DF_LOG_INFO("Entering: run");
// Default is fail unless pass critera met
m_pass = TEST_FAIL;
// Register the driver
int ret = m_sensor.init();
// Open the pressure sensor
DevHandle h;
DevMgr::getHandle(BARO_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
BARO_DEVICE_PATH, h.getError());
m_done = true;
} else {
m_done = false;
m_sensor_data.read_counter = 0;
}
while (!m_done) {
++m_read_attempts;
ret = BaroSensor::getSensorData(h, m_sensor_data, true);
DF_LOG_INFO("Got data");
if (ret == 0) {
uint32_t count = m_sensor_data.read_counter;
if (m_read_counter != count) {
DF_LOG_INFO("count: %d", count);
m_read_counter = count;
BaroSensor::printPressureValues(m_sensor_data);
}
} else {
DF_LOG_INFO("error: unable to read the pressure sensor device.");
}
if ((m_read_counter >= 1000) && (m_read_attempts == m_read_counter)) {
// Done test - PASSED
m_pass = TEST_PASS;
m_done = true;
} else if (m_read_attempts > 1000) {
DF_LOG_INFO("error: unable to read the pressure sensor device.");
m_done = true;
}
}
DF_LOG_INFO("Closing pressure sensor\n");
m_sensor.stop();
return m_pass;
}
int
Sensors::adc_init()
{
DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc);
if (!_h_adc.isValid()) {
PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError());
return PX4_ERROR;
}
return OK;
}
/**
* Reset the driver.
*/
void
reset()
{
DevHandle h;
DevMgr::getHandle(GPSSIM_DEVICE_PATH, h);
if (!h.isValid()) {
PX4_ERR("failed ");
}
if (h.ioctl(SENSORIOCRESET, 0) < 0) {
PX4_ERR("reset failed");
}
}
bool
VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id)
{
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX)
if (!h.isValid()) {
return false;
}
/* On most systems, we can just use the IOCTL call to set the calibration params. */
return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
#else
/* On QURT & POSIX, the params are read directly in the respective wrappers. */
return true;
#endif
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
DevHandle h;
DevMgr::getHandle(s, h);
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
}
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
ret = h.ioctl(GYROIOCSELFTEST, 0);
if (ret != OK) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
out:
DevMgr::releaseHandle(h);
return success;
}
int led_init()
{
blink_msg_end = 0;
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_OFF;
led_control.priority = 0;
led_control.timestamp = hrt_absolute_time();
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
#if !defined(CONFIG_ARCH_BOARD_RPI) && !defined(CONFIG_ARCH_BOARD_BBBLUE)
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
if (!h_leds.isValid()) {
PX4_WARN("LED: getHandle fail\n");
return PX4_ERROR;
}
/* the green LED is only available on FMUv5 */
(void)h_leds.ioctl(LED_ON, LED_GREEN);
/* the blue LED is only available on AeroCore but not FMUv2 */
(void)h_leds.ioctl(LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (h_leds.ioctl(LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail\n");
return PX4_ERROR;
}
/* switch amber off */
led_off(LED_AMBER);
#endif
return 0;
}
int start(bool mag_enabled, enum Rotation rotation)
{
g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfLsm9ds1Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfLsm9ds1Wrapper start failed");
return ret;
}
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_ACC_GYRO, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
DevMgr::getHandle(IMU_DEVICE_MAG, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_MAG, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int led_init()
{
blink_msg_end = 0;
#ifndef CONFIG_ARCH_BOARD_RPI
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
if (!h_leds.isValid()) {
PX4_WARN("LED: getHandle fail\n");
return PX4_ERROR;
}
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)h_leds.ioctl(LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (h_leds.ioctl(LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail\n");
return PX4_ERROR;
}
/* switch amber off */
led_off(LED_AMBER);
#endif
/* then try RGB LEDs, this can fail on FMUv1*/
DevHandle h;
DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds);
if (!h_rgbleds.isValid()) {
PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH);
}
return 0;
}
bool
VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id)
{
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
/* On most systems, we can just use the IOCTL call to set the calibration params. */
return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
#else
/* On QURT, the params are read directly in the respective wrappers. */
return true;
#endif
}
int SPIDevObj::bulkRead(DevHandle &h, uint8_t address, uint8_t *out_buffer, int length)
{
#if defined(__RPI2)
/* implement sensor interface via rpi2 spi */
SPIDevObj *obj = DevMgr::getDevObjByHandle<SPIDevObj>(h);
if (obj) {
return obj->_bulkRead(address, out_buffer, length);
}
return -1;
#elif defined(__QURT)
int result = 0;
int transfer_bytes = 1 + length; // first byte is address
struct dspal_spi_ioctl_read_write ioctl_write_read;
uint8_t write_buffer[transfer_bytes];
uint8_t read_buffer[transfer_bytes];
write_buffer[0] = address | DIR_READ;
ioctl_write_read.read_buffer = out_buffer;
ioctl_write_read.read_buffer_length = transfer_bytes;
ioctl_write_read.write_buffer = write_buffer;
ioctl_write_read.write_buffer_length = transfer_bytes;
result = h.ioctl(SPI_IOCTL_RDWR, (unsigned long)&ioctl_write_read);
if (result != transfer_bytes) {
DF_LOG_ERR("bulkRead error %d (%d)", result, h.getError());
return result;
}
memcpy(out_buffer, &read_buffer[1], transfer_bytes - 1);
return 0;
#else
return -1;
#endif
}
static int check_calibration(DevHandle &h, const char* param_template, int &devid)
{
bool calibration_found;
/* new style: ask device for calibration state */
int ret = h.ioctl(SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = h.ioctl(DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
/* old style transition: check param values */
while (!calibration_found) {
sprintf(s, param_template, instance);
param_t parm = param_find(s);
/* if the calibration param is not present, abort */
if (parm == PARAM_INVALID) {
break;
}
/* if param get succeeds */
int calibration_devid;
if (!param_get(parm, &(calibration_devid))) {
/* if the devid matches, exit early */
if (devid == calibration_devid) {
calibration_found = true;
break;
}
}
instance++;
}
return !calibration_found;
}
int SPIDevObj::setLoopbackMode(DevHandle &h, bool enable)
{
#if defined(__RPI2)
/* implement sensor interface via rpi2 spi */
DF_LOG_ERR("ERROR: attempt to set loopback mode in software fails.");
return -1;
#elif defined(__QURT)
struct dspal_spi_ioctl_loopback loopback;
loopback.state = enable ? SPI_LOOPBACK_STATE_ENABLED : SPI_LOOPBACK_STATE_DISABLED;
return h.ioctl(SPI_IOCTL_LOOPBACK_TEST, (unsigned long)&loopback);
#else
return -1;
#endif
}
int buzzer_init()
{
tune_end = 0;
tune_current = 0;
memset(tune_durations, 0, sizeof(tune_durations));
tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h_buzzer);
if (!h_buzzer.isValid()) {
PX4_WARN("Buzzer: px4_open fail\n");
return PX4_ERROR;
}
return PX4_OK;
}
int SPIDevObj::setBusFrequency(DevHandle &h, SPI_FREQUENCY freq_hz)
{
#if defined(__RPI2)
/* implement sensor interface via rpi2 spi */
SPIDevObj *obj = DevMgr::getDevObjByHandle<SPIDevObj>(h);
if (obj) {
return obj->_setBusFrequency(freq_hz);
}
return -1;
#elif defined(__QURT)
struct dspal_spi_ioctl_set_bus_frequency bus_freq;
bus_freq.bus_frequency_in_hz = freq_hz;
return h.ioctl(SPI_IOCTL_SET_BUS_FREQUENCY_IN_HZ, (unsigned long)&bus_freq);
#else
return -1;
#endif
}
void set_tune(int tune)
{
unsigned int new_tune_duration = tune_durations[tune];
/* don't interrupt currently playing non-repeating tune by repeating */
if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
/* allow interrupting current non-repeating tune by the same tune */
if (tune != tune_current || new_tune_duration != 0) {
h_buzzer.ioctl(TONE_SET_ALARM, tune);
}
tune_current = tune;
if (new_tune_duration != 0) {
tune_end = hrt_absolute_time() + new_tune_duration;
} else {
tune_end = 0;
}
}
}
void VotedSensorsUpdate::parameters_update()
{
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
/* fine tune board offset */
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation;
/* set offset parameters to new values */
bool failed;
char str[30];
unsigned mag_count = 0;
unsigned gyro_count = 0;
unsigned accel_count = 0;
/* run through all gyro sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_GYRO%u_ID", i);
int device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
if (failed) {
DevMgr::releaseHandle(h);
continue;
}
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
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);
failed = failed || (OK != param_get(param_find(str), &gscale.y_offset));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &gscale.z_offset));
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_scale));
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.y_scale));
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_gyro_calibration(h, &gscale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
}
}
break;
}
}
if (config_ok) {
gyro_count++;
}
}
/* run through all accel sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_ACC%u_ID", i);
int device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
if (failed) {
DevMgr::releaseHandle(h);
continue;
}
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
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);
failed = failed || (OK != param_get(param_find(str), &ascale.y_offset));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &ascale.z_offset));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.x_scale));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.y_scale));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.z_scale));
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_accel_calibration(h, &ascale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
}
}
break;
}
}
if (config_ok) {
accel_count++;
}
}
/* run through all mag sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
/* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced
* in the section below.
*/
_mag_rotation[s] = _board_rotation;
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
/* the driver is not running, abort */
continue;
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_MAG%u_ID", i);
int device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
(void)param_find(str);
if (failed) {
DevMgr::releaseHandle(h);
continue;
}
// int id = h.ioctl(DEVIOCGDEVICEID, 0);
// PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id);
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
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);
failed = failed || (OK != param_get(param_find(str), &mscale.y_offset));
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &mscale.z_offset));
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.x_scale));
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.y_scale));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.z_scale));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
/* mag is internal */
_mag_rotation[s] = _board_rotation;
/* reset param to -1 to indicate internal mag */
int32_t minus_one;
param_get(param_find(str), &minus_one);
if (minus_one != MAG_ROT_VAL_INTERNAL) {
minus_one = MAG_ROT_VAL_INTERNAL;
param_set_no_notification(param_find(str), &minus_one);
}
} else {
int32_t mag_rot;
param_get(param_find(str), &mag_rot);
/* check if this mag is still set as internal */
if (mag_rot < 0) {
/* it was marked as internal, change to external with no rotation */
mag_rot = 0;
param_set_no_notification(param_find(str), &mag_rot);
}
/* handling of old setups, will be removed later (noted Feb 2015) */
int32_t deprecated_mag_rot = 0;
param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
/*
* If the deprecated parameter is non-default (is != 0),
* and the new parameter is default (is == 0), then this board
* was configured already and we need to copy the old value
* to the new parameter.
* The < 0 case is special: It means that this param slot was
* used previously by an internal sensor, but the the call above
* proved that it is currently occupied by an external sensor.
* In that case we consider the orientation to be default as well.
*/
if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) {
mag_rot = deprecated_mag_rot;
param_set_no_notification(param_find(str), &mag_rot);
/* clear the old param, not supported in GUI anyway */
deprecated_mag_rot = 0;
param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot);
}
/* handling of transition from internal to external */
if (mag_rot < 0) {
mag_rot = 0;
}
get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]);
}
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_mag_calibration(h, &mscale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
}
}
break;
}
}
if (config_ok) {
mag_count++;
}
}
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
}
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub)
{
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
case vehicle_status_s::HIL_STATE_ON:
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
#ifdef __PX4_NUTTX
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
/* skip serial ports */
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
}
snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
int sensfd = ::open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_log_pub, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
#else
// Handle VDev devices
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
break;
/* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
// Handle DF devices
const char *df_dev_path;
unsigned int index = 0;
for(;;) {
if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) {
break;
}
DevHandle h;
DevMgr::getHandle(df_dev_path, h);
if (!h.isValid()) {
warn("failed opening device %s", df_dev_path);
continue;
}
int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1);
DevMgr::releaseHandle(h);
printf("Disabling %s: %s\n", df_dev_path, (block_ret == OK) ? "OK" : "ERROR");
}
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
#endif
} else {
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown HIL state");
break;
}
}
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
// XXX also set lockdown here
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
void VotedSensorsUpdate::parameters_update()
{
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
/* fine tune board offset */
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation;
// initialze all mag rotations with the board rotation in case there is no calibration data available
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
_mag_rotation[topic_instance] = _board_rotation;
}
/* Load & apply the sensor calibrations.
* IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
*/
/* temperature compensation */
_temperature_compensation.parameters_update();
/* gyro */
for (unsigned topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) {
if (topic_instance < _gyro.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct gyro_report report;
if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance);
if (temp < 0) {
PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
}
}
}
}
/* accel */
for (unsigned topic_instance = 0; topic_instance < ACCEL_COUNT_MAX; ++topic_instance) {
if (topic_instance < _accel.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct accel_report report;
if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance);
if (temp < 0) {
PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
}
}
}
}
/* baro */
for (unsigned topic_instance = 0; topic_instance < BARO_COUNT_MAX; ++topic_instance) {
if (topic_instance < _baro.subscription_count) {
// valid subscription, so get the driver id by getting the published sensor data
struct baro_report report;
if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance);
if (temp < 0) {
PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i",
report.device_id, topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
}
}
}
}
/* set offset parameters to new values */
bool failed;
char str[30];
unsigned gyro_count = 0;
unsigned accel_count = 0;
unsigned gyro_cal_found_count = 0;
unsigned accel_cal_found_count = 0;
/* run through all gyro sensors */
for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
bool config_ok = false;
/* run through all stored calibrations that are applied at the driver level*/
for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_GYRO%u_ID", i);
int32_t device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_GYRO%u_EN", i);
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_gyro.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
if (driver_index == 0 && device_id > 0) {
gyro_cal_found_count++;
}
/* if the calibration is for this device, apply it */
if (device_id == driver_device_id) {
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);
failed = failed || (OK != param_get(param_find(str), &gscale.y_offset));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &gscale.z_offset));
(void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_scale));
(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.y_scale));
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_gyro_calibration(h, &gscale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
}
}
break;
}
}
if (config_ok) {
gyro_count++;
}
}
// There are less gyros than calibrations
// reset the board calibration and fail the initial load
if (gyro_count < gyro_cal_found_count) {
// run through all stored calibrations and reset them
for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
int32_t device_id = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", i);
(void)param_set(param_find(str), &device_id);
}
}
/* run through all accel sensors */
for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_ACC%u_ID", i);
int32_t device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_ACC%u_EN", i);
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_accel.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
if (driver_index == 0 && device_id > 0) {
accel_cal_found_count++;
}
/* if the calibration is for this device, apply it */
if (device_id == driver_device_id) {
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);
failed = failed || (OK != param_get(param_find(str), &ascale.y_offset));
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &ascale.z_offset));
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.x_scale));
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.y_scale));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &ascale.z_scale));
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_accel_calibration(h, &ascale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
}
}
break;
}
}
if (config_ok) {
accel_count++;
}
}
// There are less accels than calibrations
// reset the board calibration and fail the initial load
if (accel_count < accel_cal_found_count) {
// run through all stored calibrations and reset them
for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
int32_t device_id = 0;
(void)sprintf(str, "CAL_ACC%u_ID", i);
(void)param_set(param_find(str), &device_id);
}
}
/* run through all mag sensors
* Because we store the device id in _mag_device_id, we need to get the id via uorb topic since
* the DevHandle method does not work on POSIX.
*/
for (unsigned topic_instance = 0; topic_instance < MAG_COUNT_MAX && topic_instance < _mag.subscription_count;
++topic_instance) {
struct mag_report report;
if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) {
continue;
}
int topic_device_id = report.device_id;
bool is_external = report.is_external;
_mag_device_id[topic_instance] = topic_device_id;
// find the driver handle that matches the topic_device_id
DevHandle h;
for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) {
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
DevMgr::getHandle(str, h);
if (!h.isValid()) {
/* the driver is not running, continue with the next */
continue;
}
int driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
if (driver_device_id == topic_device_id) {
break; // we found the matching driver
} else {
DevMgr::releaseHandle(h);
}
}
bool config_ok = false;
/* run through all stored calibrations */
for (unsigned i = 0; i < MAG_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_MAG%u_EN", i);
int32_t device_enabled = 1;
failed = failed || (OK != param_get(param_find(str), &device_enabled));
_mag.enabled[i] = (device_enabled == 1);
if (failed) {
continue;
}
/* if the calibration is for this device, apply it */
if (device_id == _mag_device_id[topic_instance]) {
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);
failed = failed || (OK != param_get(param_find(str), &mscale.y_offset));
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
failed = failed || (OK != param_get(param_find(str), &mscale.z_offset));
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.x_scale));
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.y_scale));
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
failed = failed || (OK != param_get(param_find(str), &mscale.z_scale));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
int32_t mag_rot;
param_get(param_find(str), &mag_rot);
if (is_external) {
/* check if this mag is still set as internal, otherwise leave untouched */
if (mag_rot < 0) {
/* it was marked as internal, change to external with no rotation */
mag_rot = 0;
param_set_no_notification(param_find(str), &mag_rot);
}
} else {
/* mag is internal - reset param to -1 to indicate internal mag */
if (mag_rot != MAG_ROT_VAL_INTERNAL) {
mag_rot = MAG_ROT_VAL_INTERNAL;
param_set_no_notification(param_find(str), &mag_rot);
}
}
/* now get the mag rotation */
if (mag_rot >= 0) {
// Set external magnetometers to use the parameter value
get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[topic_instance]);
} else {
// Set internal magnetometers to use the board rotation
_mag_rotation[topic_instance] = _board_rotation;
}
if (failed) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
config_ok = apply_mag_calibration(h, &mscale, device_id);
if (!config_ok) {
PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i);
}
}
break;
}
}
}
}
int led_toggle(int led)
{
return h_leds.ioctl(LED_TOGGLE, led);
}
/**
* Start the driver.
*
* This function call only returns once the driver is
* up and running or failed to detect the sensor.
*/
int
start(enum Rotation rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
return 0;
}
DevHandle h;
DevHandle h_mag;
/* create the driver */
g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating ACCELSIM obj");
goto fail;
}
if (OK != g_dev->init()) {
PX4_ERR("failed init of ACCELSIM obj");
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h);
if (!h.isValid()) {
PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
goto fail;
}
if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
DevMgr::releaseHandle(h);
goto fail;
}
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag);
/* don't fail if mag dev cannot be opened */
if (h_mag.isValid()) {
if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG);
}
} else {
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG);
}
DevMgr::releaseHandle(h);
DevMgr::releaseHandle(h_mag);
return 0;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("driver start failed");
return 1;
}
int led_on(int led)
{
return h_leds.ioctl(LED_ON, led);
}
int led_off(int led)
{
return h_leds.ioctl(LED_OFF, led);
}
void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
/* only read if not in HIL mode */
if (_hil_enabled) {
return;
}
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (t - _last_adc >= 10000) {
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
struct adc_msg_s buf_adc[12];
/* read all channels available */
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
float bat_voltage_v = 0.0f;
float bat_current_a = 0.0f;
bool updated_battery = false;
if (ret >= (int)sizeof(buf_adc[0])) {
/* Read add channels we got */
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
/* look for specific channels and process the raw voltage to measurement data */
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* Voltage in volts */
bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div;
if (bat_voltage_v > 0.5f) {
updated_battery = true;
}
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
bat_current_a = (((float)(buf_adc[i].am_data) * _parameters.battery_current_scaling)
- _parameters.battery_current_offset) * _parameters.battery_a_per_v;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
/**
* The voltage divider pulls the signal down, only act on
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
(diff_pres_pa_raw * 0.1f);
_diff_pres.temperature = -1000.0f;
int instance;
orb_publish_auto(ORB_ID(differential_pressure), &_diff_pres_pub, &_diff_pres, &instance,
ORB_PRIO_DEFAULT);
}
#endif
}
}
if (_parameters.battery_source == 0 && updated_battery) {
actuator_controls_s ctrl;
orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE],
_armed, &_battery_status);
int instance;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &instance, ORB_PRIO_DEFAULT);
}
_last_adc = t;
}
}
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
DevHandle h;
DevMgr::getHandle(s, h);
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
return false;
}
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
ret = h.ioctl(ACCELIOCSELFTEST, 0);
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
}
#ifdef __PX4_NUTTX
if (dynamic) {
/* check measurement result range */
struct accel_report acc;
ret = h.read(&acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
goto out;
}
}
#endif
out:
DevMgr::releaseHandle(h);
return success;
}
