zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Working towards full sensor flexibility

sbg
Lorenz Meier 13 years ago
parent
5198a9daf7
commit
1530aeccae
4 changed files with 242 additions and 125 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 13
      apps/drivers/mpu6000/mpu6000.cpp
  2. 2
      apps/sensors/Makefile
  3. 339
      apps/sensors/sensors.c
  4. 13
      nuttx/configs/px4fmu/src/up_nsh.c

13
apps/drivers/mpu6000/mpu6000.cpp
Unescape View File

@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : @@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
_accel_range_scale(1.0f),
_accel_range_scale(0.02f),
_accel_topic(-1),
_gyro_range_scale(1.0f),
_gyro_range_scale(0.02f),
_gyro_topic(-1),
_reads(0),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
@ -826,7 +826,7 @@ test() @@ -826,7 +826,7 @@ test()
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
reason = "can't open driver";
reason = "can't open driver, use <mpu6000 start> first";
break;
}
@ -841,9 +841,10 @@ test() @@ -841,9 +841,10 @@ test()
printf("single read\n");
fflush(stdout);
printf("time: %lld\n", report.timestamp);
printf("x: %f\n", report.x);
printf("y: %f\n", report.y);
printf("z: %f\n", report.z);
printf("x: %5.2f\n", (double)report.x);
printf("y: %5.2f\n", (double)report.y);
printf("z: %5.2f\n", (double)report.z);
printf("test: %8.4f\n", 1.5);
} while (0);

2
apps/sensors/Makefile
Unescape View File

@ -37,6 +37,6 @@ @@ -37,6 +37,6 @@
APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 2048
STACKSIZE = 3096
include $(APPDIR)/mk/app.mk

339
apps/sensors/sensors.c
Unescape View File

@ -57,6 +57,7 @@ @@ -57,6 +57,7 @@
#include <arch/board/drv_bma180.h>
#include <arch/board/drv_l3gd20.h>
#include <arch/board/drv_hmc5883l.h>
#include <drivers/drv_accel.h>
#include <arch/board/up_adc.h>
#include <systemlib/systemlib.h>
@ -105,11 +106,12 @@ static pthread_mutex_t sensors_read_ready_mutex; @@ -105,11 +106,12 @@ static pthread_mutex_t sensors_read_ready_mutex;
static int sensors_timer_loop_counter = 0;
/* File descriptors for all sensors */
static int fd_gyro = -1;
static int fd_accelerometer = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static int fd_gyro = -1;
static int fd_bma180 = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static int fd_accelerometer = -1;
/* Private functions declared static */
static void sensors_timer_loop(void *arg);
@ -169,30 +171,70 @@ static int sensors_init(void) @@ -169,30 +171,70 @@ static int sensors_init(void)
/* open gyro */
fd_gyro = open("/dev/l3gd20", O_RDONLY);
int errno_gyro = (int)*get_errno_ptr();
if (fd_gyro < 0) {
fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
if (!(fd_gyro < 0)) {
printf("[sensors] L3GD20 open ok\n");
}
/* open accelerometer */
fd_accelerometer = open("/dev/bma180", O_RDONLY);
/* open accelerometer, prefer the MPU-6000 */
fd_accelerometer = open("/dev/accel", O_RDONLY);
int errno_accelerometer = (int)*get_errno_ptr();
if (!(fd_accelerometer < 0)) {
printf("[sensors] Accelerometer open ok\n");
}
/* only attempt to use BMA180 if MPU-6000 is not available */
int errno_bma180 = 0;
if (fd_accelerometer < 0) {
fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
fd_bma180 = open("/dev/bma180", O_RDONLY);
errno_bma180 = (int)*get_errno_ptr();
if (!(fd_bma180 < 0)) {
printf("[sensors] Accelerometer (BMA180) open ok\n");
}
} else {
fd_bma180 = -1;
}
/* fail if no accelerometer is available */
if (fd_accelerometer < 0 && fd_bma180 < 0) {
/* print error message only if both failed, discard message else at all to not confuse users */
if (fd_accelerometer < 0) {
fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
fflush(stderr);
/* this sensor is redundant with BMA180 */
}
if (fd_bma180 < 0) {
fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180));
fflush(stderr);
/* this sensor is redundant with MPU-6000 */
}
errno = ENOSYS;
return ERROR;
}
} else {
printf("[sensors] BMA180 open ok\n");
/* fail if no gyro is available */
if (fd_accelerometer < 0 && fd_bma180 < 0) {
/* print error message only if both failed, discard message else at all to not confuse users */
if (fd_accelerometer < 0) {
fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer));
fflush(stderr);
/* this sensor is redundant with BMA180 */
}
if (fd_gyro < 0) {
fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro));
fflush(stderr);
/* this sensor is critical, exit on failed init */
}
errno = ENOSYS;
return ERROR;
}
/* open adc */
@ -209,16 +251,18 @@ static int sensors_init(void) @@ -209,16 +251,18 @@ static int sensors_init(void)
printf("[sensors] ADC open ok\n");
}
/* configure gyro */
if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] L3GD20 configuration ok\n");
/* configure gyro - if its not available and we got here the MPU-6000 is for sure available */
if (fd_gyro > 0) {
if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
fflush(stderr);
/* this sensor is critical, exit on failed init */
errno = ENOSYS;
return ERROR;
} else {
printf("[sensors] L3GD20 configuration ok\n");
}
}
/* XXX Add IOCTL configuration of remaining sensors */
@ -255,7 +299,7 @@ int sensors_main(int argc, char *argv[]) @@ -255,7 +299,7 @@ int sensors_main(int argc, char *argv[])
fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n");
/* Clean up */
close(fd_gyro);
close(fd_accelerometer);
close(fd_bma180);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);
@ -311,6 +355,7 @@ int sensors_main(int argc, char *argv[]) @@ -311,6 +355,7 @@ int sensors_main(int argc, char *argv[])
int16_t buf_gyro[3];
int16_t buf_accelerometer[3];
struct accel_report buf_accel_report;
int16_t buf_magnetometer[7];
float buf_barometer[3];
@ -354,8 +399,9 @@ int sensors_main(int argc, char *argv[]) @@ -354,8 +399,9 @@ int sensors_main(int argc, char *argv[])
/* Empty sensor buffers, avoid junk values */
/* Read first two values of each sensor into void */
(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
(void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro));
if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, 6);
if (fd_accelerometer > 0)(void)read(fd_accelerometer, buf_accelerometer, 12);
(void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer));
if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer));
@ -455,8 +501,12 @@ int sensors_main(int argc, char *argv[]) @@ -455,8 +501,12 @@ int sensors_main(int argc, char *argv[])
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
hil_enabled = true;
publishing = false;
int ret = close(sensor_pub);
printf("[sensors] Closing sensor pub: %i \n", ret);
int sens_ret = close(sensor_pub);
if (sens_ret == OK) {
printf("[sensors] Closing sensor pub OK\n");
} else {
printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret);
}
/* switching from HIL to non-HIL mode */
@ -508,79 +558,121 @@ int sensors_main(int argc, char *argv[]) @@ -508,79 +558,121 @@ int sensors_main(int argc, char *argv[])
paramcounter++;
/* try reading gyro */
uint64_t start_gyro = hrt_absolute_time();
ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
int gyrotime = hrt_absolute_time() - start_gyro;
if (fd_gyro > 0) {
/* try reading gyro */
uint64_t start_gyro = hrt_absolute_time();
ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro));
int gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime);
/* GYROSCOPE */
if (ret_gyro != sizeof(buf_gyro)) {
gyro_fail_count++;
/* GYROSCOPE */
if (ret_gyro != sizeof(buf_gyro)) {
gyro_fail_count++;
if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&gyro_present_enabled);
gyro_healthy = false;
gyro_success_count = 0;
}
if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&gyro_present_enabled);
gyro_healthy = false;
gyro_success_count = 0;
}
} else {
gyro_success_count++;
} else {
gyro_success_count++;
if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
gyro_healthy = true;
gyro_fail_count = 0;
if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&gyro_present_enabled_healthy);
gyro_healthy = true;
gyro_fail_count = 0;
}
gyro_updated = true;
}
gyro_updated = true;
}
gyrotime = hrt_absolute_time() - start_gyro;
gyrotime = hrt_absolute_time() - start_gyro;
if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
}
if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime);
/* read MPU-6000 */
if (fd_accelerometer > 0) {
/* try reading acc */
uint64_t start_acc = hrt_absolute_time();
ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report));
/* try reading acc */
uint64_t start_acc = hrt_absolute_time();
ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer));
/* ACCELEROMETER */
if (ret_accelerometer != sizeof(struct accel_report)) {
acc_fail_count++;
/* ACCELEROMETER */
if (ret_accelerometer != sizeof(buf_accelerometer)) {
acc_fail_count++;
if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) {
fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&acc_present_enabled);
gyro_healthy = false;
acc_success_count = 0;
}
if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&acc_present_enabled);
gyro_healthy = false;
acc_success_count = 0;
}
} else {
acc_success_count++;
} else {
acc_success_count++;
if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&acc_present_enabled_healthy);
acc_healthy = true;
acc_fail_count = 0;
// global_data_send_subsystem_info(&acc_present_enabled_healthy);
acc_healthy = true;
acc_fail_count = 0;
}
acc_updated = true;
}
acc_updated = true;
int acctime = hrt_absolute_time() - start_acc;
if (acctime > 500) printf("ACC: %d us\n", acctime);
}
int acctime = hrt_absolute_time() - start_acc;
/* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */
if (fd_bma180 > 0) {
/* try reading acc */
uint64_t start_acc = hrt_absolute_time();
ret_accelerometer = read(fd_bma180, buf_accelerometer, 6);
/* ACCELEROMETER */
if (ret_accelerometer != 6) {
acc_fail_count++;
if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) {
fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) {
// global_data_send_subsystem_info(&acc_present_enabled);
gyro_healthy = false;
acc_success_count = 0;
}
} else {
acc_success_count++;
if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) {
// global_data_send_subsystem_info(&acc_present_enabled_healthy);
acc_healthy = true;
acc_fail_count = 0;
}
if (acctime > 500) printf("ACC: %d us\n", acctime);
acc_updated = true;
}
int acctime = hrt_absolute_time() - start_acc;
if (acctime > 500) printf("ACC: %d us\n", acctime);
}
/* MAGNETOMETER */
if (magcounter == 4) { /* 120 Hz */
@ -607,7 +699,7 @@ int sensors_main(int argc, char *argv[]) @@ -607,7 +699,7 @@ int sensors_main(int argc, char *argv[])
if (ret_magnetometer != sizeof(buf_magnetometer)) {
mag_fail_count++;
if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) {
if ((mag_fail_count % 20) == 0 || (mag_fail_count > 20 && mag_fail_count < 100)) {
fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
@ -650,7 +742,7 @@ int sensors_main(int argc, char *argv[]) @@ -650,7 +742,7 @@ int sensors_main(int argc, char *argv[])
if (ret_barometer != sizeof(buf_barometer)) {
baro_fail_count++;
if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
@ -687,10 +779,10 @@ int sensors_main(int argc, char *argv[]) @@ -687,10 +779,10 @@ int sensors_main(int argc, char *argv[])
ret_adc = read(fd_adc, &buf_adc, adc_readsize);
nsamples_adc = ret_adc / sizeof(struct adc_msg_s);
if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) {
if (ret_adc < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) {
adc_fail_count++;
if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) {
fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr()));
}
@ -730,7 +822,7 @@ int sensors_main(int argc, char *argv[]) @@ -730,7 +822,7 @@ int sensors_main(int argc, char *argv[])
*/
if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) {
/* Read out values from HRT */
for (int i = 0; i < ppm_decoded_channels; i++) {
for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
rc.chan[i].raw = ppm_buffer[i];
/* Set the range to +-, then scale up */
rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000;
@ -759,9 +851,9 @@ int sensors_main(int argc, char *argv[]) @@ -759,9 +851,9 @@ int sensors_main(int argc, char *argv[])
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
/* throttle input */
manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled;
manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* mode switch input */
manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled;
@ -800,19 +892,56 @@ int sensors_main(int argc, char *argv[]) @@ -800,19 +892,56 @@ int sensors_main(int argc, char *argv[])
if (acc_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_raw_counter++;
if (fd_accelerometer > 0) {
/* MPU-6000 values */
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = buf_accel_report.x;
raw.accelerometer_m_s2[1] = buf_accel_report.y;
raw.accelerometer_m_s2[2] = buf_accel_report.z;
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = buf_accel_report.x*1000; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = buf_accel_report.y*1000; // y on the board is x of the sensor
raw.accelerometer_raw[2] = buf_accel_report.z*1000; // z of the board is z of the sensor
raw.accelerometer_raw_counter++;
} else if (fd_bma180 > 0) {
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_raw_counter++;
}
/* L3GD20 is not available, use MPU-6000 */
if (fd_accelerometer > 0 && fd_gyro < 0) {
raw.gyro_raw[0] = ((buf_accelerometer[3] == -32768) ? -32767 : buf_accelerometer[3]); // x of the board is y of the sensor
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.gyro_raw[1] = ((buf_accelerometer[4] == -32768) ? 32767 : -buf_accelerometer[4]); // y on the board is -x of the sensor
raw.gyro_raw[2] = ((buf_accelerometer[5] == -32768) ? -32767 : buf_accelerometer[5]); // z of the board is -z of the sensor
/* scale measurements */
// XXX request scaling from driver instead of hardcoding it
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
raw.gyro_raw_counter++;
/* mark as updated */
gyro_updated = true;
}
}
/* MAGNETOMETER */
@ -912,7 +1041,7 @@ int sensors_main(int argc, char *argv[]) @@ -912,7 +1041,7 @@ int sensors_main(int argc, char *argv[])
printf("[sensors] sensor readout stopped\n");
close(fd_gyro);
close(fd_accelerometer);
close(fd_bma180);
close(fd_magnetometer);
close(fd_barometer);
close(fd_adc);

13
nuttx/configs/px4fmu/src/up_nsh.c
Unescape View File

@ -236,19 +236,6 @@ int nsh_archinitialize(void) @@ -236,19 +236,6 @@ int nsh_archinitialize(void)
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
int mpu_attempts = 0;
int mpu_fail = 0;
while (mpu_attempts < 1)
{
mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU);
mpu_attempts++;
if (mpu_fail == 0) break;
up_udelay(200);
}
if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);

Write Preview
Loading…
Cancel
Save