rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Update the cli `status` command to show all detected sensors (Except on 

CJMCU).

Further cleanup of sensor initialisation.
This commit is contained in:
Dominic Clifton 2015-02-19 21:04:06 +00:00
parent c45efac812
commit 1de72b11ce
10 changed files with 190 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/main/io/serial_cli.c
View File

@ -146,14 +146,23 @@ static const char * const featureNames[] = {
"BLACKBOX", NULL
};
#ifndef CJMCU
// sync this with sensors_e
static const char * const sensorNames[] = {
static const char * const sensorTypeNames[] = {
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second.
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL },
{ "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "HMC5883", "AK8975", NULL }
};
#endif
typedef struct {
const char *name;
@ -1357,27 +1366,38 @@ static void cliGet(char *cmdline)
static void cliStatus(char *cmdline)
{
uint8_t i;
uint32_t mask;
UNUSED(cmdline);
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
millis() / 1000, vbat, batteryCellCount);
mask = sensorsMask();
printf("CPU %dMHz, detected sensors: ", (SystemCoreClock / 1000000));
printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
#ifndef CJMCU
uint8_t i;
uint32_t mask;
uint32_t detectedSensorsMask = sensorsMask();
for (i = 0; ; i++) {
if (sensorNames[i] == NULL)
if (sensorTypeNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", sensorNames[i]);
}
if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]);
if (acc.revisionCode)
printf(".%c", acc.revisionCode);
mask = (1 << i);
if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) {
const char *sensorHardware;
uint8_t sensorHardwareIndex = detectedSensors[i];
sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
printf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.revisionCode) {
printf(".%c", acc.revisionCode);
}
}
}
#endif
cliPrint("\r\n");
#ifdef USE_I2C

1
src/main/sensors/acceleration.c
View File

@ -37,7 +37,6 @@
int16_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
accelerationSensor_e accHardware = ACC_DEFAULT; // which accel chip is used/detected
sensor_align_e accAlign = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.

1
src/main/sensors/acceleration.h
View File

@ -31,7 +31,6 @@ typedef enum {
ACC_NONE = 9
} accelerationSensor_e;
extern uint8_t accHardware;
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;

1
src/main/sensors/barometer.c
View File

@ -41,7 +41,6 @@ static int32_t baroGroundPressure = 0;
static uint32_t baroPressureSum = 0;
barometerConfig_t *barometerConfig;
baroSensor_e baroHardware;
void useBarometerConfig(barometerConfig_t *barometerConfigToUse)
{

2
src/main/sensors/barometer.h
View File

@ -24,8 +24,6 @@ typedef enum {
BARO_MS5611 = 3
} baroSensor_e;
extern baroSensor_e baroHardware;
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {

1
src/main/sensors/compass.c
View File

@ -40,7 +40,6 @@
#endif
mag_t mag; // mag access functions
magSensor_e magHardware = MAG_DEFAULT;
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.

7
src/main/sensors/compass.h
View File

@ -20,9 +20,9 @@
// Type of accelerometer used/detected
typedef enum {
MAG_DEFAULT = 0,
MAG_HMC5883 = 1,
MAG_AK8975 = 2,
MAG_NONE = 3
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
} magSensor_e;
#ifdef MAG
@ -32,6 +32,5 @@ void updateCompass(flightDynamicsTrims_t *magZero);
extern int16_t magADC[XYZ_AXIS_COUNT];
extern uint8_t magHardware;
extern sensor_align_e magAlign;
extern mag_t mag;

12
src/main/sensors/gyro.h
View File

@ -17,6 +17,18 @@
#pragma once
typedef enum {
GYRO_NONE = 0,
GYRO_DEFAULT,
GYRO_MPU6050,
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
GYRO_SPI_MPU6000,
GYRO_SPI_MPU6500,
GYRO_FAKE
} gyroSensor_e;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;

217
src/main/sensors/initialisation.c
View File

@ -71,6 +71,8 @@ extern gyro_t gyro;
extern baro_t baro;
extern acc_t acc;
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
const mpu6050Config_t *selectMPU6050Config(void)
{
#ifdef NAZE
@ -141,82 +143,121 @@ bool fakeAccDetect(acc_t *acc)
bool detectGyro(uint16_t gyroLpf)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
gyroAlign = GYRO_MPU6050_ALIGN;
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroAlign = GYRO_L3G4200D_ALIGN;
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
gyroAlign = GYRO_MPU3050_ALIGN;
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
gyroAlign = GYRO_L3GD20_ALIGN;
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
return true;
}
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
}
break;
}
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
}
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
return true;
}
if (fakeGyroDetect(&gyro, gyroLpf)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
return false;
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
static void detectAcc(uint8_t accHardwareToUse)
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
#ifdef USE_ACC_ADXL345
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
@ -224,17 +265,16 @@ retry:
accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
case ACC_DEFAULT:
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
default:
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
if (accHardwareToUse == ACC_FAKE)
break;
break;
}
#endif
case ACC_NONE: // disable ACC
break;
case ACC_DEFAULT: // autodetect
; // fallthrough
case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
acc_params.useFifo = false;
@ -248,8 +288,7 @@ retry:
accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
if (accHardwareToUse == ACC_ADXL345)
break;
break;
}
#endif
; // fallthrough
@ -260,8 +299,7 @@ retry:
accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
if (accHardwareToUse == ACC_LSM303DLHC)
break;
break;
}
#endif
; // fallthrough
@ -272,8 +310,7 @@ retry:
accAlign = ACC_MPU6050_ALIGN;
#endif
accHardware = ACC_MPU6050;
if (accHardwareToUse == ACC_MPU6050)
break;
break;
}
#endif
; // fallthrough
@ -289,8 +326,7 @@ retry:
accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
if (accHardwareToUse == ACC_MMA8452)
break;
break;
}
#endif
; // fallthrough
@ -301,8 +337,7 @@ retry:
accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
if (accHardwareToUse == ACC_BMA280)
break;
break;
}
#endif
; // fallthrough
@ -313,8 +348,7 @@ retry:
accAlign = ACC_SPI_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
if (accHardwareToUse == ACC_SPI_MPU6000)
break;
break;
}
#endif
; // fallthrough
@ -329,25 +363,30 @@ retry:
accAlign = ACC_SPI_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
if (accHardwareToUse == ACC_SPI_MPU6500)
break;
break;
}
#endif
; // fallthrough
case ACC_NONE: // disable ACC
accHardware = ACC_NONE;
break;
}
// Found anything? Check if error or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
}
if (accHardwareToUse != ACC_DEFAULT && accHardware == ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
}
if (accHardware != ACC_NONE) {
sensorsSet(SENSOR_ACC);
if (accHardware == ACC_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
}
static void detectBaro()
@ -355,7 +394,7 @@ static void detectBaro()
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroHardware = BARO_DEFAULT;
baroSensor_e baroHardware = BARO_DEFAULT;
#ifdef USE_BARO_BMP085
@ -399,19 +438,24 @@ static void detectBaro()
break;
}
#endif
baroHardware = BARO_NONE; // nothing detected or enabled.
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
if (baroHardware != BARO_NONE) {
sensorsSet(SENSOR_BARO);
if (baroHardware == BARO_NONE) {
return;
}
#endif
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
#endif
}
static void detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
#ifdef USE_MAG_HMC5883
static hmc5883Config_t *hmc5883Config = 0;
@ -448,21 +492,17 @@ retry:
magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_NONE: // disable MAG
break;
case MAG_DEFAULT: // autodetect
case MAG_DEFAULT:
; // fallthrough
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
sensorsSet(SENSOR_MAG);
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
if (magHardwareToUse == MAG_HMC5883)
break;
break;
}
#endif
; // fallthrough
@ -470,31 +510,32 @@ retry:
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975detect(&mag)) {
sensorsSet(SENSOR_MAG);
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
if (magHardwareToUse == MAG_AK8975)
break;
break;
}
#endif
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_DEFAULT) {
if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardwareToUse != MAG_DEFAULT && magHardware == MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
}
if (magHardware != MAG_NONE) {
sensorsSet(SENSOR_MAG);
if (magHardware == MAG_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
}
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
@ -513,13 +554,13 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
memset(&acc, sizeof(acc), 0);
memset(&gyro, sizeof(gyro), 0);
if (!detectGyro(gyroLpf)) {
return false;
}
sensorsSet(SENSOR_GYRO);
detectAcc(accHardwareToUse);
detectBaro();

10
src/main/sensors/sensors.h
View File

@ -17,6 +17,16 @@
#pragma once
typedef enum {
SENSOR_INDEX_GYRO = 0,
SENSOR_INDEX_ACC,
SENSOR_INDEX_BARO,
SENSOR_INDEX_MAG
} sensorIndex_e;
#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1)
extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT];
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;