From 89f2f4d8ac0d490f1f9843c3f82c8edcd4586f83 Mon Sep 17 00:00:00 2001 From: Gary Ellingson Date: Mon, 18 Jan 2016 14:48:47 -0700 Subject: [PATCH 1/2] added driver files for ms4525D0 DS5AI_001D --- Makefile | 1 + src/board.h | 3 +++ src/cli.c | 2 +- src/drv_ms4525.c | 51 ++++++++++++++++++++++++++++++++++++++++++++++++ src/drv_ms4525.h | 3 +++ src/mw.c | 10 +++++++++- src/mw.h | 4 ++++ src/sensors.c | 34 ++++++++++++++++++++++++++++++++ src/serial.c | 6 ++++++ 9 files changed, 112 insertions(+), 2 deletions(-) create mode 100644 src/drv_ms4525.c create mode 100644 src/drv_ms4525.h diff --git a/Makefile b/Makefile index ab0bedf7..2c19886f 100644 --- a/Makefile +++ b/Makefile @@ -81,6 +81,7 @@ NAZE_SRC = drv_adc.c \ drv_ak8975.c \ drv_bma280.c \ drv_bmp085.c \ + drv_ms4525.c \ drv_ms5611.c \ drv_bmp280.c \ drv_hcsr04.c \ diff --git a/src/board.h b/src/board.h index a81b6615..b6d1413e 100755 --- a/src/board.h +++ b/src/board.h @@ -60,6 +60,7 @@ typedef enum { SENSOR_SONAR = 1 << 4, SENSOR_GPS = 1 << 5, SENSOR_GPSMAG = 1 << 6, + SENSOR_AIRSPEED = 1 << 7, } AvailableSensors; // Type of accelerometer used/detected @@ -223,6 +224,7 @@ typedef struct baro_t { #define GPS #define LEDRING #define SONAR +#define AIRSPEED #define BUZZER #define LED0 #define LED1 @@ -243,6 +245,7 @@ typedef struct baro_t { #include "drv_bma280.h" #include "drv_bmp085.h" #include "drv_bmp280.h" +#include "drv_ms4525.h" #include "drv_ms5611.h" #include "drv_hmc5883l.h" #include "drv_ak8975.h" diff --git a/src/cli.c b/src/cli.c index 6e5da1bb..c1c8738c 100644 --- a/src/cli.c +++ b/src/cli.c @@ -70,7 +70,7 @@ static const char *const featureNames[] = { // sync this with AvailableSensors enum from board.h static const char *const sensorNames[] = { - "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL + "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", "AIRSPEED", NULL }; // sync this with AccelSensors enum from board.h diff --git a/src/drv_ms4525.c b/src/drv_ms4525.c new file mode 100644 index 00000000..bc37644d --- /dev/null +++ b/src/drv_ms4525.c @@ -0,0 +1,51 @@ +#include "board.h" + +// MS4525, address 0x28 for the most common version +#define MS4525_ADDR 0x28 +#define STATUS_MASK 0x3F + +static void ms4525_read(int16_t *airspeedData); + +bool ms4525Detect(sensor_t *airspeed) +{ + bool ack; + uint8_t buf[1]; + + ack = i2cRead(MS4525_ADDR, 0xFF, 1, buf); + + if(!ack) + return false; + + airspeed->read = ms4525_read; + airspeed->scale = 0; + + return true; +} + +static void ms4525_read(int16_t *airspeedData) +{ + uint8_t buf[4]; + int16_t data[2]; + + i2cRead(MS4525_ADDR, 0xFF, 4, buf); + + uint8_t status = (buf[0] >> 5); // first two bits are status bits + if(status == 0x00) // good data packet + { + data[0] = (int16_t)(((STATUS_MASK | buf[0]) << 8) | buf[1]); + data[1] = (int16_t)((buf[2] << 3) | (buf[3] >> 5)); + } + else if(status == 0x02) // stale data packet + { + data[0] = (int16_t)(((STATUS_MASK | buf[0]) << 8) | buf[1]); + data[1] = (int16_t)((buf[2] << 3) | (buf[3] >> 5)); + } + else + { + data[0] = 0; + data[1] = 0; + } + + airspeedData[0] = data[0]; + airspeedData[1] = data[1]; +} diff --git a/src/drv_ms4525.h b/src/drv_ms4525.h new file mode 100644 index 00000000..a9e6d507 --- /dev/null +++ b/src/drv_ms4525.h @@ -0,0 +1,3 @@ +#pragma once + +bool ms4525Detect(sensor_t *airspeed); diff --git a/src/mw.c b/src/mw.c index 5a397538..5f2b800e 100755 --- a/src/mw.c +++ b/src/mw.c @@ -924,7 +924,7 @@ void loop(void) } #endif case 4: - taskOrder = 0; + taskOrder++; #ifdef SONAR if (sensors(SENSOR_SONAR)) { Sonar_update(); @@ -933,6 +933,14 @@ void loop(void) if (feature(FEATURE_VARIO) && f.VARIO_MODE) mwVario(); break; + case 5: + taskOrder = 0; +#ifdef AIRSPEED + if (sensors(SENSOR_AIRSPEED)) { + Airspeed_update(); + } +#endif + break; } } diff --git a/src/mw.h b/src/mw.h index 7f20bfa7..0f2ccbec 100755 --- a/src/mw.h +++ b/src/mw.h @@ -477,6 +477,8 @@ extern uint16_t vbat; // battery voltage in 0.1V steps extern int16_t telemTemperature1; // gyro sensor temperature extern int32_t amperage; // amperage read by current sensor in 0.01A steps extern int32_t mAhdrawn; // milli ampere hours drawn from battery since start +extern int16_t airspeedVelocity; +extern int16_t airspeedTemp; #define PITCH_LOOKUP_LENGTH 7 #define THROTTLE_LOOKUP_LENGTH 12 @@ -513,6 +515,7 @@ extern flags_t f; extern sensor_t acc; extern sensor_t gyro; extern baro_t baro; +extern sensor_t airspeed; // main void setPIDController(int type); @@ -538,6 +541,7 @@ int Mag_getADC(void); void Sonar_init(void); void Sonar_update(void); uint16_t RSSI_getValue(void); +void Airspeed_update(void); // Output void mixerInit(void); diff --git a/src/sensors.c b/src/sensors.c index 29fb6eac..948d1b76 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -12,6 +12,8 @@ uint16_t calibratingB = 0; // baro calibration = get new ground pressure va uint16_t calibratingG = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration. int16_t heading, magHold; +int16_t airspeedVelocity = 0; // airspeed m/s +int16_t airspeedTemp = 0; // air temperature deg C extern uint16_t InflightcalibratingA; extern bool AccInflightCalibrationMeasurementDone; @@ -26,6 +28,7 @@ sensor_t acc; // acc access functions sensor_t gyro; // gyro access functions sensor_t mag; // mag access functions baro_t baro; // barometer access functions +sensor_t airspeed; // airspeed access functions uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected uint8_t magHardware = MAG_DEFAULT; @@ -130,6 +133,12 @@ bool sensorsAutodetect(void) } #endif +#ifdef AIRSPEED + sensorsSet(SENSOR_AIRSPEED); + if (!ms4525Detect(&airspeed)) + sensorsClear(SENSOR_AIRSPEED); +#endif + // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(mcfg.acc_align); @@ -536,3 +545,28 @@ void Sonar_update(void) } #endif + +#ifdef AIRSPEED +void Airspeed_update(void) +{ + static uint32_t lastMeasurement = 0; + static int32_t delay = 50000; // ~20 hz + + if (lastMeasurement > currentTime) // protect from overflow of currentTime + lastMeasurement = currentTime; + + if ((int32_t)(currentTime - lastMeasurement) < delay) + return; + + lastMeasurement = currentTime; + + int16_t airspeedData[2]; + airspeed.read(airspeedData); + + if(airspeedData[0] != 0) + { + airspeedVelocity = airspeedData[0]; + airspeedTemp = airspeedData[1]; + } +} +#endif /* AIRSPEED */ diff --git a/src/serial.c b/src/serial.c index d8bfdba8..059cd7c8 100755 --- a/src/serial.c +++ b/src/serial.c @@ -40,6 +40,7 @@ #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters #define MSP_FW_CONFIG 123 //out message Returns parameters specific to Flying Wing mode +#define MSP_RAW_AIRSPEED 124 //out message airspeed, temperature #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -610,6 +611,11 @@ static void evaluateCommand(void) serialize32(EstAlt); serialize16(vario); break; + case MSP_RAW_AIRSPEED: + headSerialReply(4); + serialize16(airspeedVelocity); + serialize16(airspeedTemp); + break; case MSP_ANALOG: headSerialReply(7); serialize8((uint8_t)constrain(vbat, 0, 255)); From e72faaf03f68482caedb3825183bcc9ae799d40f Mon Sep 17 00:00:00 2001 From: Gary Ellingson Date: Mon, 18 Jan 2016 14:55:26 -0700 Subject: [PATCH 2/2] the airspeed is currently raw --- src/sensors.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sensors.c b/src/sensors.c index 948d1b76..1b78e3c7 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -12,8 +12,8 @@ uint16_t calibratingB = 0; // baro calibration = get new ground pressure va uint16_t calibratingG = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration. int16_t heading, magHold; -int16_t airspeedVelocity = 0; // airspeed m/s -int16_t airspeedTemp = 0; // air temperature deg C +int16_t airspeedVelocity = 0; // raw airspeed +int16_t airspeedTemp = 0; // raw air temperature extern uint16_t InflightcalibratingA; extern bool AccInflightCalibrationMeasurementDone;