Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge branch 'development' into te_no_homereset_on_failsafe

Browse source
This commit is contained in:
Tim Eckel 2017-11-23 18:02:29 -05:00 • committed by GitHub
commit 3050886f69
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
242 changed files with 5499 additions and 4733 deletions
Download patch file Download diff file Expand all files Collapse all files

11
Makefile
View file

@ -141,7 +141,7 @@ endif
GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE PIKOBLX
GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF3NEO SPRACINGF4EVO
GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO MATEKF405OSD
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER PIXRACER_ICM20608
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER
GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(VALID_TARGETS))
# note that there is no hardfault debugging startup file assembly handler for other platforms
@ -542,6 +542,7 @@ VPATH := $(VPATH):$(TARGET_DIR)
COMMON_SRC = \
$(TARGET_DIR_SRC) \
main.c \
target/common_hardware.c \
build/assert.c \
build/build_config.c \
build/debug.c \
@ -672,11 +673,11 @@ HIGHEND_SRC = \
common/colorconversion.c \
common/gps_conversion.c \
drivers/display_ug2864hsweg01.c \
drivers/rangefinder_hcsr04.c \
drivers/rangefinder_hcsr04_i2c.c \
drivers/rangefinder_srf10.c \
drivers/rangefinder/rangefinder_hcsr04.c \
drivers/rangefinder/rangefinder_hcsr04_i2c.c \
drivers/rangefinder/rangefinder_srf10.c \
drivers/rangefinder/rangefinder_vl53l0x.c \
drivers/opflow_fake.c \
drivers/rangefinder_vl53l0x.c \
drivers/vtx_common.c \
io/dashboard.c \
io/displayport_max7456.c \

10
docs/Board - MatekF405.md
View file

@ -14,6 +14,16 @@ F405+ICM20602, w/ Betaflight OSD & SD Card Slot
F405-AIO (only) exposes I2C pads for compass / baro etc.
## Firmware
Due to differences on the board (I2C --- see below), there are two
firmware variants:
| Board | Firmware |
| ------ | -------- |
| Matek F405-AIO | inav_x.y.z_MATEKF405.hex |
| Matek F405-OSD | inav_x.y.z_MATEKF405OSD.hex |
## MCU, Sensors and Features
### Hardware

4
docs/Board - OMNIBUS.md
View file

@ -55,8 +55,8 @@ I2C is available on J22 PWM7 and PWM8
|signal | Location | Alt. Location |
|-------|------------|---------------|
|SCL | J22 (PWM7) | J3 (SCL) |
|SDA | J22 (PWM8) | J3 (SDA) |
|SCL | J22 (PWM8) | J3 (SCL) |
|SDA | J22 (PWM7) | J3 (SDA) |
### RANGEFINDER

49
docs/Board - Omnibus F4.md
View file

@ -2,8 +2,9 @@
![Omnibus F4](https://quadmeup.com/wp-content/uploads/2016/11/Omnibus-F4-Pinout-Top-Full-768x447.jpg)
* For Omnibus F4 Pro (with BMP280 baro, current sensor and SD Card use Omnibus F4 Pro target)
* For Omnibus F4 Pro Corner use Omnibus F4 Pro target
* For Omnibus F4 Pro (with BMP280 baro, current sensor and SD Card) use **OMNIBUSF4PRO** target (LED strip on dedicated connection)
* For Onnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target (LED strip on M5 pin)
* For Omnibus F4 Pro Corner use **OMNIBUSF4PRO** target
## Features
@ -55,6 +56,7 @@
* PPM and UART1 can be used together when S.BUS jumper is removed (close to PPM/SBUS connector)
* Integrated current meter
* Uses target **OMNIBUSF4PRO**
* Omnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target (LED strip on M5 pin instead of incorrectly wired dedicated connection)
### Omnibus F4 Pro Corner
@ -92,7 +94,10 @@ SerialRX and PPM receivers should be connected to dedicated _PPM SBUS_ connector
This board uses STM32 VCP and _not_ utilizes UART when USB is connected. STM32 VCP drivers might be required!
Flashing requires DFU mode and STM32 DFU drivers. Use [Zadig](http://zadig.akeo.ie) tool to install WinUSB driver on Windows.
Flashing requires DFU mode and STM32 DFU drivers. Two options for installing the proper driver:
* [Zadig](http://zadig.akeo.ie) tool to install WinUSB driver on Windows.
* [ImpulseRC Driver Fixer](https://impulserc.com/pages/downloads) installs the STM32 DFU driver with a single click.
## Buzzer / Beeper
@ -124,12 +129,16 @@ it can do without overeating (150mA on 4S gives 1.5W of waste heat!). OSD, LED S
LED strip is enabled as indicated on flight controller silkscreen or schematics.
For INAV versions before v1.8.0, LED strip was shared with Motor 5 pin (PA1).
For Omnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target for LED strip on M5 pin as the dedicated LED strip connection on these devices is typically wired incorrectly.
## SoftwareSerial
This board allows for single **SoftwareSerial** port on small soldering pads located on the bottom side of the board.
Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount pads CH5/CH6.
### Omnibus F4 Pro SoftwareSerial Connections
![Omnibus F4 Pro SoftwareSerial Connections](assets/images/omnibusf4pro_ss.jpg)
| Pad | SoftwareSerial Role |
| ---- | ---- |
| CH5 | RX |
@ -137,13 +146,37 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount
## FrSky SmartPort using SoftwareSerial
SmartPort telemetry is possible using SoftwareSerial. RX and TX lines have to be bridged using
1kOhm resistor (confirmed working with 100Ohm, 1kOhm and 10kOhm)
SmartPort telemetry is possible using SoftwareSerial
```
SmartPort ---> RX (CH5 pad) ---> 1kOhm resistor ---> TX (CH6 pad)
```
### Omnibus F4 Pro SmartPort using SoftwareSerial
![Omnibus F4 Pro SmartPort using SoftwareSerial](assets/images/omnibusf4pro_ss.png)
* Telemetry has to be inverted with `set telemetry_inversion = ON`
* Port should be configured for _57600bps_
* Tested with FrSky X4R(SB), XSR, XSR-M, XSR-E
# Wiring diagrams for Omnibus F4 Pro
Following diagrams applies to _Pro_ version with integrated current meter and JST connectors only
## Board layout
![Omnibus F4 Pro Board Layout](assets/images/omnibusf4pro.png)
## Flying wing motor and servos
![Omnibus F4 Pro Flying Wing Setup](assets/images/omnibusf4pro_flyingwing_setup.png)
## RX setup
![Omnibus F4 Pro RX Setup](assets/images/omnibusf4pro_rx.png)
## FPV setup
![Omnibus F4 Pro FPV Setup](assets/images/omnibusf4pro_fpv_setup.png)
## GPS setup
![Omnibus F4 Pro GPS Setup](assets/images/omnibusf4pro_gps_setup.png)
_Diagrams created by Albert Kravcov (skaman82)_

2
docs/Serial.md
View file

@ -40,7 +40,7 @@ Configure serial ports first, then enable/disable features that use the ports.
If the configuration is invalid the serial port configuration will reset to its defaults and features may be disabled.
* There must always be a port available to use for MSP/CLI.
* There is a maximum of 2 MSP ports.
* There is a maximum of 3 MSP ports.
* To use a port for a function, the function's corresponding feature must be also be enabled.
e.g. after configuring a port for GPS enable the GPS feature.
* If SoftSerial is used, then all SoftSerial ports must use the same baudrate.

6
docs/Telemetry.md
View file

@ -64,13 +64,13 @@ The following sensors are transmitted
* **Hdg** : heading, North is 0°, South is 180°.
* **AccX,Y,Z** : accelerometer values.
* **Tmp1** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
* **A** : 1 = placeholder so flight mode is always 5 digits long, 2 = home reset, 4 = failsafe mode
* **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode
* **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode
* **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold
* **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode
* **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed
* **Tmp2** : GPS lock status, accuracy, and number of satellites. Additive number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
* **A** : 1 = GPS fix, 2 = GPS home fix (numbers are additive)
* **Tmp2** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
* **A** : 1 = GPS fix, 2 = GPS home fix, 4 = home reset (numbers are additive)
* **B** : GPS accuracy based on HDOP (0 = lowest to 9 = highest accuracy)
* **C** : number of satellites locked (digit C & D are the number of locked satellites)
* **D** : number of satellites locked (if 14 satellites are locked, C = 1 & D = 4)

BIN
docs/assets/images/omnibusf4pro.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 343 KiB

BIN
docs/assets/images/omnibusf4pro_flyingwing_setup.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 251 KiB

BIN
docs/assets/images/omnibusf4pro_fpv_setup.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 290 KiB

BIN
docs/assets/images/omnibusf4pro_gps_setup.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 176 KiB

BIN
docs/assets/images/omnibusf4pro_rx.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 305 KiB

BIN
docs/assets/images/omnibusf4pro_ss.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 106 KiB

BIN
docs/assets/images/omnibusf4pro_ss.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 244 KiB

1
src/main/build/debug.h
View file

@ -53,5 +53,6 @@ typedef enum {
DEBUG_RANGEFINDER,
DEBUG_RANGEFINDER_QUALITY,
DEBUG_PITOT,
DEBUG_AGL,
DEBUG_COUNT
} debugType_e;

2
src/main/build/version.h
View file

@ -17,7 +17,7 @@
#define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 8 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)

2
src/main/cms/cms_menu_blackbox.c
View file

@ -89,8 +89,10 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
};
CMS_Menu cmsx_menuBlackbox = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUBB",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

6
src/main/cms/cms_menu_builtin.c
View file

@ -81,8 +81,10 @@ static OSD_Entry menuInfoEntries[] = {
};
static CMS_Menu menuInfo = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUINFO",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_InfoInit,
.onExit = NULL,
.onGlobalExit = NULL,
@ -117,8 +119,10 @@ static OSD_Entry menuFeaturesEntries[] =
};
static CMS_Menu menuFeatures = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUFEATURES",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -149,8 +153,10 @@ static OSD_Entry menuMainEntries[] =
};
CMS_Menu menuMain = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUMAIN",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

16
src/main/cms/cms_menu_imu.c
View file

@ -150,8 +150,10 @@ static OSD_Entry cmsx_menuPidEntries[] =
};
static CMS_Menu cmsx_menuPid = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPID",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_PidOnEnter,
.onExit = cmsx_PidWriteback,
.onGlobalExit = NULL,
@ -200,8 +202,10 @@ static OSD_Entry cmsx_menuPidAltMagEntries[] = {
};
static CMS_Menu cmsx_menuPidAltMag = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XALTMAG",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_menuPidAltMag_onEnter,
.onExit = cmsx_menuPidAltMag_onExit,
.onGlobalExit = NULL,
@ -245,8 +249,10 @@ static OSD_Entry cmsx_menuPidGpsnavEntries[] = {
};
static CMS_Menu cmsx_menuPidGpsnav = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XGPSNAV",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_menuPidGpsnav_onEnter,
.onExit = cmsx_menuPidGpsnav_onExit,
.onGlobalExit = NULL,
@ -283,8 +289,10 @@ static OSD_Entry cmsx_menuRateProfileEntries[] =
};
static CMS_Menu cmsx_menuRateProfile = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENURATE",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -340,8 +348,10 @@ static OSD_Entry cmsx_menuProfileOtherEntries[] = {
};
static CMS_Menu cmsx_menuProfileOther = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XPROFOTHER",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_profileOtherOnEnter,
.onExit = cmsx_profileOtherOnExit,
.onGlobalExit = NULL,
@ -366,8 +376,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] =
};
static CMS_Menu cmsx_menuFilterPerProfile = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XFLTPP",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -387,8 +399,10 @@ static OSD_Entry cmsx_menuGyroEntries[] =
};
static CMS_Menu cmsx_menuGyro = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XGYROGLB",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -424,8 +438,10 @@ static OSD_Entry cmsx_menuImuEntries[] =
};
CMS_Menu cmsx_menuImu = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XIMU",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_menuImu_onEnter,
.onExit = cmsx_menuImu_onExit,
.onGlobalExit = NULL,

2
src/main/cms/cms_menu_ledstrip.c
View file

@ -66,8 +66,10 @@ static OSD_Entry cmsx_menuLedstripEntries[] =
};
CMS_Menu cmsx_menuLedstrip = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENULED",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

4
src/main/cms/cms_menu_misc.c
View file

@ -71,8 +71,10 @@ static OSD_Entry cmsx_menuRcEntries[] =
};
CMS_Menu cmsx_menuRcPreview = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XRCPREV",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = cmsx_menuRcConfirmBack,
.onGlobalExit = NULL,
@ -96,8 +98,10 @@ static OSD_Entry menuMiscEntries[]=
};
CMS_Menu cmsx_menuMisc = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "XMISC",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

8
src/main/cms/cms_menu_navigation.c
View file

@ -54,8 +54,10 @@ static OSD_Entry cmsx_menuNavSettingsEntries[] =
};
static CMS_Menu cmsx_menuNavSettings = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVSETTINGS",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -81,8 +83,10 @@ static CMS_Menu cmsx_menuNavSettings = {
};
static CMS_Menu cmsx_menuRTH = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVRTH",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -105,8 +109,10 @@ static OSD_Entry cmsx_menuFixedWingEntries[] =
};
static CMS_Menu cmsx_menuFixedWing = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAVFW",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -124,8 +130,10 @@ static OSD_Entry cmsx_menuNavigationEntries[] =
};
CMS_Menu cmsx_menuNavigation = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUNAV",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

6
src/main/cms/cms_menu_osd.c
View file

@ -47,8 +47,10 @@ OSD_Entry cmsx_menuAlarmsEntries[] =
};
CMS_Menu cmsx_menuAlarms = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUALARMS",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,
@ -108,8 +110,10 @@ OSD_Entry menuOsdActiveElemsEntries[] =
};
CMS_Menu menuOsdActiveElems = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUOSDACT",
.GUARD_type = OME_MENU,
#endif
.onEnter = menuOsdActiveElemsOnEnter,
.onExit = menuOsdActiveElemsOnExit,
.onGlobalExit = NULL,
@ -125,8 +129,10 @@ OSD_Entry cmsx_menuOsdLayoutEntries[] =
};
CMS_Menu cmsx_menuOsdLayout = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENULAYOUT",
.GUARD_type = OME_MENU,
#endif
.onEnter = NULL,
.onExit = NULL,
.onGlobalExit = NULL,

2
src/main/cms/cms_menu_vtx.c
View file

@ -132,8 +132,10 @@ static OSD_Entry cmsx_menuVtxEntries[] =
};
CMS_Menu cmsx_menuVtx = {
#ifdef CMS_MENU_DEBUG
.GUARD_text = "MENUVTX",
.GUARD_type = OME_MENU,
#endif
.onEnter = cmsx_Vtx_onEnter,
.onExit= cmsx_Vtx_onExit,
.onGlobalExit = cmsx_Vtx_FeatureWriteback,

5
src/main/cms/cms_types.h
View file

@ -61,7 +61,7 @@ typedef long (*CMSEntryFuncPtr)(displayPort_t *displayPort, const void *ptr);
typedef struct
{
const char *text;
const char * const text;
const OSD_MenuElement type;
const CMSEntryFuncPtr func;
void *data;
@ -110,10 +110,11 @@ typedef long (*CMSMenuOnExitPtr)(const OSD_Entry *self);
typedef struct
{
#ifdef CMS_MENU_DEBUG
// These two are debug aids for menu content creators.
const char *GUARD_text;
const OSD_MenuElement GUARD_type;
#endif
const CMSMenuFuncPtr onEnter;
const CMSMenuOnExitPtr onExit;
const CMSMenuFuncPtr onGlobalExit;

2
src/main/common/time.c
View file

@ -120,7 +120,7 @@ static bool rtcIsDateTimeValid(dateTime_t *dateTime)
(dateTime->millis <= 999);
}
static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, uint16_t minutes)
static void dateTimeWithOffset(dateTime_t *dateTimeOffset, dateTime_t *dateTimeInitial, int16_t minutes)
{
rtcTime_t initialTime = dateTimeToRtcTime(dateTimeInitial);
rtcTime_t offsetTime = rtcTimeMake(rtcTimeGetSeconds(&initialTime) + minutes * 60, rtcTimeGetMillis(&initialTime));

3
src/main/config/parameter_group_ids.h
View file

@ -95,7 +95,8 @@
#define PG_ADC_CHANNEL_CONFIG 1010
#define PG_TIME_CONFIG 1011
#define PG_OPFLOW_CONFIG 1012
#define PG_INAV_END 1012
#define PG_DISPLAY_CONFIG 1013
#define PG_INAV_END 1013
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

17
src/main/drivers/accgyro/accgyro.h
View file

@ -43,34 +43,35 @@ typedef enum {
} gyroRateKHz_e;
typedef struct gyroDev_s {
busDevice_t * busDev;
sensorGyroInitFuncPtr initFn; // initialize function
sensorGyroReadFuncPtr readFn; // read 3 axis data function
sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatusFn;
sensorGyroUpdateFuncPtr updateFn;
extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT];
uint8_t lpf;
uint8_t imuSensorToUse;
gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops;
volatile bool dataReady;
sensor_align_e gyroAlign;
mpuDetectionResult_t mpuDetectionResult;
const extiConfig_t *mpuIntExtiConfig;
mpuConfiguration_t mpuConfiguration;
// Device-specific configuration (set up by driver)
union {
mpuConfiguration_t mpu;
} devConfig;
} gyroDev_t;
typedef struct accDev_s {
busDevice_t * busDev;
sensorAccInitFuncPtr initFn; // initialize function
sensorAccReadFuncPtr readFn; // read 3 axis data function
busDevice_t bus;
uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known
uint8_t imuSensorToUse;
sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} accDev_t;

75
src/main/drivers/accgyro/accgyro_adxl345.c
View file

@ -16,17 +16,20 @@
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/system.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_adxl345.h"
#ifdef USE_ACC_ADXL345
// ADXL345, Alternative address mode 0x53
#define ADXL345_ADDRESS 0x53
@ -56,80 +59,58 @@
#define ADXL345_RANGE_16G 0x03
#define ADXL345_FIFO_STREAM 0x80
static bool useFifo = false;
static void adxl345Init(accDev_t *acc)
{
if (useFifo) {
uint8_t fifoDepth = 16;
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400);
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM);
} else {
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
}
busWrite(acc->busDev, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
busWrite(acc->busDev, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
busWrite(acc->busDev, ADXL345_BW_RATE, ADXL345_RATE_100);
acc->acc_1G = 256; // 3.3V operation
}
uint8_t acc_samples = 0;
static bool adxl345Read(accDev_t *acc)
{
uint8_t buf[8];
if (useFifo) {
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
uint8_t i = 0;
uint8_t samples_remaining;
do {
i++;
if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
return false;
}
x += (int16_t)(buf[0] + (buf[1] << 8));
y += (int16_t)(buf[2] + (buf[3] << 8));
z += (int16_t)(buf[4] + (buf[5] << 8));
samples_remaining = buf[7] & 0x7F;
} while ((i < 32) && (samples_remaining > 0));
acc->ADCRaw[0] = x / i;
acc->ADCRaw[1] = y / i;
acc->ADCRaw[2] = z / i;
acc_samples = i;
} else {
if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) {
if (!busReadBuf(acc->busDev, ADXL345_DATA_OUT, buf, 6)) {
return false;
}
acc->ADCRaw[0] = buf[0] + (buf[1] << 8);
acc->ADCRaw[1] = buf[2] + (buf[3] << 8);
acc->ADCRaw[2] = buf[4] + (buf[5] << 8);
}
return true;
}
bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init)
static bool deviceDetect(busDevice_t * dev)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig);
ack = busRead(dev, 0x00, &sig);
if (!ack || sig != 0xE5)
return false;
// use ADXL345's fifo to filter data or not
useFifo = init->useFifo;
return true;
}
bool adxl345Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ADXL345, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = adxl345Init;
acc->readFn = adxl345Read;
return true;
}
#endif

7
src/main/drivers/accgyro/accgyro_adxl345.h
View file

@ -17,9 +17,4 @@
#pragma once
typedef struct drv_adxl345_config_s {
bool useFifo;
uint16_t dataRate;
} drv_adxl345_config_t;
bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init);
bool adxl345Detect(accDev_t *acc);

35
src/main/drivers/accgyro/accgyro_bma280.c
View file

@ -16,17 +16,21 @@
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/bus_i2c.h"
#include "drivers/system.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_bma280.h"
#ifdef USE_ACC_BMA280
// BMA280, default I2C address mode 0x18
#define BMA280_WHOAMI 0x00
#define BMA280_ADDRESS 0x18
#define BMA280_ACC_X_LSB 0x02
#define BMA280_PMU_BW 0x10
@ -34,8 +38,8 @@
static void bma280Init(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
busWrite(acc->busDev, BMA280_PMU_RANGE, 0x08); // +-8g range
busWrite(acc->busDev, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc->acc_1G = 512 * 8;
}
@ -44,7 +48,7 @@ static bool bma280Read(accDev_t *acc)
{
uint8_t buf[6];
if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
if (!busReadBuf(acc->busDev, BMA280_ACC_X_LSB, buf, 6)) {
return false;
}
@ -56,16 +60,33 @@ static bool bma280Read(accDev_t *acc)
return true;
}
bool bma280Detect(accDev_t *acc)
static bool deviceDetect(busDevice_t * dev)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
ack = busRead(dev, BMA280_WHOAMI, &sig);
if (!ack || sig != 0xFB)
return false;
return true;
}
bool bma280Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMA280, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = bma280Init;
acc->readFn = bma280Read;
return true;
}
#endif

1
src/main/drivers/accgyro/accgyro_fake.c
View file

@ -104,7 +104,6 @@ bool fakeAccDetect(accDev_t *acc)
{
acc->initFn = fakeAccInit;
acc->readFn = fakeAccRead;
acc->revisionCode = 0;
return true;
}
#endif // USE_FAKE_ACC

45
src/main/drivers/accgyro/accgyro_l3g4200d.c
View file

@ -16,6 +16,7 @@
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include <platform.h>
@ -78,13 +79,14 @@ static void l3g4200dInit(gyroDev_t *gyro)
}
delay(100);
ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
if (!ack)
ack = busWrite(gyro->busDev, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
if (!ack) {
failureMode(FAILURE_ACC_INIT);
}
delay(5);
i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
busWrite(gyro->busDev, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
@ -92,7 +94,7 @@ static bool l3g4200dRead(gyroDev_t *gyro)
{
uint8_t buf[6];
if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
if (!busReadBuf(gyro->busDev, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, buf, 6)) {
return false;
}
@ -103,21 +105,40 @@ static bool l3g4200dRead(gyroDev_t *gyro)
return true;
}
bool l3g4200dDetect(gyroDev_t *gyro)
static bool deviceDetect(busDevice_t * busDev)
{
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (int retry = 0; retry < 5; retry++) {
uint8_t deviceid;
delay(25);
delay(150);
bool ack = busRead(busDev, L3G4200D_WHO_AM_I, &deviceid);
if (ack && deviceid == L3G4200D_ID) {
return true;
}
};
i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
if (deviceid != L3G4200D_ID)
return false;
}
bool l3g4200dDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_L3G4200, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!deviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
gyro->initFn = l3g4200dInit;
gyro->readFn = l3g4200dRead;
// 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f;
gyro->scale = 1.0f / 14.2857f; // 14.2857dps/lsb scalefactor
return true;
}

107
src/main/drivers/accgyro/accgyro_l3gd20.c
View file

@ -32,6 +32,8 @@
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_l3gd20.h"
#ifdef USE_GYRO_L3GD20
#define READ_CMD ((uint8_t)0x80)
#define MULTIPLEBYTE_CMD ((uint8_t)0x40)
#define DUMMY_BYTE ((uint8_t)0x00)
@ -66,57 +68,15 @@
#define BOOT ((uint8_t)0x80)
#define ENABLE_L3GD20 IOLo(mpul3gd20CsPin)
#define DISABLE_L3GD20 IOHi(mpul3gd20CsPin)
static IO_t mpul3gd20CsPin = IO_NONE;
static void l3gd20SpiInit(SPI_TypeDef *SPIx)
{
UNUSED(SPIx); // FIXME
mpul3gd20CsPin = IOGetByTag(IO_TAG(L3GD20_CS_PIN));
IOInit(mpul3gd20CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpul3gd20CsPin, SPI_IO_CS_CFG);
DISABLE_L3GD20;
spiSetSpeed(L3GD20_SPI, SPI_CLOCK_STANDARD);
}
void l3gd20GyroInit(gyroDev_t *gyro)
{
UNUSED(gyro); // FIXME use it!
busWrite(gyro->busDev, CTRL_REG5_ADDR, BOOT);
delay(1);
l3gd20SpiInit(L3GD20_SPI);
ENABLE_L3GD20;
spiTransferByte(L3GD20_SPI, CTRL_REG5_ADDR);
spiTransferByte(L3GD20_SPI, BOOT);
DISABLE_L3GD20;
delayMicroseconds(100);
ENABLE_L3GD20;
spiTransferByte(L3GD20_SPI, CTRL_REG1_ADDR);
spiTransferByte(L3GD20_SPI, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
//spiTransferByte(L3GD20_SPI, MODE_ACTIVE | OUTPUT_DATARATE_4 | AXES_ENABLE | BANDWIDTH_4);
DISABLE_L3GD20;
delayMicroseconds(1);
ENABLE_L3GD20;
spiTransferByte(L3GD20_SPI, CTRL_REG4_ADDR);
spiTransferByte(L3GD20_SPI, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
DISABLE_L3GD20;
busWrite(gyro->busDev, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
delay(1);
busWrite(gyro->busDev, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
delay(100);
}
@ -124,38 +84,39 @@ static bool l3gd20GyroRead(gyroDev_t *gyro)
{
uint8_t buf[6];
ENABLE_L3GD20;
spiTransferByte(L3GD20_SPI, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD);
uint8_t index;
for (index = 0; index < sizeof(buf); index++) {
buf[index] = spiTransferByte(L3GD20_SPI, DUMMY_BYTE);
}
DISABLE_L3GD20;
busReadBuf(gyro->busDev, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD, buf, 6);
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
#endif
return true;
}
static bool deviceDetect(busDevice_t * dev)
{
return true; // blindly assume it's present, for now.
}
bool l3gd20Detect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_L3GD20, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!deviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
gyro->initFn = l3gd20GyroInit;
gyro->readFn = l3gd20GyroRead;
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
gyro->scale = 0.07f;
return true;
}
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyroDev_t *gyro)
{
gyro->initFn = l3gd20GyroInit;
gyro->readFn = l3gd20GyroRead;
gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
return true; // blindly assume it's present, for now.
}
#endif

54
src/main/drivers/accgyro/accgyro_lsm303dlhc.c
View file

@ -67,6 +67,7 @@
// Registers
#define LSM303DLHC_STATUS_REG_A 0x27 /* Status register acceleration */
#define CTRL_REG1_A 0x20
#define CTRL_REG4_A 0x23
#define CTRL_REG5_A 0x24
@ -103,30 +104,15 @@
#define CONTINUOUS_CONVERSION 0x00
uint8_t accelCalibrating = false;
float accelOneG = 9.8065;
int32_t accelSum100Hz[3] = { 0, 0, 0 };
int32_t accelSum500Hz[3] = { 0, 0, 0 };
int32_t accelSummedSamples100Hz[3];
int32_t accelSummedSamples500Hz[3];
void lsm303dlhcAccInit(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
busWrite(acc->busDev, CTRL_REG5_A, BOOT);
delay(100);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
busWrite(acc->busDev, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
delay(10);
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
busWrite(acc->busDev, CTRL_REG4_A, FULLSCALE_4G);
delay(100);
acc->acc_1G = 512 * 8;
@ -137,7 +123,7 @@ static bool lsm303dlhcAccRead(accDev_t *acc)
{
uint8_t buf[6];
bool ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
bool ack = busReadBuf(acc->busDev, AUTO_INCREMENT_ENABLE | OUT_X_L_A, buf, 6);
if (!ack) {
return false;
@ -148,26 +134,36 @@ static bool lsm303dlhcAccRead(accDev_t *acc)
acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
#endif
return true;
}
static bool deviceDetect(busDevice_t * busDev)
{
bool ack = false;
uint8_t status = 0;
ack = busRead(busDev, LSM303DLHC_STATUS_REG_A, &status);
if (!ack)
return false;
return true;
}
bool lsm303dlhcAccDetect(accDev_t *acc)
{
bool ack;
uint8_t status;
ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_REG_A, 1, &status);
if (!ack)
acc->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LSM303DLHC, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = lsm303dlhcAccInit;
acc->readFn = lsm303dlhcAccRead;
return true;
}
#endif

421
src/main/drivers/accgyro/accgyro_lsm303dlhc.h
View file

@ -17,426 +17,5 @@
#pragma once
/**
* @brief LSM303DLHC Status
*/
/* LSM303DLHC ACC struct */
typedef struct {
uint8_t Power_Mode; /* Power-down/Normal Mode */
uint8_t AccOutput_DataRate; /* OUT data rate */
uint8_t Axes_Enable; /* Axes enable */
uint8_t High_Resolution; /* High Resolution enabling/disabling */
uint8_t BlockData_Update; /* Block Data Update */
uint8_t Endianness; /* Endian Data selection */
uint8_t AccFull_Scale; /* Full Scale selection */
} LSM303DLHCAcc_InitTypeDef;
/* LSM303DLHC Acc High Pass Filter struct */
typedef struct {
uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */
uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */
uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */
uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */
} LSM303DLHCAcc_FilterConfigTypeDef;
/* LSM303DLHC Mag struct */
typedef struct {
uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */
uint8_t MagOutput_DataRate; /* OUT data rate */
uint8_t Working_Mode; /* operating mode */
uint8_t MagFull_Scale; /* Full Scale selection */
} LSM303DLHCMag_InitTypeDef;
/**
* @}
*/
/** @defgroup STM32F3_DISCOVERY_LSM303DLHC_Exported_Constants
* @{
*/
#define LSM303DLHC_OK ((uint32_t) 0)
#define LSM303DLHC_FAIL ((uint32_t) 0)
/* Uncomment the following line to use the default LSM303DLHC_TIMEOUT_UserCallback()
function implemented in stm32f3_discovery_lgd20.c file.
LSM303DLHC_TIMEOUT_UserCallback() function is called whenever a timeout condition
occure during communication (waiting transmit data register empty flag(TXE)
or waiting receive data register is not empty flag (RXNE)). */
/* #define USE_DEFAULT_TIMEOUT_CALLBACK */
/* Maximum Timeout values for flags waiting loops. These timeouts are not based
on accurate values, they just guarantee that the application will not remain
stuck if the I2C communication is corrupted.
You may modify these timeout values depending on CPU frequency and application
conditions (interrupts routines ...). */
#define LSM303DLHC_FLAG_TIMEOUT ((uint32_t)0x1000)
#define LSM303DLHC_LONG_TIMEOUT ((uint32_t)(10 * LSM303DLHC_FLAG_TIMEOUT))
/**
* @brief LSM303DLHC I2C Interface pins
*/
#define LSM303DLHC_I2C I2C1
#define LSM303DLHC_I2C_SCK_PIN PB6 /* PB.06 */
#define LSM303DLHC_I2C_SDA_PIN PB7 /* PB.7 */
#define LSM303DLHC_DRDY_PIN PE2 /* PE.02 */
#define LSM303DLHC_I2C_INT1_PIN PE4 /* PE.04 */
#define LSM303DLHC_I2C_INT2_PIN PE5 /* PE.05 */
/******************************************************************************/
/*************************** START REGISTER MAPPING **************************/
/******************************************************************************/
/* Acceleration Registers */
#define LSM303DLHC_CTRL_REG1_A 0x20 /* Control register 1 acceleration */
#define LSM303DLHC_CTRL_REG2_A 0x21 /* Control register 2 acceleration */
#define LSM303DLHC_CTRL_REG3_A 0x22 /* Control register 3 acceleration */
#define LSM303DLHC_CTRL_REG4_A 0x23 /* Control register 4 acceleration */
#define LSM303DLHC_CTRL_REG5_A 0x24 /* Control register 5 acceleration */
#define LSM303DLHC_CTRL_REG6_A 0x25 /* Control register 6 acceleration */
#define LSM303DLHC_REFERENCE_A 0x26 /* Reference register acceleration */
#define LSM303DLHC_STATUS_REG_A 0x27 /* Status register acceleration */
#define LSM303DLHC_OUT_X_L_A 0x28 /* Output Register X acceleration */
#define LSM303DLHC_OUT_X_H_A 0x29 /* Output Register X acceleration */
#define LSM303DLHC_OUT_Y_L_A 0x2A /* Output Register Y acceleration */
#define LSM303DLHC_OUT_Y_H_A 0x2B /* Output Register Y acceleration */
#define LSM303DLHC_OUT_Z_L_A 0x2C /* Output Register Z acceleration */
#define LSM303DLHC_OUT_Z_H_A 0x2D /* Output Register Z acceleration */
#define LSM303DLHC_FIFO_CTRL_REG_A 0x2E /* Fifo control Register acceleration */
#define LSM303DLHC_FIFO_SRC_REG_A 0x2F /* Fifo src Register acceleration */
#define LSM303DLHC_INT1_CFG_A 0x30 /* Interrupt 1 configuration Register acceleration */
#define LSM303DLHC_INT1_SOURCE_A 0x31 /* Interrupt 1 source Register acceleration */
#define LSM303DLHC_INT1_THS_A 0x32 /* Interrupt 1 Threshold register acceleration */
#define LSM303DLHC_INT1_DURATION_A 0x33 /* Interrupt 1 DURATION register acceleration */
#define LSM303DLHC_INT2_CFG_A 0x34 /* Interrupt 2 configuration Register acceleration */
#define LSM303DLHC_INT2_SOURCE_A 0x35 /* Interrupt 2 source Register acceleration */
#define LSM303DLHC_INT2_THS_A 0x36 /* Interrupt 2 Threshold register acceleration */
#define LSM303DLHC_INT2_DURATION_A 0x37 /* Interrupt 2 DURATION register acceleration */
#define LSM303DLHC_CLICK_CFG_A 0x38 /* Click configuration Register acceleration */
#define LSM303DLHC_CLICK_SOURCE_A 0x39 /* Click 2 source Register acceleration */
#define LSM303DLHC_CLICK_THS_A 0x3A /* Click 2 Threshold register acceleration */
#define LSM303DLHC_TIME_LIMIT_A 0x3B /* Time Limit Register acceleration */
#define LSM303DLHC_TIME_LATENCY_A 0x3C /* Time Latency Register acceleration */
#define LSM303DLHC_TIME_WINDOW_A 0x3D /* Time window register acceleration */
/* Magnetic field Registers */
#define LSM303DLHC_CRA_REG_M 0x00 /* Control register A magnetic field */
#define LSM303DLHC_CRB_REG_M 0x01 /* Control register B magnetic field */
#define LSM303DLHC_MR_REG_M 0x02 /* Control register MR magnetic field */
#define LSM303DLHC_OUT_X_H_M 0x03 /* Output Register X magnetic field */
#define LSM303DLHC_OUT_X_L_M 0x04 /* Output Register X magnetic field */
#define LSM303DLHC_OUT_Z_H_M 0x05 /* Output Register Z magnetic field */
#define LSM303DLHC_OUT_Z_L_M 0x06 /* Output Register Z magnetic field */
#define LSM303DLHC_OUT_Y_H_M 0x07 /* Output Register Y magnetic field */
#define LSM303DLHC_OUT_Y_L_M 0x08 /* Output Register Y magnetic field */
#define LSM303DLHC_SR_REG_M 0x09 /* Status Register magnetic field */
#define LSM303DLHC_IRA_REG_M 0x0A /* IRA Register magnetic field */
#define LSM303DLHC_IRB_REG_M 0x0B /* IRB Register magnetic field */
#define LSM303DLHC_IRC_REG_M 0x0C /* IRC Register magnetic field */
#define LSM303DLHC_TEMP_OUT_H_M 0x31 /* Temperature Register magnetic field */
#define LSM303DLHC_TEMP_OUT_L_M 0x32 /* Temperature Register magnetic field */
/******************************************************************************/
/**************************** END REGISTER MAPPING ***************************/
/******************************************************************************/
#define ACC_I2C_ADDRESS 0x32
#define MAG_I2C_ADDRESS 0x3C
/** @defgroup Acc_Power_Mode_selection
* @{
*/
#define LSM303DLHC_NORMAL_MODE ((uint8_t)0x00)
#define LSM303DLHC_LOWPOWER_MODE ((uint8_t)0x08)
/**
* @}
*/
/** @defgroup Acc_OutPut_DataRate_Selection
* @{
*/
#define LSM303DLHC_ODR_1_HZ ((uint8_t)0x10) /*!< Output Data Rate = 1 Hz */
#define LSM303DLHC_ODR_10_HZ ((uint8_t)0x20) /*!< Output Data Rate = 10 Hz */
#define LSM303DLHC_ODR_25_HZ ((uint8_t)0x30) /*!< Output Data Rate = 25 Hz */
#define LSM303DLHC_ODR_50_HZ ((uint8_t)0x40) /*!< Output Data Rate = 50 Hz */
#define LSM303DLHC_ODR_100_HZ ((uint8_t)0x50) /*!< Output Data Rate = 100 Hz */
#define LSM303DLHC_ODR_200_HZ ((uint8_t)0x60) /*!< Output Data Rate = 200 Hz */
#define LSM303DLHC_ODR_400_HZ ((uint8_t)0x70) /*!< Output Data Rate = 400 Hz */
#define LSM303DLHC_ODR_1620_HZ_LP ((uint8_t)0x80) /*!< Output Data Rate = 1620 Hz only in Low Power Mode */
#define LSM303DLHC_ODR_1344_HZ ((uint8_t)0x90) /*!< Output Data Rate = 1344 Hz in Normal mode and 5376 Hz in Low Power Mode */
/**
* @}
*/
/** @defgroup Acc_Axes_Selection
* @{
*/
#define LSM303DLHC_X_ENABLE ((uint8_t)0x01)
#define LSM303DLHC_Y_ENABLE ((uint8_t)0x02)
#define LSM303DLHC_Z_ENABLE ((uint8_t)0x04)
#define LSM303DLHC_AXES_ENABLE ((uint8_t)0x07)
#define LSM303DLHC_AXES_DISABLE ((uint8_t)0x00)
/**
* @}
*/
/** @defgroup Acc_High_Resolution
* @{
*/
#define LSM303DLHC_HR_ENABLE ((uint8_t)0x08)
#define LSM303DLHC_HR_DISABLE ((uint8_t)0x00)
/**
* @}
*/
/** @defgroup Acc_Full_Scale_Selection
* @{
*/
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */
/**
* @}
*/
/** @defgroup Acc_Block_Data_Update
* @{
*/
#define LSM303DLHC_BlockUpdate_Continous ((uint8_t)0x00) /*!< Continuos Update */
#define LSM303DLHC_BlockUpdate_Single ((uint8_t)0x80) /*!< Single Update: output registers not updated until MSB and LSB reading */
/**
* @}
*/
/** @defgroup Acc_Endian_Data_selection
* @{
*/
#define LSM303DLHC_BLE_LSB ((uint8_t)0x00) /*!< Little Endian: data LSB @ lower address */
#define LSM303DLHC_BLE_MSB ((uint8_t)0x40) /*!< Big Endian: data MSB @ lower address */
/**
* @}
*/
/** @defgroup Acc_Boot_Mode_selection
* @{
*/
#define LSM303DLHC_BOOT_NORMALMODE ((uint8_t)0x00)
#define LSM303DLHC_BOOT_REBOOTMEMORY ((uint8_t)0x80)
/**
* @}
*/
/** @defgroup Acc_High_Pass_Filter_Mode
* @{
*/
#define LSM303DLHC_HPM_NORMAL_MODE_RES ((uint8_t)0x00)
#define LSM303DLHC_HPM_REF_SIGNAL ((uint8_t)0x40)
#define LSM303DLHC_HPM_NORMAL_MODE ((uint8_t)0x80)
#define LSM303DLHC_HPM_AUTORESET_INT ((uint8_t)0xC0)
/**
* @}
*/
/** @defgroup Acc_High_Pass_CUT OFF_Frequency
* @{
*/
#define LSM303DLHC_HPFCF_8 ((uint8_t)0x00)
#define LSM303DLHC_HPFCF_16 ((uint8_t)0x10)
#define LSM303DLHC_HPFCF_32 ((uint8_t)0x20)
#define LSM303DLHC_HPFCF_64 ((uint8_t)0x30)
/**
* @}
*/
/** @defgroup Acc_High_Pass_Filter_status
* @{
*/
#define LSM303DLHC_HIGHPASSFILTER_DISABLE ((uint8_t)0x00)
#define LSM303DLHC_HIGHPASSFILTER_ENABLE ((uint8_t)0x08)
/**
* @}
*/
/** @defgroup Acc_High_Pass_Filter_Click_status
* @{
*/
#define LSM303DLHC_HPF_CLICK_DISABLE ((uint8_t)0x00)
#define LSM303DLHC_HPF_CLICK_ENABLE ((uint8_t)0x04)
/**
* @}
*/
/** @defgroup Acc_High_Pass_Filter_AOI1_status
* @{
*/
#define LSM303DLHC_HPF_AOI1_DISABLE ((uint8_t)0x00)
#define LSM303DLHC_HPF_AOI1_ENABLE ((uint8_t)0x01)
/**
* @}
*/
/** @defgroup Acc_High_Pass_Filter_AOI2_status
* @{
*/
#define LSM303DLHC_HPF_AOI2_DISABLE ((uint8_t)0x00)
#define LSM303DLHC_HPF_AOI2_ENABLE ((uint8_t)0x02)
/**
* @}
*/
/** @defgroup Acc_LSM303DLHC_Interrupt1_Configuration_definition
* @{
*/
#define LSM303DLHC_IT1_CLICK ((uint8_t)0x80)
#define LSM303DLHC_IT1_AOI1 ((uint8_t)0x40)
#define LSM303DLHC_IT1_AOI2 ((uint8_t)0x20)
#define LSM303DLHC_IT1_DRY1 ((uint8_t)0x10)
#define LSM303DLHC_IT1_DRY2 ((uint8_t)0x08)
#define LSM303DLHC_IT1_WTM ((uint8_t)0x04)
#define LSM303DLHC_IT1_OVERRUN ((uint8_t)0x02)
/**
* @}
*/
/** @defgroup Acc_LSM303DLHC_Interrupt2_Configuration_definition
* @{
*/
#define LSM303DLHC_IT2_CLICK ((uint8_t)0x80)
#define LSM303DLHC_IT2_INT1 ((uint8_t)0x40)
#define LSM303DLHC_IT2_INT2 ((uint8_t)0x20)
#define LSM303DLHC_IT2_BOOT ((uint8_t)0x10)
#define LSM303DLHC_IT2_ACT ((uint8_t)0x08)
#define LSM303DLHC_IT2_HLACTIVE ((uint8_t)0x02)
/**
* @}
*/
/** @defgroup Acc_INT_Combination_Status
* @{
*/
#define LSM303DLHC_OR_COMBINATION ((uint8_t)0x00) /*!< OR combination of enabled IRQs */
#define LSM303DLHC_AND_COMBINATION ((uint8_t)0x80) /*!< AND combination of enabled IRQs */
#define LSM303DLHC_MOV_RECOGNITION ((uint8_t)0x40) /*!< 6D movement recognition */
#define LSM303DLHC_POS_RECOGNITION ((uint8_t)0xC0) /*!< 6D position recognition */
/**
* @}
*/
/** @defgroup Acc_INT_Axes
* @{
*/
#define LSM303DLHC_Z_HIGH ((uint8_t)0x20) /*!< Z High enabled IRQs */
#define LSM303DLHC_Z_LOW ((uint8_t)0x10) /*!< Z low enabled IRQs */
#define LSM303DLHC_Y_HIGH ((uint8_t)0x08) /*!< Y High enabled IRQs */
#define LSM303DLHC_Y_LOW ((uint8_t)0x04) /*!< Y low enabled IRQs */
#define LSM303DLHC_X_HIGH ((uint8_t)0x02) /*!< X High enabled IRQs */
#define LSM303DLHC_X_LOW ((uint8_t)0x01) /*!< X low enabled IRQs */
/**
* @}
*/
/** @defgroup Acc_INT_Click
* @{
*/
#define LSM303DLHC_Z_DOUBLE_CLICK ((uint8_t)0x20) /*!< Z double click IRQs */
#define LSM303DLHC_Z_SINGLE_CLICK ((uint8_t)0x10) /*!< Z single click IRQs */
#define LSM303DLHC_Y_DOUBLE_CLICK ((uint8_t)0x08) /*!< Y double click IRQs */
#define LSM303DLHC_Y_SINGLE_CLICK ((uint8_t)0x04) /*!< Y single click IRQs */
#define LSM303DLHC_X_DOUBLE_CLICK ((uint8_t)0x02) /*!< X double click IRQs */
#define LSM303DLHC_X_SINGLE_CLICK ((uint8_t)0x01) /*!< X single click IRQs */
/**
* @}
*/
/** @defgroup Acc_INT1_Interrupt_status
* @{
*/
#define LSM303DLHC_INT1INTERRUPT_DISABLE ((uint8_t)0x00)
#define LSM303DLHC_INT1INTERRUPT_ENABLE ((uint8_t)0x80)
/**
* @}
*/
/** @defgroup Acc_INT1_Interrupt_ActiveEdge
* @{
*/
#define LSM303DLHC_INT1INTERRUPT_LOW_EDGE ((uint8_t)0x20)
#define LSM303DLHC_INT1INTERRUPT_HIGH_EDGE ((uint8_t)0x00)
/**
* @}
*/
/** @defgroup Mag_Data_Rate
* @{
*/
#define LSM303DLHC_ODR_0_75_HZ ((uint8_t) 0x00) /*!< Output Data Rate = 0.75 Hz */
#define LSM303DLHC_ODR_1_5_HZ ((uint8_t) 0x04) /*!< Output Data Rate = 1.5 Hz */
#define LSM303DLHC_ODR_3_0_HZ ((uint8_t) 0x08) /*!< Output Data Rate = 3 Hz */
#define LSM303DLHC_ODR_7_5_HZ ((uint8_t) 0x0C) /*!< Output Data Rate = 7.5 Hz */
#define LSM303DLHC_ODR_15_HZ ((uint8_t) 0x10) /*!< Output Data Rate = 15 Hz */
#define LSM303DLHC_ODR_30_HZ ((uint8_t) 0x14) /*!< Output Data Rate = 30 Hz */
#define LSM303DLHC_ODR_75_HZ ((uint8_t) 0x18) /*!< Output Data Rate = 75 Hz */
#define LSM303DLHC_ODR_220_HZ ((uint8_t) 0x1C) /*!< Output Data Rate = 220 Hz */
/**
* @}
*/
/** @defgroup Mag_Full_Scale
* @{
*/
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = +/-1.3 Gauss */
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = +/-1.9 Gauss */
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = +/-2.5 Gauss */
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = +/-4.0 Gauss */
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = +/-4.7 Gauss */
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = +/-5.6 Gauss */
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = +/-8.1 Gauss */
/**
* @}
*/
/**
* @defgroup Magnetometer_Sensitivity
* @{
*/
#define LSM303DLHC_M_SENSITIVITY_XY_1_3Ga 1100 /*!< magnetometer X Y axes sensitivity for 1.3 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_1_9Ga 855 /*!< magnetometer X Y axes sensitivity for 1.9 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_2_5Ga 670 /*!< magnetometer X Y axes sensitivity for 2.5 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_4Ga 450 /*!< magnetometer X Y axes sensitivity for 4 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_4_7Ga 400 /*!< magnetometer X Y axes sensitivity for 4.7 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_5_6Ga 330 /*!< magnetometer X Y axes sensitivity for 5.6 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_XY_8_1Ga 230 /*!< magnetometer X Y axes sensitivity for 8.1 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_1_3Ga 980 /*!< magnetometer Z axis sensitivity for 1.3 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_1_9Ga 760 /*!< magnetometer Z axis sensitivity for 1.9 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_2_5Ga 600 /*!< magnetometer Z axis sensitivity for 2.5 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_4Ga 400 /*!< magnetometer Z axis sensitivity for 4 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_4_7Ga 355 /*!< magnetometer Z axis sensitivity for 4.7 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_5_6Ga 295 /*!< magnetometer Z axis sensitivity for 5.6 Ga full scale [LSB/Ga] */
#define LSM303DLHC_M_SENSITIVITY_Z_8_1Ga 205 /*!< magnetometer Z axis sensitivity for 8.1 Ga full scale [LSB/Ga] */
/**
* @}
*/
/** @defgroup Mag_Working_Mode
* @{
*/
#define LSM303DLHC_CONTINUOS_CONVERSION ((uint8_t) 0x00) /*!< Continuous-Conversion Mode */
#define LSM303DLHC_SINGLE_CONVERSION ((uint8_t) 0x01) /*!< Single-Conversion Mode */
#define LSM303DLHC_SLEEP ((uint8_t) 0x02) /*!< Sleep Mode */
/**
* @}
*/
/** @defgroup Mag_Temperature_Sensor
* @{
*/
#define LSM303DLHC_TEMPSENSOR_ENABLE ((uint8_t) 0x80) /*!< Temp sensor Enable */
#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */
bool lsm303dlhcAccDetect(accDev_t *acc);

49
src/main/drivers/accgyro/accgyro_mma845x.c
View file

@ -79,8 +79,7 @@
#define MMA8452_CTRL_REG1_LNOISE 0x04
#define MMA8452_CTRL_REG1_ACTIVE 0x01
static uint8_t device_id;
/*
static inline void mma8451ConfigureInterrupt(void)
{
#ifdef NAZE
@ -91,22 +90,22 @@ static inline void mma8451ConfigureInterrupt(void)
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
#endif
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
busWrite(acc->busDev, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
busWrite(acc->busDev, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
busWrite(acc->busDev, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}
*/
static void mma8452Init(accDev_t *acc)
{
busWrite(acc->busDev, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
busWrite(acc->busDev, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
busWrite(acc->busDev, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);
busWrite(acc->busDev, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes
// mma8451ConfigureInterrupt();
mma8451ConfigureInterrupt();
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
busWrite(acc->busDev, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
acc->acc_1G = 256;
}
@ -115,7 +114,7 @@ static bool mma8452Read(accDev_t *acc)
{
uint8_t buf[6];
if (!i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) {
if (!busReadBuf(acc->busDev, MMA8452_OUT_X_MSB, buf, 6)) {
return false;
}
@ -126,17 +125,31 @@ static bool mma8452Read(accDev_t *acc)
return true;
}
bool mma8452Detect(accDev_t *acc)
static bool deviceDetect(busDevice_t * busDev)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
ack = busRead(busDev, MMA8452_WHO_AM_I, &sig);
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
return false;
acc->initFn = mma8452Init;
acc->readFn = mma8452Read;
device_id = sig;
return true;
}
bool mma8452Detect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MMA8452, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!deviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = mma8452Init;
acc->readFn = mma8452Read;
return true;
}

277
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -41,164 +41,57 @@
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu3050.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_mpu.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
mpuResetFnPtr mpuResetFn;
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
#define MPU_ADDRESS 0x68
#define MPU_INQUIRY_MASK 0x7E
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool ack;
UNUSED(ack);
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) {
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) {
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else {
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) {
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else {
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
}
}
}
/*
* Gyro interrupt service routine
*/
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAtUs = 0;
const uint32_t nowUs = micros();
debug[0] = (uint16_t)(nowUs - lastCalledAtUs);
lastCalledAtUs = nowUs;
#endif
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
if (gyro->updateFn) {
gyro->updateFn(gyro);
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
const uint32_t now2Us = micros();
debug[1] = (uint16_t)(now2Us - nowUs);
#endif
}
#endif
static void mpuIntExtiInit(gyroDev_t *gyro)
void mpuIntExtiInit(gyroDev_t *gyro)
{
if (!gyro->mpuIntExtiConfig) {
if (!gyro->busDev->irqPin) {
return;
}
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = IORead(mpuIntIO);
uint8_t status = IORead(gyro->busDev->irqPin);
if (status) {
return;
}
#endif
#if defined (STM32F7)
IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
IOInit(gyro->busDev->irqPin, OWNER_MPU, RESOURCE_EXTI, 0);
EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
EXTIConfig(gyro->busDev->irqPin, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
#else
IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
IOInit(gyro->busDev->irqPin, OWNER_MPU, RESOURCE_EXTI, 0);
IOConfigGPIO(gyro->busDev->irqPin, IOCFG_IN_FLOATING);
EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
EXTIConfig(gyro->busDev->irqPin, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(gyro->busDev->irqPin, true);
#endif
#endif
}
bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data)
{
UNUSED(bus);
const bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
return ack;
}
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
UNUSED(bus);
const bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
return ack;
}
bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
gyro->updateFn = updateFn;
}
}
bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, gyro->mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = busReadBuf(gyro->busDev, gyro->devConfig.mpu.gyroReadXRegister, data, 6);
if (!ack) {
return false;
}
@ -210,6 +103,22 @@ bool mpuGyroRead(gyroDev_t *gyro)
return true;
}
bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = busReadBuf(acc->busDev, MPU_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool mpuCheckDataReady(gyroDev_t* gyro)
{
bool ret;
@ -222,133 +131,11 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
return ret;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
/*
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
uint8_t sensor = MPU_NONE;
UNUSED(sensor);
#ifdef USE_GYRO_SPI_MPU6000
#ifdef MPU6000_CS_PIN
gyro->bus.busdev.spi.csnPin = gyro->bus.busdev.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.busdev.spi.csnPin;
#endif
sensor = mpu6000SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6500
#ifdef MPU6500_CS_PIN
gyro->bus.busdev.spi.csnPin = gyro->bus.busdev.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.busdev.spi.csnPin;
#endif
sensor = mpu6500SpiDetect(&gyro->bus);
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
#ifdef MPU9250_CS_PIN
gyro->bus.busdev.spi.csnPin = gyro->bus.busdev.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.busdev.spi.csnPin;
#endif
sensor = mpu9250SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20608
#ifdef ICM20608_CS_PIN
gyro->bus.busdev.spi.csnPin = gyro->bus.busdev.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20608_CS_PIN)) : gyro->bus.busdev.spi.csnPin;
#endif
sensor = icm20608SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
mpuDetectionResult.sensor = ICM_20608_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.readFn = icm20608SpiReadRegister;
mpuConfiguration.writeFn = icm20608SpiWriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
#ifdef ICM20689_CS_PIN
gyro->bus.busdev.spi.csnPin = gyro->bus.busdev.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.busdev.spi.csnPin;
#endif
sensor = icm20689SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister;
return true;
}
#endif
return false;
}
#endif
void mpuDetect(gyroDev_t *gyro)
{
// MPU datasheet specifies 30ms.
delay(35);
uint8_t sig = 0;
#ifdef USE_I2C
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, 1, &sig);
#else
bool ack = false;
#endif
if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
detectSPISensorsAndUpdateDetectionResult(gyro);
#endif
return;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult;
ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
gyro->updateFn = updateFn;
}
}
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}
*/

49
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -20,17 +20,11 @@
#include "drivers/exti.h"
#include "drivers/sensor.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_USES_SPI
#endif
#define MPU_I2C_ADDRESS 0x68
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU3050, 6000 and 6050
#define MPU6000_WHO_AM_I_CONST (0x68)
#define MPU6500_WHO_AM_I_CONST (0x70)
@ -136,16 +130,7 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void (*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
@ -177,37 +162,9 @@ enum accel_fsr_e {
NUM_ACCEL_FSR
};
typedef enum {
MPU_NONE,
MPU_3050,
MPU_60x0,
MPU_60x0_SPI,
MPU_65xx_I2C,
MPU_65xx_SPI,
MPU_9250_SPI,
ICM_20601_SPI,
ICM_20602_SPI,
ICM_20608_SPI,
ICM_20649_SPI,
ICM_20679_SPI,
ICM_20689_SPI
} mpuSensor_e;
typedef enum {
MPU_HALF_RESOLUTION,
MPU_FULL_RESOLUTION
} mpu6050Resolution_e;
typedef struct mpuDetectionResult_s {
mpuSensor_e sensor;
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data);
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
void mpuIntExtiInit(struct gyroDev_s *gyro);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro);

67
src/main/drivers/accgyro/accgyro_mpu3050.c
View file

@ -16,6 +16,7 @@
*/
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#include "platform.h"
@ -32,6 +33,8 @@
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu3050.h"
#ifdef USE_GYRO_MPU3050
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
@ -44,25 +47,38 @@
#define MPU3050_DLPF_188HZ 0x01
#define MPU3050_DLPF_256HZ 0x00
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
#define MPU_INQUIRY_MASK 0x7E
static void mpu3050Init(gyroDev_t *gyro)
{
bool ack;
busDevice_t * busDev = gyro->busDev;
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
if (!ack)
ack = busWrite(busDev, MPU3050_SMPLRT_DIV, 0);
if (!ack) {
failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
busWrite(busDev, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
busWrite(busDev, MPU3050_INT_CFG, 0);
busWrite(busDev, MPU3050_USER_CTRL, MPU3050_USER_RESET);
busWrite(busDev, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
/*
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
uint8_t buf[2];
@ -74,19 +90,46 @@ static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
return true;
}
*/
static bool deviceDetect(busDevice_t * busDev)
{
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (int retry = 0; retry < 5; retry++) {
uint8_t inquiryResult;
delay(150);
bool ack = busRead(busDev, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
return true;
}
};
return false;
}
bool mpu3050Detect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_3050) {
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6000, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!deviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
gyro->devConfig.mpu.gyroReadXRegister = MPU3050_GYRO_OUT;
gyro->initFn = mpu3050Init;
gyro->readFn = mpuGyroRead;
gyro->temperatureFn = mpu3050ReadTemperature;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

9
src/main/drivers/accgyro/accgyro_mpu3050.h
View file

@ -17,13 +17,4 @@
#pragma once
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
bool mpu3050Detect(gyroDev_t *gyro);

222
src/main/drivers/accgyro/accgyro_mpu6000.c Normal file
View file

@ -0,0 +1,222 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Authors:
* Dominic Clifton - Cleanflight implementation
* John Ihlein - Initial FF32 code
* Konstantin Sharlaimov - busDevice refactoring
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/gyro_sync.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6000.h"
#if (defined(USE_GYRO_MPU6000) || defined(USE_ACC_MPU6000))
// Bits
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define BIT_I2C_IF_DIS 0x10
#define BIT_GYRO 3
#define BIT_ACC 2
#define BIT_TEMP 1
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
busDevice_t * busDev = gyro->busDev;
mpuIntExtiInit(gyro);
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
// Device Reset
busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
busWrite(busDev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150);
// Clock Source PPL with Z axis gyro reference
busWrite(busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Disable Primary I2C Interface
busWrite(busDev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
busWrite(busDev, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
busWrite(busDev, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale
busWrite(busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
busWrite(busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
busWrite(busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
// Accel and Gyro DLPF Setting
busWrite(busDev, MPU_RA_CONFIG, gyro->lpf);
delayMicroseconds(1);
busSetSpeed(busDev, BUS_SPEED_FAST);
mpuGyroRead(gyro);
if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
static void mpu6000AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
}
bool mpu6000AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6000, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF6000) {
return false;
}
acc->initFn = mpu6000AccInit;
acc->readFn = mpuAccRead;
return true;
}
static bool mpu6000DeviceDetect(busDevice_t * busDev)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
busWrite(busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
do {
delay(150);
busRead(busDev, MPU_RA_WHO_AM_I, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
busRead(busDev, MPU_RA_PRODUCT_ID, &in);
/* look for a product ID we recognise */
switch (in) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
}
return false;
}
bool mpu6000GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6000, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!mpu6000DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
while(1);
return false;
}
busDeviceWriteScratchpad(gyro->busDev, 0xFFFF6000); // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
gyro->devConfig.mpu.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->initFn = mpu6000AccAndGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

7
src/main/drivers/barometer/barometer_spi_ms56xx.h → src/main/drivers/accgyro/accgyro_mpu6000.h Executable file → Normal file
View file

@ -17,6 +17,7 @@
#pragma once
void ms56xxSpiInit(void);
bool ms56xxSpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data);
bool ms56xxSpiWriteCommand(uint8_t reg, uint8_t data);
#include "drivers/sensor.h"
bool mpu6000AccDetect(accDev_t *acc);
bool mpu6000GyroDetect(gyroDev_t *gyro);

202
src/main/drivers/accgyro/accgyro_mpu6050.c Normal file → Executable file
View file

@ -1,37 +1,42 @@
/*
* This file is part of Cleanflight.
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Authors:
* Dominic Clifton - Cleanflight implementation
* John Ihlein - Initial FF32 code
* Konstantin Sharlaimov - busDevice refactoring
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/nvic.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/gpio.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/gyro_sync.h"
#include "drivers/sensor.h"
@ -39,78 +44,175 @@
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050)
// MPU6050, Standard address 0x68
// MPU_INT on PB13 on rev4 Naze32 hardware
#define MPU6050_ADDRESS 0x68
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_INQUIRY_MASK 0x7E
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
typedef enum {
MPU6050_NONE,
MPU6050_HALF_RESOLUTION,
MPU6050_FULL_RESOLUTION
} mpuDetectionResult_e;
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static bool mpu6050InitDone = false;
static void mpu6050AccAndGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
if (!mpu6050InitDone) {
// Device Reset
busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
// Clock Source PPL with Z axis gyro reference
busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
busWrite(gyro->busDev, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Accel and Gyro DLPF Setting
busWrite(gyro->busDev, MPU_RA_CONFIG, gyro->lpf);
delayMicroseconds(1);
// Gyro +/- 2000 DPS Full Scale
busWrite(gyro->busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
busWrite(gyro->busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(gyro->busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
mpu6050InitDone = true;
}
busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
}
static void mpu6050AccInit(accDev_t *acc)
{
switch (acc->mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 8;
break;
case MPU_FULL_RESOLUTION:
uint32_t magic = busDeviceReadScratchpad(acc->busDev);
if (magic == 0xFFFF6050) {
acc->acc_1G = 512 * 8;
break;
}
else {
acc->acc_1G = 256 * 8;
}
}
bool mpu6050AccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != MPU_60x0) {
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6050, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
uint32_t magic = busDeviceReadScratchpad(acc->busDev);
if (magic == 0x00006050 || magic == 0xFFFF6050) {
acc->initFn = mpu6050AccInit;
acc->readFn = mpuAccRead;
acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
return true;
}
static void mpu6050GyroInit(gyroDev_t *gyro)
return false;
}
static bool mpu6050DeviceDetect(busDevice_t * dev)
{
mpuGyroInit(gyro);
uint8_t in;
uint8_t readBuffer[6];
uint8_t attemptsRemaining = 5;
bool ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
do {
delay(150);
#ifdef USE_MPU_DATA_READY_SIGNAL
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
UNUSED(ack);
busRead(dev, MPU_RA_WHO_AM_I, &in);
in &= MPU_INQUIRY_MASK;
if (in == MPUx0x0_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
busReadBuf(dev, MPU_RA_XA_OFFS_H, readBuffer, 6);
uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
return MPU6050_HALF_RESOLUTION;
} else if (revision == 2) {
return MPU6050_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) {
return MPU6050_FULL_RESOLUTION;
} else {
return MPU6050_NONE;
}
} else {
uint8_t productId;
busRead(dev, MPU_RA_PRODUCT_ID, &productId);
revision = productId & 0x0F;
if (!revision) {
return MPU6050_NONE;
} else if (revision == 4) {
return MPU6050_HALF_RESOLUTION;
} else {
return MPU6050_FULL_RESOLUTION;
}
}
return MPU6050_NONE;
}
bool mpu6050GyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6050, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
gyro->initFn = mpu6050GyroInit;
mpuDetectionResult_e res = mpu6050DeviceDetect(gyro->busDev);
if (res == MPU6050_NONE) {
busDeviceDeInit(gyro->busDev);
return false;
}
busDeviceWriteScratchpad(gyro->busDev, res == MPU6050_FULL_RESOLUTION ? 0xFFFF6050 : 0x00006050);
gyro->devConfig.mpu.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->initFn = mpu6050AccAndGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

10
src/main/drivers/accgyro/accgyro_mpu6050.h Normal file → Executable file
View file

@ -1,21 +1,23 @@
/*
* This file is part of Cleanflight.
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/sensor.h"
bool mpu6050AccDetect(accDev_t *acc);
bool mpu6050GyroDetect(gyroDev_t *gyro);

133
src/main/drivers/accgyro/accgyro_mpu6500.c Normal file → Executable file
View file

@ -1,18 +1,18 @@
/*
* This file is part of Cleanflight.
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
@ -35,14 +35,27 @@
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
void mpu6500AccInit(accDev_t *acc)
#if defined(USE_GYRO_MPU6500) || defined(USE_ACC_MPU6500)
#define MPU6500_BIT_RESET (0x80)
#define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4)
#define MPU6500_BIT_BYPASS_EN (1 << 0)
#define MPU6500_BIT_I2C_IF_DIS (1 << 4)
#define MPU6500_BIT_RAW_RDY_EN (0x01)
static void mpu6500AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
}
bool mpu6500AccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != MPU_65xx_I2C) {
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6500, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF6500) {
return false;
}
@ -52,67 +65,107 @@ bool mpu6500AccDetect(accDev_t *acc)
return true;
}
void mpu6500GyroInit(gyroDev_t *gyro)
static void mpu6500AccAndGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
busDevice_t * dev = gyro->busDev;
mpuIntExtiInit(gyro);
#ifdef NAZE
// FIXME target specific code in driver code.
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
busWrite(dev, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
busWrite(dev, MPU_RA_SIGNAL_PATH_RESET, 0x07); // BIT_GYRO | BIT_ACC | BIT_TEMP
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
busWrite(dev, MPU_RA_PWR_MGMT_1, 0);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
delay(100);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
busWrite(dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
busWrite(dev, MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
busWrite(dev, MPU_RA_CONFIG, gyro->lpf);
delay(15);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider
busWrite(dev, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#endif
busWrite(dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
busWrite(dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delay(15);
#endif
busSetSpeed(dev, BUS_SPEED_FAST);
}
static bool mpu6500DeviceDetect(busDevice_t * dev)
{
uint8_t tmp;
uint8_t attemptsRemaining = 5;
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
do {
delay(150);
busRead(dev, MPU_RA_WHO_AM_I, &tmp);
switch (tmp) {
case MPU6500_WHO_AM_I_CONST:
case ICM20608G_WHO_AM_I_CONST:
case ICM20602_WHO_AM_I_CONST:
case ICM20689_WHO_AM_I_CONST:
// Compatible chip detected
return true;
default:
// Retry detection
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
return false;
}
bool mpu6500GyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6500, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
gyro->initFn = mpu6500GyroInit;
if (!mpu6500DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
busDeviceWriteScratchpad(gyro->busDev, 0xFFFF6500); // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
gyro->devConfig.mpu.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->initFn = mpu6500AccAndGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

17
src/main/drivers/accgyro/accgyro_mpu6500.h Normal file → Executable file
View file

@ -1,30 +1,21 @@
/*
* This file is part of Cleanflight.
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define MPU6500_BIT_RESET (0x80)
#define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4)
#define MPU6500_BIT_BYPASS_EN (1 << 0)
#define MPU6500_BIT_I2C_IF_DIS (1 << 4)
#define MPU6500_BIT_RAW_RDY_EN (0x01)
bool mpu6500AccDetect(accDev_t *acc);
bool mpu6500GyroDetect(gyroDev_t *gyro);
void mpu6500AccInit(accDev_t *acc);
void mpu6500GyroInit(gyroDev_t *gyro);

168
src/main/drivers/accgyro/accgyro_mpu9250.c Executable file
View file

@ -0,0 +1,168 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/gpio.h"
#include "drivers/gyro_sync.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu9250.h"
#if defined(USE_GYRO_MPU9250) || defined(USE_ACC_MPU9250)
#define MPU9250_BIT_RESET (0x80)
#define MPU9250_BIT_INT_ANYRD_2CLEAR (1 << 4)
#define MPU9250_BIT_BYPASS_EN (1 << 0)
#define MPU9250_BIT_I2C_IF_DIS (1 << 4)
#define MPU9250_BIT_RAW_RDY_EN (0x01)
static void mpu9250AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
}
bool mpu9250AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU9250, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF9250) {
return false;
}
acc->initFn = mpu9250AccInit;
acc->readFn = mpuAccRead;
return true;
}
static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
{
busDevice_t * dev = gyro->busDev;
mpuIntExtiInit(gyro);
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(100);
busWrite(dev, MPU_RA_SIGNAL_PATH_RESET, 0x07); // BIT_GYRO | BIT_ACC | BIT_TEMP
delay(100);
busWrite(dev, MPU_RA_PWR_MGMT_1, 0);
delay(100);
busWrite(dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
busWrite(dev, MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15);
busWrite(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delay(15);
busWrite(dev, MPU_RA_CONFIG, gyro->lpf);
delay(15);
busWrite(dev, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider
delay(100);
// Data ready interrupt configuration
busWrite(dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delay(15);
#endif
busSetSpeed(dev, BUS_SPEED_FAST);
}
static bool mpu9250DeviceDetect(busDevice_t * dev)
{
uint8_t tmp;
uint8_t attemptsRemaining = 5;
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
do {
delay(150);
busRead(dev, MPU_RA_WHO_AM_I, &tmp);
switch (tmp) {
case MPU9250_WHO_AM_I_CONST:
// Compatible chip detected
return true;
default:
// Retry detection
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
return false;
}
bool mpu9250GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU9250, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!mpu9250DeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
busDeviceWriteScratchpad(gyro->busDev, 0xFFFF9250); // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
gyro->devConfig.mpu.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->initFn = mpu9250AccAndGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
return true;
}
#endif

15
src/main/drivers/barometer/barometer_spi_bmp280.h → src/main/drivers/accgyro/accgyro_mpu9250.h Normal file → Executable file
View file

@ -1,24 +1,21 @@
/*
* This file is part of Betaflight.
* This file is part of INAV.
*
* Betaflight is free software: you can redistribute it and/or modify
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Betaflight is distributed in the hope that it will be useful,
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
void bmp280SpiInit(void);
bool bmp280ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
bool bmp280WriteRegister(uint8_t reg, uint8_t data);
void bmp280_spi_start_up(void);
void bmp280_spi_get_up(void);
bool mpu9250AccDetect(accDev_t *acc);
bool mpu9250GyroDetect(gyroDev_t *gyro);

282
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file

@ -1,282 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Authors:
* Dominic Clifton - Cleanflight implementation
* John Ihlein - Initial FF32 code
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus_spi.h"
#include "drivers/gyro_sync.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000)
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi6000InitDone = false;
// Bits
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_2G 0x00
#define BITS_FS_4G 0x08
#define BITS_FS_8G 0x10
#define BITS_FS_16G 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
#define BIT_GYRO 3
#define BIT_ACC 2
#define BIT_TEMP 1
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin)
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU6000(bus->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000(bus->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6000(bus->busdev.spi.csnPin);
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000(bus->busdev.spi.csnPin);
return true;
}
void mpu6000SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu6000AccAndGyroInit(gyro);
spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
mpuGyroRead(gyro);
if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
}
bool mpu6000SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
IOInit(bus->busdev.spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->busdev.spi.csnPin, SPI_IO_CS_CFG);
spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
do {
delay(150);
mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
// verify product revision
switch (in) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
}
return false;
}
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
if (mpuSpi6000InitDone) {
return;
}
spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Device Reset
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150);
// Clock Source PPL with Z axis gyro reference
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Disable Primary I2C Interface
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
spiSetSpeed(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
delayMicroseconds(1);
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
acc->initFn = mpu6000SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
gyro->initFn = mpu6000SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}
#endif

24
src/main/drivers/accgyro/accgyro_spi_mpu6000.h
View file

@ -1,24 +0,0 @@
#pragma once
#include "drivers/sensor.h"
#define MPU6000_CONFIG 0x1A
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
bool mpu6000SpiDetect(const busDevice_t *bus);
bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

168
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View file

@ -1,168 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/bus_spi.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin)
#define BIT_SLEEP 0x40
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU6500(bus->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
spiTransferByte(MPU6500_SPI_INSTANCE, data);
DISABLE_MPU6500(bus->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6500(bus->busdev.spi.csnPin);
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6500(bus->busdev.spi.csnPin);
return true;
}
static void mpu6500SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
IOInit(bus->busdev.spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->busdev.spi.csnPin, SPI_IO_CS_CFG);
spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
hardwareInitialised = true;
}
uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{
mpu6500SpiInit(bus);
mpu6500SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
for (int attempts = 0; attempts < 5; attempts++) {
uint8_t tmp;
delay(150);
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
switch (tmp) {
case MPU6500_WHO_AM_I_CONST:
return MPU_65xx_SPI;
case MPU9250_WHO_AM_I_CONST:
return MPU_9250_SPI;
case ICM20608G_WHO_AM_I_CONST:
return ICM_20608_SPI;
case ICM20602_WHO_AM_I_CONST:
return ICM_20602_SPI;
case ICM20689_WHO_AM_I_CONST:
return ICM_20689_SPI;
}
}
return MPU_NONE;
}
void mpu6500SpiAccInit(accDev_t *acc)
{
mpu6500AccInit(acc);
}
bool mpu6500SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI &&
acc->mpuDetectionResult.sensor != MPU_9250_SPI &&
acc->mpuDetectionResult.sensor != ICM_20608_SPI &&
acc->mpuDetectionResult.sensor != ICM_20602_SPI &&
acc->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
}
acc->initFn = mpu6500SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
void mpu6500SpiGyroInit(gyroDev_t *gyro)
{
spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
delayMicroseconds(1);
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
delay(100);
spiSetSpeed(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
delayMicroseconds(1);
}
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_65xx_SPI &&
gyro->mpuDetectionResult.sensor != MPU_9250_SPI &&
gyro->mpuDetectionResult.sensor != ICM_20608_SPI &&
gyro->mpuDetectionResult.sensor != ICM_20602_SPI &&
gyro->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
}
gyro->initFn = mpu6500SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}
#endif

234
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View file

@ -1,234 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Authors:
* Dominic Clifton - Cleanflight implementation
* John Ihlein - Initial FF32 code
* Kalyn Doerr (RS2K) - Robust rewrite
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/exti.h"
#include "drivers/bus_spi.h"
#include "drivers/gyro_sync.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
static bool mpuSpi9250InitDone = false;
#define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin)
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250(bus->busdev.spi.csnPin);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250(bus->busdev.spi.csnPin);
return true;
}
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250(bus->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250(bus->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU9250(bus->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
spiTransferByte(MPU9250_SPI_INSTANCE, data);
DISABLE_MPU9250(bus->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
uint8_t attemptsRemaining = 20;
do {
uint8_t in;
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
return false;
}
void mpu9250SpiResetGyro(void)
{
// Device Reset
#ifdef MPU9250_CS_PIN
busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } };
mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(150);
#endif
}
static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
{
if (mpuSpi9250InitDone) {
return;
}
spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
//Fchoice_b defaults to 00 which makes fchoice 11
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (gyro->lpf == 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (gyro->lpf < 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (gyro->lpf > 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#if defined(USE_MPU_DATA_READY_SIGNAL)
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif
spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
mpuSpi9250InitDone = true; //init done
}
bool mpu9250SpiDetect(const busDevice_t *bus)
{
/* not the best place for this - should really have an init method */
IOInit(bus->busdev.spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->busdev.spi.csnPin, SPI_IO_CS_CFG);
spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
uint8_t in;
mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU9250_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
return false;
}
} while (attemptsRemaining--);
spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
return true;
}
static void mpu9250SpiAccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 8;
}
bool mpu9250SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
}
acc->initFn = mpu9250SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
static void mpu9250SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu9250AccAndGyroInit(gyro);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
spiSetSpeed(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
mpuGyroRead(gyro);
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
}
gyro->initFn = mpu9250SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}

36
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View file

@ -1,36 +0,0 @@
#pragma once
#include "drivers/sensor.h"
#define mpu9250_CONFIG 0x1A
/* We should probably use these. :)
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
*/
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU9250_BIT_RESET (0x80)
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
void mpu9250SpiResetGyro(void);
bool mpu9250SpiDetect(const busDevice_t *bus);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

12
src/main/drivers/barometer/barometer.h
View file

@ -17,10 +17,14 @@
#pragma once
typedef void (*baroOpFuncPtr)(void); // baro start operation
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
#include "drivers/bus.h"
struct baroDev_s;
typedef bool (*baroOpFuncPtr)(struct baroDev_s * baro);
typedef bool (*baroCalculateFuncPtr)(struct baroDev_s * baro, int32_t *pressure, int32_t *temperature);
typedef struct baroDev_s {
busDevice_t * busDev;
uint16_t ut_delay;
uint16_t up_delay;
baroOpFuncPtr start_ut;
@ -29,7 +33,3 @@ typedef struct baroDev_s {
baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baroDev_t;
#ifndef BARO_I2C_INSTANCE
#define BARO_I2C_INSTANCE I2C_DEVICE
#endif

289
src/main/drivers/barometer/barometer_bmp085.c
View file

@ -21,36 +21,23 @@
#include <platform.h>
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/barometer/barometer.h"
#include "drivers/gpio.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/nvic.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/barometer/barometer_bmp085.h"
#ifdef BARO
#if defined(USE_BARO_BMP085)
#if defined(BARO_EOC_GPIO)
static IO_t eocIO;
static bool isConversionComplete = false;
static bool isEOCConnected = true;
// EXTI14 for BMP085 End of Conversion Interrupt
void bmp085_extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
isConversionComplete = true;
}
bool bmp085TestEOCConnected(const bmp085Config_t *config);
# endif
#define BMP085_I2C_ADDR 0x77
#define BMP085_CHIP_ID 0x55
typedef struct {
int16_t ac1;
@ -68,15 +55,10 @@ typedef struct {
typedef struct {
bmp085_smd500_calibration_param_t cal_param;
uint8_t mode;
uint8_t chip_id, ml_version, al_version;
uint8_t dev_addr;
int32_t param_b5;
int16_t oversampling_setting;
} bmp085_t;
#define BMP085_I2C_ADDR 0x77
#define BMP085_CHIP_ID 0x55
#define BOSCH_PRESSURE_BMP085 85
#define BMP085_CHIP_ID_REG 0xD0
#define BMP085_VERSION_REG 0xD1
@ -103,8 +85,8 @@ typedef struct {
#define BMP085_AL_VERSION__MSK 0xF0
#define BMP085_AL_VERSION__REG BMP085_VERSION_REG
#define BMP085_GET_BITSLICE(regvar, bitname) (regvar & bitname##__MSK) >> bitname##__POS
#define BMP085_SET_BITSLICE(regvar, bitname, val) (regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK)
#define BMP085_GET_BITSLICE(regvar, bitname) ((regvar & bitname##__MSK) >> bitname##__POS)
#define BMP085_SET_BITSLICE(regvar, bitname, val) ((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
#define SMD500_PARAM_MG 3038 //calibration parameter
#define SMD500_PARAM_MH -7357 //calibration parameter
@ -115,111 +97,30 @@ STATIC_UNIT_TESTED bmp085_t bmp085;
#define UT_DELAY 6000 // 1.5ms margin according to the spec (4.5ms T conversion time)
#define UP_DELAY 27000 // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P conversion time with OSS=3)
static bool bmp085InitDone = false;
STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement
static void bmp085_get_cal_param(void);
static void bmp085_start_ut(void);
static void bmp085_get_ut(void);
static void bmp085_start_up(void);
static void bmp085_get_up(void);
static int32_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_pressure(uint32_t up);
STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature);
static IO_t xclrIO;
#ifdef BARO_XCLR_PIN
#define BMP085_OFF IOLo(xclrIO);
#define BMP085_ON IOHi(xclrIO);
#else
#define BMP085_OFF
#define BMP085_ON
#endif
void bmp085InitXclrIO(const bmp085Config_t *config)
static void bmp085_get_cal_param(baroDev_t *baro)
{
if (!xclrIO && config && config->xclrIO) {
xclrIO = IOGetByTag(config->xclrIO);
IOInit(xclrIO, OWNER_BARO, RESOURCE_OUTPUT, 0);
IOConfigGPIO(xclrIO, IOCFG_OUT_PP);
}
}
uint8_t data[BMP085_PROM_DATA__LEN];
busReadBuf(baro->busDev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN);
void bmp085Disable(const bmp085Config_t *config)
{
bmp085InitXclrIO(config);
BMP085_OFF;
}
/* parameters AC1-AC6 */
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
bmp085.cal_param.ac2 = (data[2] << 8) | data[3];
bmp085.cal_param.ac3 = (data[4] << 8) | data[5];
bmp085.cal_param.ac4 = (data[6] << 8) | data[7];
bmp085.cal_param.ac5 = (data[8] << 8) | data[9];
bmp085.cal_param.ac6 = (data[10] << 8) | data[11];
#define DETECTION_MAX_RETRY_COUNT 5
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{
uint8_t data;
bool ack;
#if defined(BARO_EOC_GPIO)
IO_t eocIO = IO_NONE;
#endif
/* parameters B1,B2 */
bmp085.cal_param.b1 = (data[12] << 8) | data[13];
bmp085.cal_param.b2 = (data[14] << 8) | data[15];
if (bmp085InitDone)
return true;
bmp085InitXclrIO(config);
BMP085_ON; // enable baro
#if defined(BARO_EOC_GPIO) && defined(USE_EXTI)
if (config && config->eocIO) {
eocIO = IOGetByTag(config->eocIO);
// EXTI interrupt for barometer EOC
IOInit(eocIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
IOConfigGPIO(eocIO, Mode_IN_FLOATING);
EXTIHandlerInit(&bmp085_extiCallbackRec, bmp085_extiHandler);
EXTIConfig(eocIO, &bmp085_extiCallbackRec, NVIC_PRIO_BARO_EXTI, EXTI_Trigger_Rising);
EXTIEnable(eocIO, true);
}
#else
UNUSED(config);
#endif
delay(20); // datasheet says 10ms, we'll be careful and do 20.
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
if (ack) {
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3;
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
baro->ut_delay = UT_DELAY;
baro->up_delay = UP_DELAY;
baro->start_ut = bmp085_start_ut;
baro->get_ut = bmp085_get_ut;
baro->start_up = bmp085_start_up;
baro->get_up = bmp085_get_up;
baro->calculate = bmp085_calculate;
#if defined(BARO_EOC_GPIO)
isEOCConnected = bmp085TestEOCConnected(config);
#endif
bmp085InitDone = true;
return true;
}
}
}
#if defined(BARO_EOC_GPIO)
if (eocIO)
EXTIRelease(eocIO);
#endif
BMP085_OFF;
return false;
/* parameters MB,MC,MD */
bmp085.cal_param.mb = (data[16] << 8) | data[17];
bmp085.cal_param.mc = (data[18] << 8) | data[19];
bmp085.cal_param.md = (data[20] << 8) | data[21];
}
static int32_t bmp085_get_temperature(uint32_t ut)
@ -275,64 +176,49 @@ static int32_t bmp085_get_pressure(uint32_t up)
return pressure;
}
static void bmp085_start_ut(void)
static bool bmp085_start_ut(baroDev_t *baro)
{
#if defined(BARO_EOC_GPIO)
isConversionComplete = false;
#endif
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
bool ack = busWrite(baro->busDev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
return ack;
}
static void bmp085_get_ut(void)
static bool bmp085_get_ut(baroDev_t *baro)
{
uint8_t data[2];
#if defined(BARO_EOC_GPIO)
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
bool ack = busReadBuf(baro->busDev, BMP085_ADC_OUT_MSB_REG, data, 2);
if (ack) {
bmp085_ut = (data[0] << 8) | data[1];
}
static void bmp085_start_up(void)
return ack;
}
static bool bmp085_start_up(baroDev_t *baro)
{
uint8_t ctrl_reg_data;
ctrl_reg_data = BMP085_P_MEASURE + (bmp085.oversampling_setting << 6);
#if defined(BARO_EOC_GPIO)
isConversionComplete = false;
#endif
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
uint8_t ctrl_reg_data = BMP085_P_MEASURE + (bmp085.oversampling_setting << 6);
bool ack = busWrite(baro->busDev, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
return ack;
}
/** read out up for pressure conversion
depending on the oversampling ratio setting up can be 16 to 19 bit
\return up parameter that represents the uncompensated pressure value
*/
static void bmp085_get_up(void)
static bool bmp085_get_up(baroDev_t *baro)
{
uint8_t data[3];
bool ack = busReadBuf(baro->busDev, BMP085_ADC_OUT_MSB_REG, data, 3);
#if defined(BARO_EOC_GPIO)
// return old baro value if conversion time exceeds datasheet max when EOC is connected
if ((isEOCConnected) && (!isConversionComplete)) {
return;
}
#endif
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
>> (8 - bmp085.oversampling_setting);
if (ack) {
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting);
}
STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature)
return ack;
}
STATIC_UNIT_TESTED bool bmp085_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
{
UNUSED(baro);
int32_t temp, press;
temp = bmp085_get_temperature(bmp085_ut);
@ -341,48 +227,53 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature
*pressure = press;
if (temperature)
*temperature = temp;
}
static void bmp085_get_cal_param(void)
{
uint8_t data[BMP085_PROM_DATA__LEN];
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
/*parameters AC1-AC6*/
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
bmp085.cal_param.ac2 = (data[2] << 8) | data[3];
bmp085.cal_param.ac3 = (data[4] << 8) | data[5];
bmp085.cal_param.ac4 = (data[6] << 8) | data[7];
bmp085.cal_param.ac5 = (data[8] << 8) | data[9];
bmp085.cal_param.ac6 = (data[10] << 8) | data[11];
/*parameters B1,B2*/
bmp085.cal_param.b1 = (data[12] << 8) | data[13];
bmp085.cal_param.b2 = (data[14] << 8) | data[15];
/*parameters MB,MC,MD*/
bmp085.cal_param.mb = (data[16] << 8) | data[17];
bmp085.cal_param.mc = (data[18] << 8) | data[19];
bmp085.cal_param.md = (data[20] << 8) | data[21];
}
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config)
{
UNUSED(config);
if (!bmp085InitDone && eocIO) {
bmp085_start_ut();
delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
// conversion should have finished now so check if EOC is high
uint8_t status = IORead(eocIO);
if (status) {
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
uint8_t chipId = 0;
delay(10);
bool ack = busRead(busDev, BMP085_CHIP_ID__REG, &chipId);
if (ack && BMP085_GET_BITSLICE(chipId, BMP085_CHIP_ID) == BMP085_CHIP_ID) {
return true;
}
return false; // assume EOC is not connected
};
return false;
}
bool bmp085Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMP085, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
if (!deviceDetect(baro->busDev)) {
busDeviceDeInit(baro->busDev);
return false;
}
bmp085.oversampling_setting = 3;
bmp085_get_cal_param(baro); /* readout bmp085 calibparam structure */
baro->ut_delay = UT_DELAY;
baro->start_ut = bmp085_start_ut;
baro->get_ut = bmp085_get_ut;
baro->up_delay = UP_DELAY;
baro->start_up = bmp085_start_up;
baro->get_up = bmp085_get_up;
baro->calculate = bmp085_calculate;
return true;
}
#endif
#endif /* BARO */

16
src/main/drivers/barometer/barometer_bmp085.h
View file

@ -19,18 +19,4 @@
#include "drivers/io_types.h"
typedef struct bmp085Config_s {
ioTag_t xclrIO;
ioTag_t eocIO;
} bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro);
void bmp085Disable(const bmp085Config_t *config);
#if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config);
#endif
#ifdef UNIT_TEST
void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState);
#endif
bool bmp085Detect(baroDev_t *baro);

185
src/main/drivers/barometer/barometer_bmp280.c
View file

@ -20,16 +20,16 @@
#include <platform.h>
#include "build/build_config.h"
#include "drivers/barometer/barometer.h"
#include "build/debug.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp280.h"
#include "drivers/barometer/barometer_spi_bmp280.h"
#ifdef BARO
#if defined(USE_BARO_BMP280)
// BMP280, address 0x76
@ -49,98 +49,32 @@ typedef struct bmp280_calib_param_s {
int32_t t_fine; /* calibration t_fine data */
} bmp280_calib_param_t;
static uint8_t bmp280_chip_id = 0;
static bool bmp280InitDone = false;
STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
// uncompensated pressure and temperature
int32_t bmp280_up = 0;
int32_t bmp280_ut = 0;
static void bmp280_start_ut(void);
static void bmp280_get_ut(void);
#ifndef USE_BARO_SPI_BMP280
static void bmp280_start_up(void);
static void bmp280_get_up(void);
#endif
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
#define DETECTION_MAX_RETRY_COUNT 5
bool bmp280Detect(baroDev_t *baro)
static bool bmp280_start_ut(baroDev_t * baro)
{
if (bmp280InitDone)
return true;
delay(20);
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
#ifdef USE_BARO_SPI_BMP280
bmp280SpiInit();
bmp280ReadRegister(BMP280_CHIP_ID_REG, 1, &bmp280_chip_id);
if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
continue;
// read calibration
bmp280ReadRegister(BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
// set oversampling + power mode (forced), and start sampling
bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE);
//set filter setting
bmp280WriteRegister(BMP280_CONFIG_REG, BMP280_FILTER);
#else
bool ack = i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
if (!ack || bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
continue;
// read calibration
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
// set oversampling + power mode (forced), and start sampling
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
//set filter setting
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CONFIG_REG, BMP280_FILTER);
#endif
bmp280InitDone = true;
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0;
baro->get_ut = bmp280_get_ut;
baro->start_ut = bmp280_start_ut;
// only _up part is executed, and gets both temperature and pressure
baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
#ifdef USE_BARO_SPI_BMP280
baro->start_up = bmp280_spi_start_up;
baro->get_up = bmp280_spi_get_up;
#else
baro->start_up = bmp280_start_up;
baro->get_up = bmp280_get_up;
#endif
baro->calculate = bmp280_calculate;
UNUSED(baro);
return true;
}
return false;
}
static void bmp280_start_ut(void)
static bool bmp280_get_ut(baroDev_t * baro)
{
// dummy
UNUSED(baro);
return true;
}
static void bmp280_get_ut(void)
{
// dummy
}
#ifndef USE_BARO_SPI_BMP280
static void bmp280_start_up(void)
static bool bmp280_start_up(baroDev_t * baro)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
return true;
}
static void bmp280_get_up(void)
static bool bmp280_get_up(baroDev_t * baro)
{
uint8_t data[BMP280_DATA_FRAME_SIZE];
@ -149,28 +83,23 @@ static void bmp280_get_up(void)
static int32_t bmp280_ut_valid;
//read data from sensor
bool ack = i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
bool ack = busReadBuf(baro->busDev, BMP280_PRESSURE_MSB_REG, data, BMP280_DATA_FRAME_SIZE);
//check if pressure and temperature readings are valid, otherwise use previous measurements from the moment
if (ack) {
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
bmp280_up_valid = bmp280_up;
bmp280_ut_valid = bmp280_ut;
}
else
{
else {
//assign previous valid measurements
bmp280_up = bmp280_up_valid;
bmp280_ut = bmp280_ut_valid;
}
return ack;
}
#endif
// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
// t_fine carries fine temperature as global value
@ -197,8 +126,10 @@ static uint32_t bmp280_compensate_P(int32_t adc_P)
var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);
var1 = ((var1 * var1 * (int64_t)bmp280_cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280_cal.dig_P2) << 12);
var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280_cal.dig_P1) >> 33;
if (var1 == 0)
if (var1 == 0) {
return 0;
}
p = 1048576 - adc_P;
p = (((p << 31) - var2) * 3125) / var1;
var1 = (((int64_t)bmp280_cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
@ -207,18 +138,76 @@ static uint32_t bmp280_compensate_P(int32_t adc_P)
return (uint32_t)p;
}
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
STATIC_UNIT_TESTED bool bmp280_calculate(baroDev_t * baro, int32_t * pressure, int32_t * temperature)
{
// calculate
int32_t t;
uint32_t p;
t = bmp280_compensate_T(bmp280_ut);
p = bmp280_compensate_P(bmp280_up);
UNUSED(baro);
if (pressure)
int32_t t = bmp280_compensate_T(bmp280_ut);
uint32_t p = bmp280_compensate_P(bmp280_up);
if (pressure) {
*pressure = (int32_t)(p / 256);
if (temperature)
}
if (temperature) {
*temperature = t;
}
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
uint8_t chipId = 0;
delay(100);
bool ack = busRead(busDev, BMP280_CHIP_ID_REG, &chipId);
if (ack && chipId == BMP280_DEFAULT_CHIP_ID) {
return true;
}
};
return false;
}
bool bmp280Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMP280, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
if (!deviceDetect(baro->busDev)) {
busDeviceDeInit(baro->busDev);
return false;
}
// read calibration
busReadBuf(baro->busDev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, 24);
//set filter setting
busWrite(baro->busDev, BMP280_CONFIG_REG, BMP280_FILTER);
// set oversampling + power mode (forced), and start sampling
busWrite(baro->busDev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
baro->ut_delay = 0;
baro->get_ut = bmp280_get_ut;
baro->start_ut = bmp280_start_ut;
baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
baro->start_up = bmp280_start_up;
baro->get_up = bmp280_get_up;
baro->calculate = bmp280_calculate;
return true;
}
#endif

8
src/main/drivers/barometer/barometer_fake.c
View file

@ -22,6 +22,7 @@
#ifdef USE_FAKE_BARO
#include "common/utils.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_fake.h"
@ -30,16 +31,19 @@ static int32_t fakePressure;
static int32_t fakeTemperature;
static void fakeBaroStartGet(void)
static bool fakeBaroStartGet(baroDev_t * baro)
{
UNUSED(baro);
return true;
}
static void fakeBaroCalculate(int32_t *pressure, int32_t *temperature)
static bool fakeBaroCalculate(baroDev_t * baro, int32_t *pressure, int32_t *temperature)
{
if (pressure)
*pressure = fakePressure;
if (temperature)
*temperature = fakeTemperature;
return true;
}
void fakeBaroSet(int32_t pressure, int32_t temperature)

185
src/main/drivers/barometer/barometer_ms56xx.c
View file

@ -23,12 +23,13 @@
#include "build/build_config.h"
#include "common/utils.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/time.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_ms56xx.h"
#include "drivers/barometer/barometer_spi_ms56xx.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#if defined(USE_BARO_MS5607) || defined(USE_BARO_MS5611)
// MS56xx, Standard address 0x77
#define MS56XX_ADDR 0x77
@ -46,34 +47,11 @@
#define CMD_PROM_RD 0xA0 // Prom read command
#define PROM_NB 8
#if defined(USE_BARO_MS5607) || defined(USE_BARO_MS5611)
STATIC_UNIT_TESTED uint32_t ms56xx_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t ms56xx_up; // static result of pressure measurement
STATIC_UNIT_TESTED uint16_t ms56xx_c[PROM_NB]; // on-chip ROM
static uint8_t ms56xx_osr = CMD_ADC_4096;
static void ms56xx_reset(void)
{
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiWriteCommand(CMD_RESET, 1);
#else
i2cWrite(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_RESET, 1);
#endif
delayMicroseconds(2800);
}
static uint16_t ms56xx_prom(int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiReadCommand(CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
#else
i2cRead(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
#endif
return rxbuf[0] << 8 | rxbuf[1];
}
STATIC_UNIT_TESTED int8_t ms56xx_crc(uint16_t *prom)
{
int32_t i, j;
@ -104,48 +82,39 @@ STATIC_UNIT_TESTED int8_t ms56xx_crc(uint16_t *prom)
return -1;
}
static uint32_t ms56xx_read_adc(void)
static uint32_t ms56xx_read_adc(baroDev_t *baro)
{
uint8_t rxbuf[3];
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiReadCommand(CMD_ADC_READ, 3, rxbuf); // read ADC
#else
i2cRead(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
#endif
busReadBuf(baro->busDev, CMD_ADC_READ, rxbuf, 3);
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
static void ms56xx_start_ut(void)
static bool ms56xx_start_ut(baroDev_t *baro)
{
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiWriteCommand(CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1); // D2 (temperature) conversion start!
#else
i2cWrite(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1); // D2 (temperature) conversion start!
#endif
return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D2 + ms56xx_osr, 1);
}
static void ms56xx_get_ut(void)
static bool ms56xx_get_ut(baroDev_t *baro)
{
ms56xx_ut = ms56xx_read_adc();
ms56xx_ut = ms56xx_read_adc(baro);
return true;
}
static void ms56xx_start_up(void)
static bool ms56xx_start_up(baroDev_t *baro)
{
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiWriteCommand(CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1); // D1 (pressure) conversion start!
#else
i2cWrite(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1); // D1 (pressure) conversion start!
#endif
return busWrite(baro->busDev, CMD_ADC_CONV + CMD_ADC_D1 + ms56xx_osr, 1);
}
static void ms56xx_get_up(void)
static bool ms56xx_get_up(baroDev_t *baro)
{
ms56xx_up = ms56xx_read_adc();
ms56xx_up = ms56xx_read_adc(baro);
return true;
}
#ifdef USE_BARO_MS5611
STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature)
STATIC_UNIT_TESTED bool ms5611_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
{
UNUSED(baro);
uint32_t press;
int64_t temp;
int64_t delt;
@ -173,12 +142,15 @@ STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature
*pressure = press;
if (temperature)
*temperature = temp;
return true;
}
#endif
#ifdef USE_BARO_MS5607
STATIC_UNIT_TESTED void ms5607_calculate(int32_t *pressure, int32_t *temperature)
STATIC_UNIT_TESTED bool ms5607_calculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
{
UNUSED(baro);
uint32_t press;
int64_t temp;
int64_t delt;
@ -206,36 +178,47 @@ STATIC_UNIT_TESTED void ms5607_calculate(int32_t *pressure, int32_t *temperature
*pressure = press;
if (temperature)
*temperature = temp;
return true;
}
#endif
#define DETECTION_MAX_RETRY_COUNT 5
bool ms56xxDetect(baroDev_t *baro, baroSensor_e baroType)
static bool deviceDetect(busDevice_t * dev)
{
UNUSED(baroType);
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
uint8_t sig = 0;
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
uint8_t sig;
int i;
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
ms56xxSpiInit();
ms56xxSpiReadCommand(CMD_PROM_RD, 1, &sig);
if (sig != 0xFF) {
#else
bool ack = i2cRead(BARO_I2C_INSTANCE, MS56XX_ADDR, CMD_PROM_RD, 1, &sig);
if (ack) {
#endif
delay(10);
bool ack = busRead(dev, CMD_PROM_RD, &sig);
if (ack && sig != 0xFF) {
return true;
}
};
ms56xx_reset();
// read all coefficients
for (i = 0; i < PROM_NB; i++)
ms56xx_c[i] = ms56xx_prom(i);
// check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
if (ms56xx_crc(ms56xx_c) != 0)
return false;
}
static bool deviceInit(baroDev_t *baro)
{
busSetSpeed(baro->busDev, BUS_SPEED_STANDARD);
busWrite(baro->busDev, CMD_RESET, 1);
delay(5);
// read all coefficients
for (int i = 0; i < PROM_NB; i++) {
uint8_t rxbuf[2] = { 0, 0 };
busReadBuf(baro->busDev, CMD_PROM_RD + i * 2, rxbuf, 2);
ms56xx_c[i] = (rxbuf[0] << 8 | rxbuf[1]);
}
// check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
if (ms56xx_crc(ms56xx_c) != 0) {
return false;
}
// TODO prom + CRC
baro->ut_delay = 10000;
baro->up_delay = 10000;
baro->start_ut = ms56xx_start_ut;
@ -243,25 +226,55 @@ bool ms56xxDetect(baroDev_t *baro, baroSensor_e baroType)
baro->start_up = ms56xx_start_up;
baro->get_up = ms56xx_get_up;
#if defined(USE_BARO_MS5607) && defined(USE_BARO_MS5611)
if (baroType == BARO_MS5607) {
baro->calculate = ms5607_calculate;
} else { // default to MS5611
baro->calculate = ms5611_calculate;
}
#elif defined(USE_BARO_MS5607)
baro->calculate = ms5607_calculate;
#else
baro->calculate = ms5611_calculate;
#endif
return true;
}
delay(10);
}
#ifdef USE_BARO_MS5607
bool ms5607Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MS5607, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
if (!deviceDetect(baro->busDev)) {
busDeviceDeInit(baro->busDev);
return false;
}
if (!deviceInit(baro)) {
busDeviceDeInit(baro->busDev);
return false;
}
baro->calculate = ms5607_calculate;
return true;
}
#endif
#ifdef USE_BARO_MS5611
bool ms5611Detect(baroDev_t *baro)
{
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MS5611, 0, OWNER_BARO);
if (baro->busDev == NULL) {
return false;
}
if (!deviceDetect(baro->busDev)) {
busDeviceDeInit(baro->busDev);
return false;
}
if (!deviceInit(baro)) {
busDeviceDeInit(baro->busDev);
return false;
}
baro->calculate = ms5611_calculate;
return true;
}
#endif
#endif

3
src/main/drivers/barometer/barometer_ms56xx.h
View file

@ -18,4 +18,5 @@
#pragma once
#include "sensors/barometer.h" // for baroSensor_e enum
bool ms56xxDetect(baroDev_t *baro, baroSensor_e baroType);
bool ms5607Detect(baroDev_t *baro);
bool ms5611Detect(baroDev_t *baro);

95
src/main/drivers/barometer/barometer_spi_bmp280.c
View file

@ -1,95 +0,0 @@
/*
* This file is part of Betaflight.
*
* Betaflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Betaflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp280.h"
#ifdef USE_BARO_SPI_BMP280
#define DISABLE_BMP280 IOHi(bmp280CsPin)
#define ENABLE_BMP280 IOLo(bmp280CsPin)
extern int32_t bmp280_up;
extern int32_t bmp280_ut;
static IO_t bmp280CsPin = IO_NONE;
BUSDEV_REGISTER_SPI(bmp280_busdev, DEVHW_BMP280, 0, BMP280_CS_PIN);
bool bmp280WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_BMP280;
spiTransferByte(BMP280_SPI_INSTANCE, reg & 0x7F);
spiTransferByte(BMP280_SPI_INSTANCE, data);
DISABLE_BMP280;
return true;
}
bool bmp280ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_BMP280;
spiTransferByte(BMP280_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(BMP280_SPI_INSTANCE, data, NULL, length);
DISABLE_BMP280;
return true;
}
void bmp280SpiInit(void)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
bmp280CsPin = IOGetByTag(IO_TAG(BMP280_CS_PIN));
IOInit(bmp280CsPin, OWNER_BARO, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bmp280CsPin, IOCFG_OUT_PP);
DISABLE_BMP280;
spiSetSpeed(BMP280_SPI_INSTANCE, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
void bmp280_spi_start_up(void)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE);
}
void bmp280_spi_get_up(void)
{
uint8_t data[BMP280_DATA_FRAME_SIZE];
// read data from sensor
bmp280ReadRegister(BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
}
#endif

74
src/main/drivers/barometer/barometer_spi_ms56xx.c
View file

@ -1,74 +0,0 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/bus_spi.h"
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_ms56xx.h"
#if defined(USE_BARO_SPI_MS5611) || defined(USE_BARO_SPI_MS5607)
#define DISABLE_MS56XX(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MS56XX(spiCsnPin) IOLo(spiCsnPin)
static IO_t spiCsnPin = IO_NONE;
bool ms56xxSpiWriteCommand(uint8_t reg, uint8_t data)
{
ENABLE_MS56XX(spiCsnPin);
spiTransferByte(MS56XX_SPI_INSTANCE, reg);
spiTransferByte(MS56XX_SPI_INSTANCE, data);
DISABLE_MS56XX(spiCsnPin);
return true;
}
bool ms56xxSpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MS56XX(spiCsnPin);
spiTransferByte(MS56XX_SPI_INSTANCE, reg);
spiTransfer(MS56XX_SPI_INSTANCE, data, NULL, length);
DISABLE_MS56XX(spiCsnPin);
return true;
}
void ms56xxSpiInit(void)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
spiCsnPin = IOGetByTag(IO_TAG(MS56XX_CS_PIN));
IOInit(spiCsnPin, OWNER_BARO, RESOURCE_SPI_CS, 0);
IOConfigGPIO(spiCsnPin, IOCFG_OUT_PP);
DISABLE_MS56XX(spiCsnPin);
spiSetSpeed(MS56XX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
#endif

244
src/main/drivers/bus.c
View file

@ -22,11 +22,17 @@
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <string.h>
#include "platform.h"
#include "build/debug.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#define BUSDEV_MAX_DEVICES 8
#ifdef USE_SPI
static void busDevPreInit_SPI(const busDeviceDescriptor_t * descriptor)
{
// Pre-initialize SPI device chip-select line to input with weak pull-up
@ -36,11 +42,10 @@ static void busDevPreInit_SPI(const busDeviceDescriptor_t * descriptor)
IOConfigGPIO(io, IOCFG_IPU);
}
}
#endif
void busInit(void)
static void busDevPreInit(const busDeviceDescriptor_t * descriptor)
{
/* Pre-initialize bus devices */
for (const busDeviceDescriptor_t * descriptor = __busdev_registry_start; (descriptor) < __busdev_registry_end; descriptor++) {
switch (descriptor->busType) {
case BUSTYPE_NONE:
break;
@ -49,26 +54,39 @@ void busInit(void)
break;
case BUSTYPE_SPI:
#ifdef USE_SPI
busDevPreInit_SPI(descriptor);
#endif
break;
}
}
void busInit(void)
{
/* Pre-initialize bus devices */
for (const busDeviceDescriptor_t * descriptor = __busdev_registry_start; (descriptor) < __busdev_registry_end; descriptor++) {
busDevPreInit(descriptor);
}
}
#ifdef USE_I2C
static bool busDevInit_I2C(busDevice_t * dev, const busDeviceDescriptor_t * descriptor)
{
dev->busType = descriptor->busType;
dev->irqPin = IOGetByTag(descriptor->irqPin);
dev->busdev.i2c.i2cBus = descriptor->busdev.i2c.i2cBus;
dev->busdev.i2c.address = descriptor->busdev.i2c.address;
return true;
}
#endif
#ifdef USE_SPI
static bool busDevInit_SPI(busDevice_t * dev, const busDeviceDescriptor_t * descriptor, resourceOwner_e owner)
{
dev->busType = descriptor->busType;
dev->irqPin = IOGetByTag(descriptor->irqPin);
dev->busdev.spi.spiBus = descriptor->busdev.spi.spiBus;
dev->busdev.spi.csnPin = IOGetByTag(descriptor->busdev.spi.csnPin);
if (dev->busdev.spi.csnPin) {
IOInit(dev->busdev.spi.csnPin, owner, RESOURCE_SPI_CS, 0);
IOConfigGPIO(dev->busdev.spi.csnPin, SPI_IO_CS_CFG);
@ -78,81 +96,241 @@ static bool busDevInit_SPI(busDevice_t * dev, const busDeviceDescriptor_t * desc
return false;
}
#endif
bool busDeviceInit(busDevice_t * dev, busType_e bus, devHardwareType_e hw, resourceOwner_e owner)
void busDeviceDeInit(busDevice_t * dev)
{
busDevPreInit(dev->descriptorPtr);
dev->descriptorPtr = NULL;
dev->busType = BUSTYPE_NONE;
}
busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, resourceOwner_e owner)
{
UNUSED(owner);
for (const busDeviceDescriptor_t * descriptor = __busdev_registry_start; (descriptor) < __busdev_registry_end; descriptor++) {
if (hw == descriptor->devHwType && (bus == descriptor->busType || bus == BUSTYPE_ANY)) {
if (hw == descriptor->devHwType && (bus == descriptor->busType || bus == BUSTYPE_ANY) && (tag == descriptor->tag)) {
// We have a candidate - initialize device context memory
busDevice_t * dev = descriptor->devicePtr;
memset(dev, 0, sizeof(busDevice_t));
dev->descriptorPtr = descriptor;
dev->busType = descriptor->busType;
dev->flags = descriptor->flags;
if (dev) {
switch (descriptor->busType) {
case BUSTYPE_NONE:
dev->busType = BUSTYPE_NONE;
return false;
return NULL;
case BUSTYPE_I2C:
return busDevInit_I2C(dev, descriptor);
#ifdef USE_I2C
if (!busDevInit_I2C(dev, descriptor)) {
busDeviceDeInit(dev);
return NULL;
}
break;
#else
busDeviceDeInit(dev);
return NULL;
#endif
case BUSTYPE_SPI:
return busDevInit_SPI(dev, descriptor, owner);
#ifdef USE_SPI
if (!busDevInit_SPI(dev, descriptor, owner)) {
busDeviceDeInit(dev);
return NULL;
}
break;
#else
busDeviceDeInit(dev);
return NULL;
#endif
}
return dev;
}
else {
return NULL;
}
}
}
return NULL;
}
busDevice_t * busDeviceOpen(busType_e bus, devHardwareType_e hw, uint8_t tag)
{
for (const busDeviceDescriptor_t * descriptor = __busdev_registry_start; (descriptor) < __busdev_registry_end; descriptor++) {
if (hw == descriptor->devHwType && (bus == descriptor->busType || bus == BUSTYPE_ANY) && (tag == descriptor->tag)) {
// Found a hardware descriptor. Now check if device context is valid
busDevice_t * dev = descriptor->devicePtr;
if (dev->busType == descriptor->busType && dev->descriptorPtr == descriptor) {
return dev;
}
}
}
return NULL;
}
void busSetSpeed(const busDevice_t * dev, busSpeed_e speed)
{
UNUSED(speed);
switch (dev->busType) {
case BUSTYPE_NONE:
// Not available
break;
case BUSTYPE_SPI:
#ifdef USE_SPI
spiBusSetSpeed(dev, speed);
#endif
break;
case BUSTYPE_I2C:
// Do nothing for I2C
break;
}
}
uint32_t busDeviceReadScratchpad(const busDevice_t * dev)
{
return dev->scratchpad;
}
void busDeviceWriteScratchpad(busDevice_t * dev, uint32_t value)
{
dev->scratchpad = value;
}
bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length)
{
UNUSED(rxBuf);
UNUSED(txBuf);
UNUSED(length);
switch (dev->busType) {
case BUSTYPE_SPI:
#ifdef USE_SPI
return spiBusTransfer(dev, rxBuf, txBuf, length);
#else
return false;
#endif
case BUSTYPE_I2C:
// Raw transfer operation is not supported on I2C
return false;
default:
return false;
}
}
/*
bool busWriteBuf(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length)
bool busWriteBuf(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length)
{
switch (dev->busType) {
case BUSTYPE_NONE:
return false;
case BUSTYPE_SPI:
return spiBusWriteBuffer(dev, reg & 0x7f, data, length);
#ifdef USE_SPI
if (dev->flags & DEVFLAGS_USE_RAW_REGISTERS) {
return spiBusWriteBuffer(dev, reg, data, length);
}
else {
return spiBusWriteBuffer(dev, reg | 0x80, data, length);
}
#else
return false;
#endif
case BUSTYPE_I2C:
return i2cBusWriteBuffer(dev, reg, data);
#ifdef USE_I2C
return i2cBusWriteBuffer(dev, reg, data, length);
#else
return false;
#endif
default:
return false;
}
}
*/
bool busWrite(const busDevice_t * dev, uint8_t reg, uint8_t data)
{
switch (dev->busType) {
case BUSTYPE_NONE:
return false;
case BUSTYPE_SPI:
return spiBusWriteRegister(dev, reg & 0x7f, data);
case BUSTYPE_I2C:
return i2cBusWriteRegister(dev, reg, data);
#ifdef USE_SPI
if (dev->flags & DEVFLAGS_USE_RAW_REGISTERS) {
return spiBusWriteRegister(dev, reg, data);
}
else {
return spiBusWriteRegister(dev, reg & 0x7F, data);
}
#else
return false;
#endif
case BUSTYPE_I2C:
#ifdef USE_I2C
return i2cBusWriteRegister(dev, reg, data);
#else
return false;
#endif
default:
return false;
}
}
bool busReadBuf(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length)
{
switch (dev->busType) {
case BUSTYPE_NONE:
return false;
case BUSTYPE_SPI:
return spiBusReadBuffer(dev, reg | 0x80, data, length);
case BUSTYPE_I2C:
return i2cBusReadBuffer(dev, reg, data, length);
#ifdef USE_SPI
if (dev->flags & DEVFLAGS_USE_RAW_REGISTERS) {
return spiBusReadBuffer(dev, reg, data, length);
}
else {
return spiBusReadBuffer(dev, reg | 0x80, data, length);
}
#else
return false;
#endif
case BUSTYPE_I2C:
#ifdef USE_I2C
return i2cBusReadBuffer(dev, reg, data, length);
#else
return false;
#endif
default:
return false;
}
}
bool busRead(const busDevice_t * dev, uint8_t reg, uint8_t * data)
{
switch (dev->busType) {
case BUSTYPE_SPI:
#ifdef USE_SPI
if (dev->flags & DEVFLAGS_USE_RAW_REGISTERS) {
return spiBusReadRegister(dev, reg, data);
}
else {
return spiBusReadRegister(dev, reg | 0x80, data);
case BUSTYPE_I2C:
return i2cBusReadRegister(dev, reg, data);
case BUSTYPE_NONE:
return false;
}
#else
return false;
#endif
case BUSTYPE_I2C:
#ifdef USE_I2C
return i2cBusReadRegister(dev, reg, data);
#else
return false;
#endif
default:
return false;
}
}

184
src/main/drivers/bus.h
View file

@ -1,18 +1,25 @@
/*
* This file is part of Cleanflight.
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
@ -26,6 +33,26 @@
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
// FIXME: Hack until we rework SPI driver
#define BUS_SPI1 SPIDEV_1
#define BUS_SPI2 SPIDEV_2
#define BUS_SPI3 SPIDEV_3
#define BUS_SPI4 SPIDEV_4
#define BUS_I2C1 I2CDEV_1
#define BUS_I2C2 I2CDEV_2
#define BUS_I2C3 I2CDEV_3
#define BUS_I2C4 I2CDEV_4
#define BUS_I2C_EMULATED I2CINVALID
typedef enum {
BUS_SPEED_INITIALIZATION = 0,
BUS_SPEED_SLOW = 1,
BUS_SPEED_STANDARD = 2,
BUS_SPEED_FAST = 3,
BUS_SPEED_ULTRAFAST = 4
} busSpeed_e;
typedef enum {
BUSTYPE_ANY = 0,
BUSTYPE_NONE = 0,
@ -36,38 +63,98 @@ typedef enum {
/* Ultimately all hardware descriptors will go to target definition files.
* Driver code will merely query for it's HW descriptor and initialize it */
typedef enum {
DEVHW_NONE,
DEVHW_NONE = 0,
/* Dedicated ACC chips */
DEVHW_BMA280,
DEVHW_ADXL345,
DEVHW_MMA8452,
DEVHW_LSM303DLHC,
/* Dedicated GYRO chips */
DEVHW_L3GD20,
DEVHW_L3G4200,
/* Combined ACC/GYRO chips */
DEVHW_MPU6000,
DEVHW_MPU6050,
DEVHW_MPU6500,
/* Combined ACC/GYRO/MAG chips */
DEVHW_MPU9250,
/* Barometer chips */
DEVHW_BMP085,
DEVHW_BMP280,
DEVHW_MS5611,
DEVHW_MS5607,
/* Compass chips */
DEVHW_HMC5883,
DEVHW_AK8963,
DEVHW_AK8975,
DEVHW_IST8310,
DEVHW_QMC5883,
DEVHW_MAG3110,
/* OSD chips */
DEVHW_MAX7456,
DEVHW_BMP280
/* Rangefinder modules */
DEVHW_SRF10,
DEVHW_HCSR04_I2C, // DIY-style adapter
DEVHW_VL53L0X,
} devHardwareType_e;
typedef enum {
DEVFLAGS_NONE = 0,
DEVFLAGS_USE_RAW_REGISTERS = (1 << 0), // Don't manipulate MSB for R/W selection
} deviceFlags_e;
typedef struct busDeviceDescriptor_s {
void * devicePtr;
busType_e busType;
devHardwareType_e devHwType;
uint16_t flags;
uint8_t tag;
union {
#ifdef USE_SPI
struct {
SPIDevice spiBus;
ioTag_t csnPin;
} spi;
#endif
#ifdef USE_I2C
struct {
I2CDevice i2cBus;
uint8_t address;
} i2c;
#endif
} busdev;
ioTag_t irqPin;
} busDeviceDescriptor_t;
typedef struct busDevice_s {
busType_e busType;
const busDeviceDescriptor_t * descriptorPtr;
busType_e busType; // Copy of busType to avoid additional pointer dereferencing
uint32_t flags; // Copy of flags
union {
#ifdef USE_SPI
struct {
SPIDevice spiBus;
IO_t csnPin;
SPIDevice spiBus; // SPI bus ID
IO_t csnPin; // IO for CS# pin
} spi;
#endif
#ifdef USE_I2C
struct {
I2CDevice i2cBus;
uint8_t address;
I2CDevice i2cBus; // I2C bus ID
uint8_t address; // I2C bus device address
} i2c;
#endif
} busdev;
IO_t irqPin; // Device IRQ pin. Bus system will only assign IO_t object to this var. Initialization is up to device driver
uint32_t scratchpad; // Memory where device driver can store persistent data. Zeroed out when initializing the device for the first time
// Useful when once device is shared between several sensors (like MPU/ICM acc-gyro sensors)
} busDevice_t;
#ifdef __APPLE__
@ -80,24 +167,65 @@ extern const busDeviceDescriptor_t __busdev_registry_end[];
#define BUSDEV_REGISTER_ATTRIBUTES __attribute__ ((section(".busdev_registry"), used, aligned(4)))
#endif
#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin) \
extern const busDeviceDescriptor_t _name; \
const busDeviceDescriptor_t _name BUSDEV_REGISTER_ATTRIBUTES = { \
#define BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags) \
extern const busDeviceDescriptor_t _name ## _registry; \
static busDevice_t _name ## _memory; \
const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = { \
.devicePtr = (void *) & _name ## _memory, \
.busType = BUSTYPE_SPI, \
.devHwType = _devHw, \
.flags = _flags, \
.tag = _tag, \
.busdev.spi = { \
.spiBus = _spiBus, \
.csnPin = IO_TAG(_csnPin) \
} \
}, \
.irqPin = IO_TAG(_irqPin) \
}; \
struct _dummy \
/**/
#define BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags) \
extern const busDeviceDescriptor_t _name ## _registry; \
static busDevice_t _name ## _memory; \
const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = { \
.devicePtr = (void *) & _name ## _memory, \
.busType = BUSTYPE_I2C, \
.devHwType = _devHw, \
.flags = _flags, \
.tag = _tag, \
.busdev.i2c = { \
.i2cBus = _i2cBus, \
.address = _devAddr \
}, \
.irqPin = IO_TAG(_irqPin) \
}; \
struct _dummy \
/**/
#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin, _irqPin, _flags) \
BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, 0, _flags)
#define BUSDEV_REGISTER_SPI_TAG(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags) \
BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)
#define BUSDEV_REGISTER_I2C(_name, _devHw, _i2cBus, _devAddr, _irqPin, _flags) \
BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, 0, _flags)
#define BUSDEV_REGISTER_I2C_TAG(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags) \
BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)
/* Internal abstraction function */
bool i2cBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length);
bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data);
bool i2cBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length);
bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data);
void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed);
bool spiBusTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length);
bool spiBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length);
bool spiBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data);
bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length);
bool spiBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data);
@ -107,9 +235,17 @@ bool spiBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data);
void busInit(void);
/* Finds a device in registry. First matching device is returned. Also performs the low-level initialization of the hardware (CS line for SPI) */
bool busDeviceInit(busDevice_t * dev, busType_e bus, devHardwareType_e hw, resourceOwner_e owner);
busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, resourceOwner_e owner);
busDevice_t * busDeviceOpen(busType_e bus, devHardwareType_e hw, uint8_t tag);
void busDeviceDeInit(busDevice_t * dev);
bool busWriteBuf(const busDevice_t * busdev, uint8_t reg, uint8_t * data, uint8_t length);
bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data);
uint32_t busDeviceReadScratchpad(const busDevice_t * dev);
void busDeviceWriteScratchpad(busDevice_t * dev, uint32_t value);
void busSetSpeed(const busDevice_t * dev, busSpeed_e speed);
bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length);
bool busWriteBuf(const busDevice_t * busdev, uint8_t reg, const uint8_t * data, uint8_t length);
bool busReadBuf(const busDevice_t * busdev, uint8_t reg, uint8_t * data, uint8_t length);
bool busRead(const busDevice_t * busdev, uint8_t reg, uint8_t * data);
bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data);

5
src/main/drivers/bus_busdev_i2c.c
View file

@ -26,6 +26,11 @@
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
bool i2cBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length)
{
return i2cWriteBuffer(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data);
}
bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data)
{
return i2cWrite(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, data);

39
src/main/drivers/bus_busdev_spi.c
View file

@ -20,26 +20,49 @@
#include <string.h>
#include <platform.h>
#include <build/debug.h>
#if defined(USE_SPI)
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/bus_spi.h"
#include "drivers/time.h"
void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed)
{
const SPIClockSpeed_e spiClock[] = { SPI_CLOCK_INITIALIZATON, SPI_CLOCK_SLOW, SPI_CLOCK_STANDARD, SPI_CLOCK_FAST, SPI_CLOCK_ULTRAFAST };
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
spiSetSpeed(instance, spiClock[speed]);
}
bool spiBusTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length)
{
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
IOLo(dev->busdev.spi.csnPin);
spiTransfer(instance, rxBuf, txBuf, length);
IOHi(dev->busdev.spi.csnPin);
return true;
}
bool spiBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data)
{
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
IOLo(dev->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(instance, reg);
spiTransferByte(instance, data);
IOHi(dev->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length)
bool spiBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length)
{
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
@ -51,13 +74,25 @@ bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint
return true;
}
bool spiBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length)
{
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
IOLo(dev->busdev.spi.csnPin);
spiTransferByte(instance, reg);
spiTransfer(instance, data, NULL, length);
IOHi(dev->busdev.spi.csnPin);
return true;
}
bool spiBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data)
{
SPI_TypeDef * instance = spiInstanceByDevice(dev->busdev.spi.spiBus);
IOLo(dev->busdev.spi.csnPin);
spiTransferByte(instance, reg);
spiTransfer(instance, NULL, data, 1);
spiTransfer(instance, data, NULL, 1);
IOHi(dev->busdev.spi.csnPin);
return true;

2
src/main/drivers/compass/compass.h
View file

@ -20,9 +20,9 @@
#include "drivers/sensor.h"
typedef struct magDev_s {
busDevice_t * busDev;
sensorMagInitFuncPtr init; // initialize function
sensorMagReadFuncPtr read; // read 3 axis data function
busDevice_t bus;
sensor_align_e magAlign;
int16_t magADCRaw[XYZ_AXIS_COUNT];
} magDev_t;

312
src/main/drivers/compass/compass_ak8963.c
View file

@ -33,23 +33,15 @@
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/compass/compass_ak8963.h"
// This sensor is available in MPU-9250.
// AK8963, mag sensor address
#define AK8963_MAG_I2C_ADDRESS 0x0C
#define AK8963_Device_ID 0x48
#define AK8963_DEVICE_ID 0x48
// Registers
#define AK8963_MAG_REG_WHO_AM_I 0x00
@ -85,305 +77,97 @@
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
#define DISABLE_SPI_MPU(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_SPI_MPU(spiCsnPin) IOLo(spiCsnPin)
#define MPU_SPI_INSTANCE MPU6500_SPI_INSTANCE
typedef struct queuedReadState_s {
bool waiting;
uint8_t len;
timeUs_t readStartedAt; // time read was queued in micros.
} queuedReadState_t;
typedef enum {
CHECK_STATUS = 0,
WAITING_FOR_STATUS,
WAITING_FOR_DATA
} ak8963ReadState_e;
static queuedReadState_t queuedRead = { false, 0, 0};
/* We have AK8963 connected internally to MPU9250 I2C master. Accessing the compass sensor requires
* setting up the MPU's I2C host. We have separate implementation of SPI read/write functions to access
* the MPU registers
*/
static bool mpuSpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_SPI_MPU(bus->busdev.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU_SPI_INSTANCE, reg);
spiTransferByte(MPU_SPI_INSTANCE, data);
DISABLE_SPI_MPU(bus->busdev.spi.csnPin);
delayMicroseconds(1);
return true;
}
static bool mpuSpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_SPI_MPU(bus->busdev.spi.csnPin);
spiTransferByte(MPU_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU_SPI_INSTANCE, data, NULL, length);
DISABLE_SPI_MPU(bus->busdev.spi.csnPin);
return true;
}
static bool ak8963SensorRead(magDev_t *magDev, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
{
// Setting up MPU9250's I2C master to read from AK8963 via internal I2C bus
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
delay(10); // wait for transaction to complete
__disable_irq();
mpuSpiReadRegister(&magDev->bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
__enable_irq();
return true;
}
static bool ak8963SensorWrite(magDev_t *magDev, uint8_t addr_, uint8_t reg_, uint8_t data)
{
// Setting up MPU9250's I2C master to write to AK8963 via internal I2C bus
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true;
}
static bool ak8963SensorStartRead(magDev_t *magDev, uint8_t addr_, uint8_t reg_, uint8_t len_)
{
// Setting up a read. We can't busy-wait with delay() when in normal operation.
// Instead we'll set up a read and raise the flag to finalize the read after certain timeout
if (queuedRead.waiting) {
return false;
}
queuedRead.len = len_;
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
queuedRead.readStartedAt = micros();
queuedRead.waiting = true;
return true;
}
static timeDelta_t ak8963SensorQueuedReadTimeRemaining(void)
{
if (!queuedRead.waiting) {
return 0;
}
timeDelta_t timeSinceStarted = micros() - queuedRead.readStartedAt;
timeDelta_t timeRemaining = 8000 - timeSinceStarted;
if (timeRemaining < 0) {
return 0;
}
return timeRemaining;
}
static bool ak8963SensorCompleteRead(magDev_t *magDev, uint8_t *buf)
{
// Finalizing the read of AK8963 registers via MPU9250's built-in I2C master
timeDelta_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
if (timeRemaining > 0) {
delayMicroseconds(timeRemaining);
}
queuedRead.waiting = false;
mpuSpiReadRegister(&magDev->bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
return true;
}
#else
static bool ak8963SensorRead(magDev_t *magDev, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
{
UNUSED(magDev);
return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf);
}
static bool ak8963SensorWrite(magDev_t *magDev, uint8_t addr_, uint8_t reg_, uint8_t data)
{
UNUSED(magDev);
return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data);
}
#endif
static bool ak8963Init(magDev_t *magDev)
static bool ak8963Init(magDev_t * mag)
{
bool ack;
UNUSED(ack);
uint8_t calibration[3];
uint8_t status;
ack = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
delay(20);
ack = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
delay(10);
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values
ack = busReadBuf(mag->busDev, AK8963_MAG_REG_ASAX, calibration, sizeof(calibration)); // Read the x-, y-, and z-axis calibration values
delay(10);
magGain[X] = (((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30);
magGain[Y] = (((((float)(int8_t)calibration[Y] - 128) / 256) + 1) * 30);
magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * 30);
ack = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
// Clear status registers
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status);
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status);
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &status);
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS2, &status);
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
// Trigger first measurement
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
ack = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1);
#else
ack = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
#endif
return true;
}
static bool ak8963Read(magDev_t *magDev)
static bool ak8963Read(magDev_t * mag)
{
bool ack = false;
bool readResult = false;
uint8_t buf[7];
static bool lastReadResult = false;
ack = busRead(mag->busDev, AK8963_MAG_REG_STATUS1, &buf[0]);
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
static int16_t cachedMagData[3];
// set magData to latest cached value
memcpy(&magDev->magADCRaw, cachedMagData, sizeof(cachedMagData));
// we currently need a different approach for the MPU9250 connected via SPI.
// we cannot use the ak8963SensorRead() method for SPI, it is to slow and blocks for far too long.
static ak8963ReadState_e state = CHECK_STATUS;
bool retry = true;
restart:
switch (state) {
case CHECK_STATUS:
ak8963SensorStartRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
state++;
return lastReadResult;
case WAITING_FOR_STATUS: {
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
if (timeRemaining) {
return lastReadResult;
if (!ack || (buf[0] & STATUS1_DATA_READY) == 0) {
return false;
}
ack = ak8963SensorCompleteRead(magDev, &buf[0]);
ack = busReadBuf(mag->busDev, AK8963_MAG_REG_HXL, &buf[0], 7);
uint8_t status = buf[0];
if (!ack || ((status & STATUS1_DATA_READY) == 0 && (status & STATUS1_DATA_OVERRUN) == 0)) {
// too early. queue the status read again
state = CHECK_STATUS;
if (retry) {
retry = false;
goto restart;
if (!ack || (buf[6] & STATUS2_DATA_ERROR) || (buf[6] & STATUS2_MAG_SENSOR_OVERFLOW)) {
return false;
}
lastReadResult = false;
return lastReadResult;
}
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
// read the 6 bytes of data and the status2 register
ak8963SensorStartRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
ack = busWrite(mag->busDev, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
state++;
return lastReadResult;
}
case WAITING_FOR_DATA: {
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
if (timeRemaining) {
return lastReadResult;
}
ack = ak8963SensorCompleteRead(magDev, &buf[0]);
}
}
#else
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]);
uint8_t status = buf[0];
if (!ack || (status & STATUS1_DATA_READY) == 0) {
lastReadResult = false;
return lastReadResult;
}
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]);
#endif
uint8_t status2 = buf[6];
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
lastReadResult = false;
return lastReadResult;
}
magDev->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
magDev->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
magDev->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
// cache mag data for reuse
memcpy(cachedMagData, &magDev->magADCRaw, sizeof(cachedMagData));
state = CHECK_STATUS;
lastReadResult = true;
#else
lastReadResult = ak8963SensorWrite(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
#endif
return lastReadResult;
return readResult;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool ak8963Detect(magDev_t *magDev)
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
bool ack = false;
delay(10);
uint8_t sig = 0;
bool ack = busRead(mag->busDev, AK8963_MAG_REG_WHO_AM_I, &sig);
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
// Initialize I2C master via SPI bus (MPU9250)
ack = mpuSpiWriteRegister(&magDev->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(10);
ack = mpuSpiWriteRegister(&magDev->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
ack = mpuSpiWriteRegister(&magDev->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif
// check for AK8963
ack = ak8963SensorRead(magDev, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
if (ack && sig == AK8963_Device_ID) { // 0x48 / 01001000 / 'H'
magDev->init = ak8963Init;
magDev->read = ak8963Read;
if (ack && sig == AK8963_DEVICE_ID) {
return true;
}
}
return false;
}
bool ak8963Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_AK8963, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = ak8963Init;
mag->read = ak8963Read;
return true;
}
#endif

82
src/main/drivers/compass/compass_ak8975.c
View file

@ -32,7 +32,7 @@
#include "drivers/time.h"
#include "drivers/gpio.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
@ -44,7 +44,6 @@
// AK8975, mag sensor address
#define AK8975_MAG_I2C_ADDRESS 0x0C
// Registers
#define AK8975_MAG_REG_WHO_AM_I 0x00
#define AK8975_MAG_REG_INFO 0x01
@ -63,33 +62,32 @@
#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
static bool ak8975Init(magDev_t *magDev)
static bool ak8975Init(magDev_t * mag)
{
bool ack;
uint8_t buffer[3];
uint8_t status;
UNUSED(ack);
UNUSED(magDev);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
delay(20);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
delay(10);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
ack = busReadBuf(mag->busDev, AK8975A_ASAX, &buffer[0], 3); // Read the x-, y-, and z-axis calibration values
delay(10);
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
delay(10);
// Clear status registers
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS1, &status);
ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS2, &status);
// Trigger first measurement
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x01);
return true;
}
@ -98,59 +96,69 @@ static bool ak8975Init(magDev_t *magDev)
#define BIT_STATUS2_REG_DATA_ERROR (1 << 2)
#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3)
static bool ak8975Read(magDev_t *magDev)
static bool ak8975Read(magDev_t * mag)
{
uint8_t status;
uint8_t buf[6];
// set magData to zero for case of failed read
magDev->magADCRaw[X] = 0;
magDev->magADCRaw[Y] = 0;
magDev->magADCRaw[Z] = 0;
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
bool ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS1, &status);
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
return false;
}
#if 1 // USE_I2C_SINGLE_BYTE_READS
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
#else
for (uint8_t i = 0; i < 6; i++) {
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
if (!ack) {
return false
}
}
#endif
ack = busReadBuf(mag->busDev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
ack = busRead(mag->busDev, AK8975_MAG_REG_STATUS2, &status);
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
if (!ack || (status & BIT_STATUS2_REG_DATA_ERROR) || (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW)) {
return false;
}
magDev->magADCRaw[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
magDev->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4;
magDev->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
mag->magADCRaw[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * 4;
mag->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
ack = busWrite(mag->busDev, AK8975_MAG_REG_CNTL, 0x01); // start reading again
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool ak8975Detect(magDev_t *magDev)
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
delay(10);
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
if (ack && sig == 'H') { // 0x48 / 01001000 / 'H'
magDev->init = ak8975Init;
magDev->read = ak8975Read;
bool ack = busRead(mag->busDev, AK8975_MAG_REG_WHO_AM_I, &sig);
if (ack && sig == 0x48) {
return true;
}
}
return false;
}
bool ak8975Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_AK8975, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = ak8975Init;
mag->read = ak8975Read;
return true;
}
#endif

160
src/main/drivers/compass/compass_hmc5883l.c
View file

@ -33,7 +33,7 @@
#include "drivers/nvic.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/light_led.h"
#include "drivers/logging.h"
@ -43,8 +43,6 @@
#include "drivers/compass/compass_hmc5883l.h"
//#define DEBUG_MAG_DATA_READY_INTERRUPT
// HMC5883L, default address 0x1E
// NAZE Target connections
// PB12 connected to MAG_DRDY on rev4 hardware
@ -107,10 +105,10 @@
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
#define HMC58X3_R_CONFA 0
#define HMC58X3_R_CONFB 1
#define HMC58X3_R_MODE 2
#define MAG_DATA_REGISTER_SPI (0x03 | 0x40)
#define HMC58X3_R_CONFA 0x00
#define HMC58X3_R_CONFB 0x01
#define HMC58X3_R_MODE 0x02
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied.
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied.
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f) // Z axis level when bias current is applied.
@ -121,105 +119,65 @@
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static const hmc5883Config_t *hmc5883Config = NULL;
#ifdef USE_MAG_DATA_READY_SIGNAL
static IO_t intIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;
static void hmc5883_extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
#ifdef DEBUG_MAG_DATA_READY_INTERRUPT
// Measure the delta between calls to the interrupt handler
// currently should be around 65/66 milli seconds / 15hz output rate
static uint32_t lastCalledAt = 0;
static int32_t callDelta = 0;
uint32_t now = millis();
callDelta = now - lastCalledAt;
//UNUSED(callDelta);
debug[0] = callDelta;
lastCalledAt = now;
#endif
}
#endif
static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
if (!(hmc5883Config->intTag)) {
return;
}
intIO = IOGetByTag(hmc5883Config->intTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = IORead(intIO);
if (!status) {
return;
}
#endif
EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(intIO, true);
#endif
}
static bool hmc5883lRead(magDev_t* magDev)
static bool hmc5883lRead(magDev_t * mag)
{
uint8_t buf[6];
bool ack;
if (mag->busDev->busType == BUSTYPE_SPI) {
ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER_SPI, buf, 6);
}
else {
ack = busReadBuf(mag->busDev, MAG_DATA_REGISTER, buf, 6);
}
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
magDev->magADCRaw[X] = 0;
magDev->magADCRaw[Y] = 0;
magDev->magADCRaw[Z] = 0;
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
return false;
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
magDev->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
magDev->magADCRaw[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
magDev->magADCRaw[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
mag->magADCRaw[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
mag->magADCRaw[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
return true;
}
#define INITIALISATION_MAX_READ_FAILURES 5
static bool hmc5883lInit(magDev_t* magDev)
static bool hmc5883lInit(magDev_t * mag)
{
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator
delay(50);
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x18 + HMC_POS_BIAS); // Reg A DOR = 0x18 + MS1, MS0 set to pos bias
busWrite(mag->busDev, HMC58X3_R_CONFA, 0x18 + HMC_POS_BIAS); // Reg A DOR = 0x18 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x80); // Set the Gain to 4Ga (7:5->100)
busWrite(mag->busDev, HMC58X3_R_CONFB, 0x80); // Set the Gain to 4Ga (7:5->100)
delay(100);
hmc5883lRead(magDev);
hmc5883lRead(mag);
int validSamples1 = 0;
int failedSamples1 = 0;
int saturatedSamples1 = 0;
while (validSamples1 < 10 && failedSamples1 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
busWrite(mag->busDev, HMC58X3_R_MODE, 1);
delay(70);
if (hmc5883lRead(magDev)) { // Get the raw values in case the scales have already been changed.
if (hmc5883lRead(mag)) { // Get the raw values in case the scales have already been changed.
// Detect saturation.
if (-4096 >= MIN(magDev->magADCRaw[X], MIN(magDev->magADCRaw[Y], magDev->magADCRaw[Z]))) {
if (-4096 >= MIN(mag->magADCRaw[X], MIN(mag->magADCRaw[Y], mag->magADCRaw[Z]))) {
++saturatedSamples1;
++failedSamples1;
} else {
++validSamples1;
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magDev->magADCRaw[X];
xyz_total[Y] += magDev->magADCRaw[Y];
xyz_total[Z] += magDev->magADCRaw[Z];
xyz_total[X] += mag->magADCRaw[X];
xyz_total[Y] += mag->magADCRaw[Y];
xyz_total[Z] += mag->magADCRaw[Z];
}
} else {
@ -229,24 +187,24 @@ static bool hmc5883lInit(magDev_t* magDev)
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x18 + HMC_NEG_BIAS); // Reg A DOR = 0x18 + MS1, MS0 set to negative bias.
busWrite(mag->busDev, HMC58X3_R_CONFA, 0x18 + HMC_NEG_BIAS); // Reg A DOR = 0x18 + MS1, MS0 set to negative bias.
int validSamples2 = 0;
int failedSamples2 = 0;
int saturatedSamples2 = 0;
while (validSamples2 < 10 && failedSamples2 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
busWrite(mag->busDev, HMC58X3_R_MODE, 1);
delay(70);
if (hmc5883lRead(magDev)) { // Get the raw values in case the scales have already been changed.
if (hmc5883lRead(mag)) { // Get the raw values in case the scales have already been changed.
// Detect saturation.
if (-4096 >= MIN(magDev->magADCRaw[X], MIN(magDev->magADCRaw[Y], magDev->magADCRaw[Z]))) {
if (-4096 >= MIN(mag->magADCRaw[X], MIN(mag->magADCRaw[Y], mag->magADCRaw[Z]))) {
++saturatedSamples2;
++failedSamples2;
} else {
++validSamples2;
// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magDev->magADCRaw[X];
xyz_total[Y] -= magDev->magADCRaw[Y];
xyz_total[Z] -= magDev->magADCRaw[Z];
xyz_total[X] -= mag->magADCRaw[X];
xyz_total[Y] -= mag->magADCRaw[Y];
xyz_total[Z] -= mag->magADCRaw[Z];
}
} else {
++failedSamples2;
@ -275,34 +233,46 @@ static bool hmc5883lInit(magDev_t* magDev)
}
// leave test mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x78); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 75Hz ; normal measurement mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
busWrite(mag->busDev, HMC58X3_R_CONFA, 0x78); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 75Hz ; normal measurement mode
busWrite(mag->busDev, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
busWrite(mag->busDev, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
hmc5883lConfigureDataReadyInterruptHandling();
return bret;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool hmc5883lDetect(magDev_t* magDev, const hmc5883Config_t *hmc5883ConfigToUse)
static bool deviceDetect(magDev_t * mag)
{
hmc5883Config = hmc5883ConfigToUse;
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (ack && sig == 'H') {
magDev->init = hmc5883lInit;
magDev->read = hmc5883lRead;
delay(10);
uint8_t sig = 0;
bool ack = busRead(mag->busDev, 0x0A, &sig);
if (ack && sig == 'H') {
return true;
}
delay(10);
}
return false;
}
bool hmc5883lDetect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_HMC5883, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = hmc5883lInit;
mag->read = hmc5883lRead;
return true;
}
#endif

7
src/main/drivers/compass/compass_hmc5883l.h
View file

@ -18,9 +18,6 @@
#pragma once
#include "drivers/io_types.h"
#include "drivers/compass/compass.h"
typedef struct hmc5883Config_s {
ioTag_t intTag;
} hmc5883Config_t;
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
bool hmc5883lDetect(magDev_t * mag);

61
src/main/drivers/compass/compass_ist8310.c
View file

@ -33,7 +33,7 @@
#include "drivers/nvic.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "drivers/light_led.h"
#include "drivers/sensor.h"
@ -41,6 +41,12 @@
#include "drivers/compass/compass_ist8310.h"
#define IST8310_ADDRESS 0x0C
// Hardware descriptor
#if defined(MAG_I2C_BUS)
#endif
/* ist8310 Slave Address Select : default address 0x0C
* CAD1 | CAD0 | Address
* ------------------------------
@ -79,7 +85,6 @@
* This bit will be set to zero after POR routine
*/
#define IST8310_ADDRESS 0x0C
#define IST8310_REG_DATA 0x03
#define IST8310_REG_WHOAMI 0x00
@ -104,50 +109,50 @@
#define IST8310_CNTRL2_DRPOL 0x04
#define IST8310_CNTRL2_DRENA 0x08
static bool ist8310Init(magDev_t *magDev)
static bool ist8310Init(magDev_t * mag)
{
UNUSED(magDev);
i2cWrite(MAG_I2C_INSTANCE, IST8310_ADDRESS, IST8310_REG_CNTRL1, IST8310_ODR_50_HZ);
busWrite(mag->busDev, IST8310_REG_CNTRL1, IST8310_ODR_50_HZ);
delay(5);
i2cWrite(MAG_I2C_INSTANCE, IST8310_ADDRESS, IST8310_REG_AVERAGE, IST8310_AVG_16);
busWrite(mag->busDev, IST8310_REG_AVERAGE, IST8310_AVG_16);
delay(5);
return true;
}
static bool ist8310Read(magDev_t *magDev)
static bool ist8310Read(magDev_t * mag)
{
uint8_t buf[6];
uint8_t LSB2FSV = 3; // 3mG - 14 bit
// set magData to zero for case of failed read
magDev->magADCRaw[X] = 0;
magDev->magADCRaw[Y] = 0;
magDev->magADCRaw[Z] = 0;
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, IST8310_ADDRESS, IST8310_REG_DATA, 6, buf);
bool ack = busReadBuf(mag->busDev, IST8310_REG_DATA, buf, 6);
if (!ack) {
return false;
}
// need to modify when confirming the pcb direction
magDev->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
magDev->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
magDev->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool ist8310Detect(magDev_t *magDev)
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
delay(10);
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, IST8310_ADDRESS, IST8310_REG_WHOAMI, 1, &sig);
bool ack = busRead(mag->busDev, IST8310_REG_WHOAMI, &sig);
if (ack && sig == IST8310_CHIP_ID) {
magDev->init = ist8310Init;
magDev->read = ist8310Read;
return true;
}
}
@ -155,4 +160,22 @@ bool ist8310Detect(magDev_t *magDev)
return false;
}
bool ist8310Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_IST8310, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = ist8310Init;
mag->read = ist8310Read;
return true;
}
#endif

63
src/main/drivers/compass/compass_mag3110.c
View file

@ -32,7 +32,7 @@
#include "drivers/time.h"
#include "drivers/gpio.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
@ -43,10 +43,8 @@
#include "drivers/compass/compass_mag3110.h"
#define MAG3110_MAG_I2C_ADDRESS 0x0E
// Registers
#define MAG3110_MAG_I2C_ADDRESS 0x0E
#define MAG3110_MAG_REG_STATUS 0x00
#define MAG3110_MAG_REG_HXL 0x01
#define MAG3110_MAG_REG_HXH 0x02
@ -59,18 +57,16 @@
#define MAG3110_MAG_REG_CTRL_REG1 0x10
#define MAG3110_MAG_REG_CTRL_REG2 0x11
static bool mag3110Init(magDev_t *magDev)
static bool mag3110Init(magDev_t * mag)
{
UNUSED(magDev);
bool ack = i2cWrite(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG1, 0x01); // active mode 80 Hz ODR with OSR = 1
delay(20);
bool ack = busWrite(mag->busDev, MAG3110_MAG_REG_CTRL_REG1, 0x01); // active mode 80 Hz ODR with OSR = 1
if (!ack) {
return false;
}
ack = i2cWrite(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
delay(10);
delay(20);
ack = busWrite(mag->busDev, MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
if (!ack) {
return false;
}
@ -80,46 +76,65 @@ static bool mag3110Init(magDev_t *magDev)
#define BIT_STATUS_REG_DATA_READY (1 << 3)
static bool mag3110Read(magDev_t *magDev)
static bool mag3110Read(magDev_t * mag)
{
uint8_t status;
uint8_t buf[6];
// set magData to zero for case of failed read
magDev->magADCRaw[X] = 0;
magDev->magADCRaw[Y] = 0;
magDev->magADCRaw[Z] = 0;
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_STATUS, 1, &status);
bool ack = busRead(mag->busDev, MAG3110_MAG_REG_STATUS, &status);
if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
return false;
}
ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_HXL, 6, buf);
ack = busReadBuf(mag->busDev, MAG3110_MAG_REG_HXL, buf, 6);
if (!ack) {
return false;
}
magDev->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]);
magDev->magADCRaw[Y] = (int16_t)(buf[2] << 8 | buf[3]);
magDev->magADCRaw[Z] = (int16_t)(buf[4] << 8 | buf[5]);
mag->magADCRaw[X] = (int16_t)(buf[0] << 8 | buf[1]);
mag->magADCRaw[Y] = (int16_t)(buf[2] << 8 | buf[3]);
mag->magADCRaw[Z] = (int16_t)(buf[4] << 8 | buf[5]);
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool mag3110detect(magDev_t *magDev)
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
delay(10);
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_WHO_AM_I, 1, &sig);
bool ack = busRead(mag->busDev, MAG3110_MAG_REG_WHO_AM_I, &sig);
if (ack && sig == 0xC4) {
magDev->init = mag3110Init;
magDev->read = mag3110Read;
return true;
}
}
return false;
}
bool mag3110detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MAG3110, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = mag3110Init;
mag->read = mag3110Read;
return true;
}
#endif

319
src/main/drivers/compass/compass_mpu9250.c Executable file
View file

@ -0,0 +1,319 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_mpu9250.h"
#if defined(USE_MAG_MPU9250) && defined(USE_GYRO_MPU9250)
// No separate hardware descriptor needed. Hardware descriptor initialization is handled by GYRO driver
// AK8963, mag sensor address
#define AK8963_MAG_I2C_ADDRESS 0x0C
#define AK8963_DEVICE_ID 0x48
// Registers
#define AK8963_MAG_REG_WHO_AM_I 0x00
#define AK8963_MAG_REG_INFO 0x01
#define AK8963_MAG_REG_STATUS1 0x02
#define AK8963_MAG_REG_HXL 0x03
#define AK8963_MAG_REG_HXH 0x04
#define AK8963_MAG_REG_HYL 0x05
#define AK8963_MAG_REG_HYH 0x06
#define AK8963_MAG_REG_HZL 0x07
#define AK8963_MAG_REG_HZH 0x08
#define AK8963_MAG_REG_STATUS2 0x09
#define AK8963_MAG_REG_CNTL 0x0a
#define AK8963_MAG_REG_ASCT 0x0c // self test
#define AK8963_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_MAG_REG_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
#define READ_FLAG 0x80
#define STATUS1_DATA_READY 0x01
#define STATUS1_DATA_OVERRUN 0x02
#define STATUS2_DATA_ERROR 0x02
#define STATUS2_MAG_SENSOR_OVERFLOW 0x03
#define CNTL_MODE_POWER_DOWN 0x00
#define CNTL_MODE_ONCE 0x01
#define CNTL_MODE_CONT1 0x02
#define CNTL_MODE_CONT2 0x06
#define CNTL_MODE_SELF_TEST 0x08
#define CNTL_MODE_FUSE_ROM 0x0F
#define DETECTION_MAX_RETRY_COUNT 5
typedef enum {
CHECK_STATUS = 0,
WAITING_FOR_STATUS,
WAITING_FOR_DATA
} mpu9250CompassReadState_e;
typedef struct {
mpu9250CompassReadState_e state;
bool waiting;
uint8_t len;
timeUs_t readStartedAt; // time read was queued in micros.
} mpu9250CompassReadContext_s;
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
static mpu9250CompassReadContext_s ctx;
static bool lastReadResult = false;
static int16_t cachedMagData[3];
static bool mpu9250SlaveI2CRead(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len)
{
// Setting up MPU9250's I2C master to read from AK8963 via internal I2C bus
busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr | READ_FLAG); // set I2C slave address for read
busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, len | 0x80); // read number of bytes
delay(10); // wait for transaction to complete
busReadBuf(mag->busDev, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C
return true;
}
static bool mpu9250SlaveI2CWrite(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t data)
{
// Setting up MPU9250's I2C master to write to AK8963 via internal I2C bus
busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr); // set I2C slave address for write
busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
busWrite(mag->busDev, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true;
}
static bool mpu9250SlaveI2CStartRead(magDev_t * mag, uint8_t addr, uint8_t reg, uint8_t len)
{
if (ctx.waiting) {
return false;
}
ctx.len = len;
busWrite(mag->busDev, MPU_RA_I2C_SLV0_ADDR, addr | READ_FLAG); // set I2C slave address for read
busWrite(mag->busDev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
busWrite(mag->busDev, MPU_RA_I2C_SLV0_CTRL, len | 0x80); // read number of bytes
ctx.readStartedAt = micros();
ctx.waiting = true;
return true;
}
static timeDelta_t mpu9250SlaveI2CReadTimeRemaining(void)
{
if (!ctx.waiting) {
return 0;
}
timeDelta_t timeSinceStarted = micros() - ctx.readStartedAt;
timeDelta_t timeRemaining = 8000 - timeSinceStarted;
if (timeRemaining < 0) {
return 0;
}
return timeRemaining;
}
static bool mpu9250SlaveI2CCompleteRead(magDev_t * mag, uint8_t * buf)
{
timeDelta_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
if (timeRemaining > 0) {
delayMicroseconds(timeRemaining);
}
ctx.waiting = false;
busReadBuf(mag->busDev, MPU_RA_EXT_SENS_DATA_00, buf, ctx.len);
return true;
}
static bool mpu9250CompassInit(magDev_t * mag)
{
uint8_t calibration[3];
uint8_t status;
// Do init at low speed
busSetSpeed(mag->busDev, BUS_SPEED_INITIALIZATION);
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
delay(20);
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
delay(10);
mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, calibration, sizeof(calibration)); // Read the x-, y-, and z-axis calibration values
delay(10);
magGain[X] = (((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30);
magGain[Y] = (((((float)(int8_t)calibration[Y] - 128) / 256) + 1) * 30);
magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * 30);
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
// Clear status registers
mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, &status, 1);
mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, &status, 1);
// Trigger first measurement
mpu9250SlaveI2CWrite(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1);
busSetSpeed(mag->busDev, BUS_SPEED_FAST);
return true;
}
static bool mpu9250CompassRead(magDev_t * mag)
{
bool ack = false;
uint8_t buf[7];
// set magData to latest cached value
memcpy(&mag->magADCRaw, cachedMagData, sizeof(cachedMagData));
bool reprocess;
do {
reprocess = false;
switch (ctx.state) {
case CHECK_STATUS:
mpu9250SlaveI2CStartRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
ctx.state = WAITING_FOR_STATUS;
return lastReadResult;
case WAITING_FOR_STATUS: {
uint32_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
if (timeRemaining) {
return lastReadResult;
}
ack = mpu9250SlaveI2CCompleteRead(mag, &buf[0]);
uint8_t status = buf[0];
if (!ack || ((status & STATUS1_DATA_READY) == 0 && (status & STATUS1_DATA_OVERRUN) == 0)) {
// too early. queue the status read again
ctx.state = CHECK_STATUS;
reprocess = true;
debug[0]++;
return lastReadResult;
}
// read the 6 bytes of data and the status2 register
mpu9250SlaveI2CStartRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
ctx.state = WAITING_FOR_DATA;
return lastReadResult;
}
case WAITING_FOR_DATA: {
uint32_t timeRemaining = mpu9250SlaveI2CReadTimeRemaining();
if (timeRemaining) {
return lastReadResult;
}
ack = mpu9250SlaveI2CCompleteRead(mag, &buf[0]);
break;
}
}
} while(reprocess);
uint8_t status2 = buf[6];
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
ctx.state = CHECK_STATUS;
debug[1]++;
lastReadResult = false;
return lastReadResult;
}
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
memcpy(cachedMagData, &mag->magADCRaw, sizeof(cachedMagData));
ctx.state = CHECK_STATUS;
lastReadResult = true;
return lastReadResult;
}
bool mpu9250CompassDetect(magDev_t * mag)
{
// Compass on MPU9250 is only supported if MPU9250 is connected to SPI bus
mag->busDev = busDeviceOpen(BUSTYPE_SPI, DEVHW_MPU9250, 0);
if (mag->busDev == NULL) {
return false;
}
// Check if Gyro driver initialized the chip
if (busDeviceReadScratchpad(mag->busDev) != 0xFFFF9250) {
return false;
}
busSetSpeed(mag->busDev, BUS_SPEED_INITIALIZATION);
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
bool ack = false;
uint8_t sig = 0;
// Initialize I2C master via SPI bus (MPU9250)
busWrite(mag->busDev, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(15);
busWrite(mag->busDev, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(15);
busWrite(mag->busDev, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(15);
// check for AK8963
ack = mpu9250SlaveI2CRead(mag, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, &sig, 1);
if (ack && sig == AK8963_DEVICE_ID) { // 0x48 / 01001000 / 'H'
mag->init = mpu9250CompassInit;
mag->read = mpu9250CompassRead;
return true;
}
}
busSetSpeed(mag->busDev, BUS_SPEED_FAST);
return false;
}
#endif

13
src/main/drivers/accgyro/accgyro_spi_mpu6500.h → src/main/drivers/compass/compass_mpu9250.h
View file

@ -17,15 +17,4 @@
#pragma once
#include "drivers/sensor.h"
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
void mpu6500SpiGyroInit(gyroDev_t *gyro);
void mpu6500SpiAccInit(accDev_t *acc);
bool mpu9250CompassDetect(magDev_t *mag);

61
src/main/drivers/compass/compass_qmc5883l.c
View file

@ -81,68 +81,81 @@
#define QMC5883L_REG_ID 0x0D
#define QMC5883_ID_VAL 0xFF
static bool qmc5883Init(magDev_t *magDev)
static bool qmc5883Init(magDev_t * mag)
{
UNUSED(magDev);
bool ack = true;
ack = ack && i2cWrite(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, 0x0B, 0x01);
ack = ack && busWrite(mag->busDev, 0x0B, 0x01);
// ack = ack && i2cWrite(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, 0x20, 0x40);
// ack = ack && i2cWrite(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, 0x21, 0x01);
ack = ack && i2cWrite(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
ack = ack && busWrite(mag->busDev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
if (!ack) {
return false;
return ack;
}
return true;
}
static bool qmc5883Read(magDev_t *magDev)
static bool qmc5883Read(magDev_t * mag)
{
uint8_t status;
uint8_t buf[6];
// set magData to zero for case of failed read
magDev->magADCRaw[X] = 0;
magDev->magADCRaw[Y] = 0;
magDev->magADCRaw[Z] = 0;
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, QMC5883L_REG_STATUS, 1, &status);
bool ack = busRead(mag->busDev, QMC5883L_REG_STATUS, &status);
if (!ack || (status & 0x04) == 0) {
return false;
}
ack = i2cRead(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, QMC5883L_REG_DATA_OUTPUT_X, 6, buf);
ack = busReadBuf(mag->busDev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
if (!ack) {
return false;
}
magDev->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]);
magDev->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]);
magDev->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]);
mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]);
mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]);
mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]);
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
bool qmc5883Detect(magDev_t *magDev)
static bool deviceDetect(magDev_t * mag)
{
// Must write reset first - don't care about the result
i2cWrite(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, QMC5883L_REG_CONF2, QMC5883L_RST);
busWrite(mag->busDev, QMC5883L_REG_CONF2, QMC5883L_RST);
delay(20);
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
delay(10);
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, QMC5883L_MAG_I2C_ADDRESS, QMC5883L_REG_ID, 1, &sig);
bool ack = busRead(mag->busDev, QMC5883L_REG_ID, &sig);
if (ack && sig == QMC5883_ID_VAL) {
magDev->init = qmc5883Init;
magDev->read = qmc5883Read;
return true;
}
}
return false;
}
bool qmc5883Detect(magDev_t * mag)
{
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_QMC5883, 0, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = qmc5883Init;
mag->read = qmc5883Read;
return true;
}
#endif

75
src/main/drivers/display.c
View file

@ -23,8 +23,57 @@
#include "common/utils.h"
#include "config/parameter_group_ids.h"
#include "drivers/time.h"
#include "display.h"
#define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off
// XXX: This is the number of characters in a MAX7456 line.
// Increment this number appropiately or enable support for
// multiple iterations in displayWriteWithAttr() if bigger
// displays are supported (implementation can be found in commit
// 22a48278 before it was deleted).
#define DISPLAY_MAX_STRING_SIZE 30
PG_REGISTER_WITH_RESET_TEMPLATE(displayConfig_t, displayConfig, PG_DISPLAY_CONFIG, 0);
PG_RESET_TEMPLATE(displayConfig_t, displayConfig,
.force_sw_blink = false,
);
static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttributes_t attr)
{
if (attr & ~instance->cachedSupportedTextAttributes) {
// We only emulate blink for now
return TEXT_ATTRIBUTES_HAVE_BLINK(attr);
}
return false;
}
static bool displayEmulateTextAttributes(displayPort_t *instance,
char *buf,
const char *s, size_t length,
textAttributes_t *attr)
{
UNUSED(instance);
UNUSED(s);
// We only emulate blink for now, so there's no need to test
// for it again.
TEXT_ATTRIBUTES_REMOVE_BLINK(*attr);
if ((millis() / SW_BLINK_CYCLE_MS) % 2) {
memset(buf, ' ', length);
buf[length] = '\0';
// Tell the caller to use buf
return true;
}
// Tell the caller to use s but with the updated attributes
return false;
}
void displayClearScreen(displayPort_t *instance)
{
instance->vTable->clearScreen(instance);
@ -85,8 +134,22 @@ int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s)
int displayWriteWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, const char *s, textAttributes_t attr)
{
instance->posX = x + strlen(s);
size_t length = strlen(s);
char buf[DISPLAY_MAX_STRING_SIZE + 1];
instance->posX = x + length;
instance->posY = y;
if (displayAttributesRequireEmulation(instance, attr)) {
// We can't overwrite s, so we use an intermediate buffer if we need
// text attribute emulation.
size_t blockSize = length > sizeof(buf) ? sizeof(buf) : length;
if (displayEmulateTextAttributes(instance, buf, s, blockSize, &attr)) {
// Emulation required rewriting the string, use buf.
s = buf;
}
}
return instance->vTable->writeString(instance, x, y, s, attr);
}
@ -99,6 +162,9 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c)
int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c, textAttributes_t attr)
{
if (displayAttributesRequireEmulation(instance, attr)) {
displayEmulateTextAttributes(instance, (char *)&c, (char *)&c, 1, &attr);
}
instance->posX = x + 1;
instance->posY = y;
return instance->vTable->writeChar(instance, x, y, c, attr);
@ -131,5 +197,12 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
instance->cleared = true;
instance->grabCount = 0;
instance->cursorRow = -1;
instance->cachedSupportedTextAttributes = TEXT_ATTRIBUTES_NONE;
if (vTable->supportedTextAttributes) {
instance->cachedSupportedTextAttributes = vTable->supportedTextAttributes(instance);
}
if (displayConfig()->force_sw_blink) {
TEXT_ATTRIBUTES_REMOVE_BLINK(instance->cachedSupportedTextAttributes);
}
}

22
src/main/drivers/display.h
View file

@ -20,6 +20,14 @@
#include <stdbool.h>
#include <stdint.h>
#include "config/parameter_group.h"
typedef struct displayConfig_s {
bool force_sw_blink; // Enable SW blinking. Used for chips which don't work correctly with HW blink.
} displayConfig_t;
PG_DECLARE(displayConfig_t, displayConfig);
// Represents the attributes for a given piece of text
// either a single character or a string. For forward
// compatibility, always use the TEXT_ATTRIBUTE...
@ -32,14 +40,20 @@ typedef uint8_t textAttributes_t;
#define _TEXT_ATTRIBUTES_SOLID_BG_BIT (1 << 2)
#define TEXT_ATTRIBUTES_NONE 0
#define TEXT_ATTRIBUTES_ADD_BLINK(x) (x |= _TEXT_ATTRIBUTES_BLINK_BIT)
#define TEXT_ATTRIBUTES_ADD_INVERTED(x) (x |= _TEXT_ATTRIBUTES_INVERTED_BIT)
#define TEXT_ATTRIBUTES_ADD_SOLID_BG(x) (x |= _TEXT_ATTRIBUTES_SOLID_BG_BIT)
#define TEXT_ATTRIBUTES_ADD_BLINK(x) ((x) |= _TEXT_ATTRIBUTES_BLINK_BIT)
#define TEXT_ATTRIBUTES_ADD_INVERTED(x) ((x) |= _TEXT_ATTRIBUTES_INVERTED_BIT)
#define TEXT_ATTRIBUTES_ADD_SOLID_BG(x) ((x) |= _TEXT_ATTRIBUTES_SOLID_BG_BIT)
#define TEXT_ATTRIBUTES_REMOVE_BLINK(x) ((x) &= ~_TEXT_ATTRIBUTES_BLINK_BIT)
#define TEXT_ATTRIBUTES_REMOVE_INVERTED(x) ((x) &= ~_TEXT_ATTRIBUTES_INVERTED_BIT)
#define TEXT_ATTRIBUTES_REMOVE_SOLID_BG(x) ((x) &= ~_TEXT_ATTRIBUTES_SOLID_BG_BIT)
#define TEXT_ATTRIBUTES_HAVE_BLINK(x) (x & _TEXT_ATTRIBUTES_BLINK_BIT)
#define TEXT_ATTRIBUTES_HAVE_INVERTED(x) (x & _TEXT_ATTRIBUTES_INVERTED_BIT)
#define TEXT_ATTRIBUTES_HAVE_SOLID_BG(x) (x & _TEXT_ATTRIBUTES_SOLID_BG_BIT)
static inline void TEXT_ATTRIBUTES_COPY(textAttributes_t *dst, textAttributes_t *src) { *dst = *src; }
struct displayPortVTable_s;
typedef struct displayPort_s {
const struct displayPortVTable_s *vTable;
@ -53,6 +67,7 @@ typedef struct displayPort_s {
bool cleared;
int8_t cursorRow;
int8_t grabCount;
textAttributes_t cachedSupportedTextAttributes;
} displayPort_t;
typedef struct displayPortVTable_s {
@ -67,6 +82,7 @@ typedef struct displayPortVTable_s {
int (*heartbeat)(displayPort_t *displayPort);
void (*resync)(displayPort_t *displayPort);
uint32_t (*txBytesFree)(const displayPort_t *displayPort);
textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort);
} displayPortVTable_t;
typedef struct displayPortProfile_s {

371
src/main/drivers/max7456.c
View file

@ -72,16 +72,16 @@
#define BLINK_TIME_3 0x0C
// background mode brightness (percent)
#define BACKGROUND_BRIGHTNESS_0 0x00
#define BACKGROUND_BRIGHTNESS_7 0x01
#define BACKGROUND_BRIGHTNESS_14 0x02
#define BACKGROUND_BRIGHTNESS_21 0x03
#define BACKGROUND_BRIGHTNESS_28 0x04
#define BACKGROUND_BRIGHTNESS_35 0x05
#define BACKGROUND_BRIGHTNESS_42 0x06
#define BACKGROUND_BRIGHTNESS_49 0x07
#define BACKGROUND_BRIGHTNESS_0 (0x00 << 4)
#define BACKGROUND_BRIGHTNESS_7 (0x01 << 4)
#define BACKGROUND_BRIGHTNESS_14 (0x02 << 4)
#define BACKGROUND_BRIGHTNESS_21 (0x03 << 4)
#define BACKGROUND_BRIGHTNESS_28 (0x04 << 4)
#define BACKGROUND_BRIGHTNESS_35 (0x05 << 4)
#define BACKGROUND_BRIGHTNESS_42 (0x06 << 4)
#define BACKGROUND_BRIGHTNESS_49 (0x07 << 4)
#define BACKGROUND_MODE_GRAY 0x40
#define BACKGROUND_MODE_GRAY 0x80
// STAT register bits
@ -159,30 +159,6 @@
#define CHAR_BYTE(x) (x >> 8)
#define MODE_BYTE(x) (x & 0xFF)
// On shared SPI buss we want to change clock for OSD chip and restore for other devices.
#ifdef MAX7456_SPI_CLK
#define ENABLE_MAX7456() do { \
spiSetSpeed(MAX7456_SPI_INSTANCE, MAX7456_SPI_CLK); \
IOLo(max7456CsPin); \
} while(0)
#else
#define ENABLE_MAX7456() do { \
IOLo(max7456CsPin); \
} while(0)
#endif
#ifdef MAX7456_RESTORE_CLK
#define DISABLE_MAX7456() do { \
IOHi(max7456CsPin); \
spiSetSpeed(MAX7456_SPI_INSTANCE, MAX7456_RESTORE_CLK); \
} while(0)
#else
#define DISABLE_MAX7456() do { \
IOHi(max7456CsPin); \
} while(0)
#endif
uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
// we write everything in screenBuffer and set a dirty bit
@ -194,139 +170,20 @@ static BITARRAY_DECLARE(screenIsDirty, VIDEO_BUFFER_CHARS_PAL);
//max chars to update in one idle
#define MAX_CHARS2UPDATE 10
#define BYTES_PER_CHAR2UPDATE 8 // [3-4] spi regs + values for them
#ifdef MAX7456_DMA_CHANNEL_TX
volatile bool dmaTransactionInProgress = false;
#endif
#define SPI_BUFF_SIZE (MAX_CHARS2UPDATE*BYTES_PER_CHAR2UPDATE)
static uint8_t spiBuff[SPI_BUFF_SIZE];
static uint8_t videoSignalCfg = 0;
static uint8_t videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE; //PAL by default
static bool max7456Lock = false;
static bool fontIsLoading = false;
static IO_t max7456CsPin = IO_NONE;
static busDevice_t * max7456dev = NULL;
// Register bus device descriptor
BUSDEV_REGISTER_SPI(max7456_busdev, DEVHW_MAX7456, 0, MAX7456_SPI_CS_PIN);
static uint8_t max7456Send(uint8_t add, uint8_t data)
static int max7456PrepareBuffer(uint8_t * buf, int bufPtr, uint8_t add, uint8_t data)
{
spiTransferByte(MAX7456_SPI_INSTANCE, add);
return spiTransferByte(MAX7456_SPI_INSTANCE, data);
buf[bufPtr++] = add;
buf[bufPtr++] = data;
return bufPtr;
}
#ifdef MAX7456_DMA_CHANNEL_TX
static void max7456SendDma(void* tx_buffer, void* rx_buffer, uint16_t buffer_size)
{
DMA_InitTypeDef DMA_InitStructure;
#ifdef MAX7456_DMA_CHANNEL_RX
static uint16_t dummy[] = {0xffff};
#else
UNUSED(rx_buffer);
#endif
while (dmaTransactionInProgress); // Wait for prev DMA transaction
DMA_DeInit(MAX7456_DMA_CHANNEL_TX);
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_DeInit(MAX7456_DMA_CHANNEL_RX);
#endif
// Common to both channels
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(MAX7456_SPI_INSTANCE->DR));
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_BufferSize = buffer_size;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
#ifdef MAX7456_DMA_CHANNEL_RX
// Rx Channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
#endif
DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE);
#endif
// Tx channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
#endif
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE);
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_ITConfig(MAX7456_DMA_CHANNEL_RX, DMA_IT_TC, ENABLE);
#else
DMA_ITConfig(MAX7456_DMA_CHANNEL_TX, DMA_IT_TC, ENABLE);
#endif
// Enable SPI TX/RX request
ENABLE_MAX7456();
dmaTransactionInProgress = true;
SPI_I2S_DMACmd(MAX7456_SPI_INSTANCE,
#ifdef MAX7456_DMA_CHANNEL_RX
SPI_I2S_DMAReq_Rx |
#endif
SPI_I2S_DMAReq_Tx, ENABLE);
}
void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_Cmd(MAX7456_DMA_CHANNEL_RX, DISABLE);
#endif
// make sure spi dmd transfer is complete
while (SPI_I2S_GetFlagStatus (MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_TXE) == RESET) {};
while (SPI_I2S_GetFlagStatus (MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_BSY) == SET) {};
//Empty RX buffer. RX DMA takes care of it if enabled
//this should be done after transmission finish!!!
while (SPI_I2S_GetFlagStatus(MAX7456_SPI_INSTANCE, SPI_I2S_FLAG_RXNE) == SET) {
MAX7456_SPI_INSTANCE->DR;
}
DMA_Cmd(MAX7456_DMA_CHANNEL_TX, DISABLE);
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
SPI_I2S_DMACmd(MAX7456_SPI_INSTANCE,
#ifdef MAX7456_DMA_CHANNEL_RX
SPI_I2S_DMAReq_Rx |
#endif
SPI_I2S_DMAReq_Tx, DISABLE);
DISABLE_MAX7456();
dmaTransactionInProgress = false;
}
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_HTIF)) {
DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF);
}
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) {
DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
}
}
#endif
uint8_t max7456GetRowsCount(void)
{
if (videoSignalReg & VIDEO_MODE_PAL)
@ -340,17 +197,21 @@ uint8_t max7456GetRowsCount(void)
//it might already have some data by the first time this function is called.
void max7456ReInit(void)
{
uint8_t buf[(VIDEO_BUFFER_CHARS_PAL + 3) * 2];
int bufPtr;
uint8_t maxScreenRows;
uint8_t srdata = 0;
uint16_t x;
static bool firstInit = true;
// Check if device is available
if (max7456dev == NULL) {
return;
}
//do not init MAX before camera power up correctly
if (millis() < 1500)
return;
ENABLE_MAX7456();
switch (videoSignalCfg) {
case PAL:
videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
@ -359,7 +220,7 @@ void max7456ReInit(void)
videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
break;
default:
srdata = max7456Send(MAX7456ADD_STAT, 0x00);
busRead(max7456dev, MAX7456ADD_STAT, &srdata);
if ((0x02 & srdata) == 0x02)
videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
}
@ -373,19 +234,22 @@ void max7456ReInit(void)
}
// set all rows to same charactor black/white level
for (x = 0; x < maxScreenRows; x++) {
max7456Send(MAX7456ADD_RB0 + x, BWBRIGHTNESS);
bufPtr = 0;
for (int x = 0; x < maxScreenRows; x++) {
bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_RB0 + x, BWBRIGHTNESS);
}
// make sure the Max7456 is enabled
max7456Send(MAX7456ADD_VM0, videoSignalReg);
max7456Send(MAX7456ADD_DMM, DMM_CLEAR_DISPLAY);
DISABLE_MAX7456();
bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM0, videoSignalReg);
bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_VM1, BLINK_DUTY_CYCLE_50_50 | BLINK_TIME_3 | BACKGROUND_BRIGHTNESS_28);
bufPtr = max7456PrepareBuffer(buf, bufPtr, MAX7456ADD_DMM, DMM_CLEAR_DISPLAY);
// Transfer data to SPI
busTransfer(max7456dev, NULL, buf, bufPtr);
// force redrawing all screen in non-dma mode
memset(screenIsDirty, 0xFF, sizeof(screenIsDirty));
if (firstInit)
{
if (firstInit) {
max7456RefreshAll();
firstInit = false;
}
@ -395,17 +259,17 @@ void max7456ReInit(void)
//here we init only CS and try to init MAX for first time
void max7456Init(const vcdProfile_t *pVcdProfile)
{
#ifdef MAX7456_SPI_CS_PIN
max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0);
IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);
max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD);
if (max7456dev == NULL) {
return;
}
busSetSpeed(max7456dev, BUS_SPEED_STANDARD);
spiSetSpeed(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
// force soft reset on Max7456
ENABLE_MAX7456();
max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
DISABLE_MAX7456();
busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET);
videoSignalCfg = pVcdProfile->video_system;
// Set screenbuffer to all blanks
@ -413,9 +277,6 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
screenBuffer[ii] = CHAR_BLANK;
}
#ifdef MAX7456_DMA_CHANNEL_TX
dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif
//real init will be made letter when driver idle detect
}
@ -457,15 +318,6 @@ void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode)
}
}
bool max7456DmaInProgress(void)
{
#ifdef MAX7456_DMA_CHANNEL_TX
return dmaTransactionInProgress;
#else
return false;
#endif
}
void max7456DrawScreenPartial(void)
{
static uint32_t lastSigCheckMs = 0;
@ -478,17 +330,21 @@ void max7456DrawScreenPartial(void)
uint8_t videoSense;
uint32_t nowMs;
int pos;
int buff_len = 0;
uint_fast16_t updatedCharCount;
uint8_t currentMode;
// Check if device is available
if (max7456dev == NULL) {
return;
}
if (!max7456Lock && !fontIsLoading) {
// (Re)Initialize MAX7456 at startup or stall is detected.
max7456Lock = true;
ENABLE_MAX7456();
stallCheck = max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00);
DISABLE_MAX7456();
// Stall check
busRead(max7456dev, MAX7456ADD_VM0 | MAX7456ADD_READ, &stallCheck);
nowMs = millis();
@ -498,10 +354,7 @@ void max7456DrawScreenPartial(void)
&& ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
// Adjust output format based on the current input format.
ENABLE_MAX7456();
videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
DISABLE_MAX7456();
busRead(max7456dev, MAX7456ADD_STAT, &videoSense);
if (videoSense & STAT_LOS) {
videoDetectTimeMs = 0;
@ -523,6 +376,8 @@ void max7456DrawScreenPartial(void)
}
//------------ end of (re)init-------------------------------------
uint8_t spiBuff[MAX_CHARS2UPDATE * BYTES_PER_CHAR2UPDATE];
int bufPtr = 0;
for (pos = 0, updatedCharCount = 0;;) {
pos = BITARRAY_FIND_FIRST_SET(screenIsDirty, pos);
@ -530,22 +385,22 @@ void max7456DrawScreenPartial(void)
// No more dirty chars.
break;
}
currentMode = MODE_BYTE(screenBuffer[pos]);
// Found one dirty character to send
if (setMode != currentMode) {
setMode = currentMode;
// Send the attributes for the character run. They
// will be applied to all characters until we change
// the DMM register.
spiBuff[buff_len++] = MAX7456ADD_DMM;
spiBuff[buff_len++] = currentMode;
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, currentMode);
}
spiBuff[buff_len++] = MAX7456ADD_DMAH;
spiBuff[buff_len++] = pos >> 8;
spiBuff[buff_len++] = MAX7456ADD_DMAL;
spiBuff[buff_len++] = pos & 0xff;
spiBuff[buff_len++] = MAX7456ADD_DMDI;
spiBuff[buff_len++] = CHAR_BYTE(screenBuffer[pos]);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, pos >> 8);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, pos & 0xff);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[pos]));
bitArrayClr(screenIsDirty, pos);
if (++updatedCharCount == MAX_CHARS2UPDATE) {
break;
@ -554,16 +409,10 @@ void max7456DrawScreenPartial(void)
pos++;
}
if (buff_len) {
#ifdef MAX7456_DMA_CHANNEL_TX
if (buff_len > 0)
max7456SendDma(spiBuff, NULL, buff_len);
#else
ENABLE_MAX7456();
spiTransfer(MAX7456_SPI_INSTANCE, NULL, spiBuff, buff_len);
DISABLE_MAX7456();
#endif // MAX7456_DMA_CHANNEL_TX
if (bufPtr) {
busTransfer(max7456dev, NULL, spiBuff, bufPtr);
}
max7456Lock = false;
}
}
@ -573,33 +422,43 @@ void max7456DrawScreenPartial(void)
// when copter is armed.
void max7456RefreshAll(void)
{
uint8_t spiBuff[(VIDEO_BUFFER_CHARS_PAL + 4) * 2];
int bufPtr;
// Check if device is available
if (max7456dev == NULL) {
return;
}
if (!max7456Lock) {
#ifdef MAX7456_DMA_CHANNEL_TX
while (dmaTransactionInProgress);
#endif
uint16_t xx;
max7456Lock = true;
ENABLE_MAX7456();
// Write characters. Start at character zero.
max7456Send(MAX7456ADD_DMAH, 0);
max7456Send(MAX7456ADD_DMAL, 0);
// Enable auto-increment mode
max7456Send(MAX7456ADD_DMM, DMM_AUTOINCREMENT);
bufPtr = 0;
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 0);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT);
for (xx = 0; xx < maxScreenSize; ++xx) {
max7456Send(MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx]));
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, CHAR_BYTE(screenBuffer[xx]));
}
// Exit auto-increment mode by writing the 0xFF escape
// sequence to DMDI.
max7456Send(MAX7456ADD_DMDI, 0xFF);
// Exit auto-increment mode by writing the 0xFF escape sequence to DMDI.
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
// Write chunk of data
busTransfer(max7456dev, NULL, spiBuff, bufPtr);
// Write character attributes. Start at zero, but
// set DMAH[1] = 1, to signal that we're sending
// attributes rather than characters. Process is the
// same as for the characters a few lines up.
max7456Send(MAX7456ADD_DMAH, 1<<1);
max7456Send(MAX7456ADD_DMAL, 0);
max7456Send(MAX7456ADD_DMM, DMM_AUTOINCREMENT);
bufPtr = 0;
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAH, 1<<1);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMAL, 0);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMM, DMM_AUTOINCREMENT);
for (xx = 0; xx < maxScreenSize; ++xx) {
// Note that atttribute bits in DMDI are in different
// positions than in DMM (DMM is used for partial writes),
@ -612,11 +471,14 @@ void max7456RefreshAll(void)
//
// Thus, we need to shift the bits by 2 when writing character
// attributes to DMDI.
max7456Send(MAX7456ADD_DMDI, MODE_BYTE(screenBuffer[xx]) << 2);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, MODE_BYTE(screenBuffer[xx]) << 2);
}
max7456Send(MAX7456ADD_DMDI, 0xFF);
DISABLE_MAX7456();
// Exit auto-increment mode by writing the 0xFF escape sequence to DMDI.
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_DMDI, 0xFF);
// Write chunk of data
busTransfer(max7456dev, NULL, spiBuff, bufPtr);
// All characters have been set to the MAX7456, none is dirty now.
memset(screenIsDirty, 0, sizeof(screenIsDirty));
@ -626,24 +488,41 @@ void max7456RefreshAll(void)
void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
{
uint8_t x;
uint8_t spiBuff[(54 * 2 + 3) * 2];
int bufPtr = 0;
// Check if device is available
if (max7456dev == NULL) {
return;
}
#ifdef MAX7456_DMA_CHANNEL_TX
while (dmaTransactionInProgress);
#endif
while (max7456Lock);
max7456Lock = true;
ENABLE_MAX7456();
// disable display
fontIsLoading = true;
max7456Send(MAX7456ADD_VM0, 0);
max7456Send(MAX7456ADD_CMAH, char_address); // set start address high
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_VM0, 0);
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAH, char_address); // set start address high
for (int x = 0; x < 54; x++) {
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x); //set start address low
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, font_data[x]);
}
// transfer 54 bytes from shadow ram to NVM
bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMM, WRITE_NVR);
busTransfer(max7456dev, NULL, spiBuff, bufPtr);
while (1) {
uint8_t status;
busRead(max7456dev, MAX7456ADD_STAT, &status);
if ((status & STAT_NVR_BUSY) == 0x00) {
break;
}
for (x = 0; x < 54; x++) {
max7456Send(MAX7456ADD_CMAL, x); //set start address low
max7456Send(MAX7456ADD_CMDI, font_data[x]);
#ifdef LED0_TOGGLE
LED0_TOGGLE;
#else
@ -651,14 +530,6 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
#endif
}
// transfer 54 bytes from shadow ram to NVM
max7456Send(MAX7456ADD_CMM, WRITE_NVR);
// wait until bit 5 in the status register returns to 0 (12ms)
while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00);
DISABLE_MAX7456();
max7456Lock = false;
}

1
src/main/drivers/max7456.h
View file

@ -50,4 +50,3 @@ void max7456WriteChar(uint8_t x, uint8_t y, uint8_t c, uint8_t mode);
void max7456ClearScreen(void);
void max7456RefreshAll(void);
uint8_t* max7456GetScreenBuffer(void);
bool max7456DmaInProgress(void);

4
src/main/drivers/rangefinder.h → src/main/drivers/rangefinder/rangefinder.h
View file

@ -18,8 +18,8 @@
#pragma once
#include "common/time.h"
#include "drivers/io.h"
#include "drivers/bus.h"
#define RANGEFINDER_OUT_OF_RANGE (-1)
#define RANGEFINDER_HARDWARE_FAILURE (-2)
@ -39,6 +39,8 @@ typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev);
typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev);
typedef struct rangefinderDev_s {
busDevice_t * busDev; // Device on a bus (if applicable)
timeMs_t delayMs;
int16_t maxRangeCm;

6
src/main/drivers/rangefinder_hcsr04.c → src/main/drivers/rangefinder/rangefinder_hcsr04.c
View file

@ -31,12 +31,12 @@
#include "drivers/io.h"
#include "drivers/gpio.h"
#include "drivers/nvic.h"
#include "rcc.h"
#include "drivers/rcc.h"
#include "drivers/logging.h"
#include "drivers/rangefinder.h"
#include "drivers/rangefinder_hcsr04.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet

2
src/main/drivers/rangefinder_hcsr04.h → src/main/drivers/rangefinder/rangefinder_hcsr04.h
View file

@ -17,7 +17,7 @@
#pragma once
#include "drivers/rangefinder.h"
#include "drivers/rangefinder/rangefinder.h"
#define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70

74
src/main/drivers/rangefinder_hcsr04_i2c.c → src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.c
View file

@ -27,15 +27,11 @@
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder.h"
#include "drivers/rangefinder_hcsr04_i2c.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_hcsr04_i2c.h"
#include "build/debug.h"
#ifndef RANGEFINDER_HCSR04_I2C_I2C_INSTANCE
#define RANGEFINDER_HCSR04_I2C_I2C_INSTANCE I2C_DEVICE
#endif
#define HCSR04_I2C_MAX_RANGE_CM 400
#define HCSR04_I2C_DETECTION_CONE_DECIDEGREES 300
#define HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450
@ -49,23 +45,16 @@
volatile int32_t hcsr04i2cMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
static bool isHcsr04i2cResponding = false;
static uint8_t hcsr04i2cReadByte(uint8_t registry) {
uint8_t buffer;
isHcsr04i2cResponding = i2cRead(I2C_DEVICE, HCSR04_I2C_Address, registry, 1, &buffer);
return buffer;
static void hcsr04i2cInit(rangefinderDev_t *rangefinder)
{
UNUSED(rangefinder);
}
static void hcsr04i2cInit(rangefinderDev_t *dev)
void hcsr04i2cUpdate(rangefinderDev_t *rangefinder)
{
UNUSED(dev);
}
void hcsr04i2cUpdate(rangefinderDev_t *dev)
{
UNUSED(dev);
uint8_t response[3];
isHcsr04i2cResponding = busReadBuf(rangefinder->busDev, HCSR04_I2C_REGISTRY_STATUS, response, 3);
isHcsr04i2cResponding = i2cRead(I2C_DEVICE, HCSR04_I2C_Address, HCSR04_I2C_REGISTRY_STATUS, 3, response);
if (!isHcsr04i2cResponding) {
@ -90,29 +79,48 @@ void hcsr04i2cUpdate(rangefinderDev_t *dev)
/**
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t hcsr04i2cGetDistance(rangefinderDev_t *dev)
int32_t hcsr04i2cGetDistance(rangefinderDev_t *rangefinder)
{
UNUSED(dev);
UNUSED(rangefinder);
return hcsr04i2cMeasurementCm;
}
bool hcsr04i2c0Detect(rangefinderDev_t *dev)
static bool deviceDetect(busDevice_t * busDev)
{
hcsr04i2cReadByte(HCSR04_I2C_REGISTRY_STATUS);
if (isHcsr04i2cResponding) {
dev->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS;
dev->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES;
dev->init = &hcsr04i2cInit;
dev->update = &hcsr04i2cUpdate;
dev->read = &hcsr04i2cGetDistance;
for (int retry = 0; retry < 5; retry++) {
uint8_t inquiryResult;
delay(150);
bool ack = busRead(busDev, HCSR04_I2C_REGISTRY_STATUS, &inquiryResult);
if (ack) {
return true;
} else {
}
};
return false;
}
bool hcsr04i2c0Detect(rangefinderDev_t *rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_HCSR04_I2C, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS;
rangefinder->maxRangeCm = HCSR04_I2C_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = HCSR04_I2C_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = HCSR04_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &hcsr04i2cInit;
rangefinder->update = &hcsr04i2cUpdate;
rangefinder->read = &hcsr04i2cGetDistance;
return true;
}
#endif

0
src/main/drivers/rangefinder_hcsr04_i2c.h → src/main/drivers/rangefinder/rangefinder_hcsr04_i2c.h
View file

107
src/main/drivers/rangefinder_srf10.c → src/main/drivers/rangefinder/rangefinder_srf10.c
View file

@ -28,8 +28,8 @@
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder.h"
#include "drivers/rangefinder_srf10.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_srf10.h"
#ifndef SRF10_I2C_INSTANCE
#define SRF10_I2C_INSTANCE I2CDEV_1
@ -93,37 +93,14 @@ static int16_t minimumFiringIntervalMs;
static uint32_t timeOfLastMeasurementMs;
static bool isSensorResponding = true;
#ifdef UNIT_TEST
bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) {UNUSED(addr_); UNUSED(reg); UNUSED(data); return false;}
bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) {UNUSED(addr_); UNUSED(reg);UNUSED(len); UNUSED(buf); return false;}
#endif
static bool i2c_srf10_send_command(uint8_t command)
static void srf10_init(rangefinderDev_t * rangefinder)
{
isSensorResponding = i2cWrite(SRF10_I2C_INSTANCE, SRF10_AddressI2C, SRF10_WRITE_CommandRegister, command);
return isSensorResponding;
}
busWrite(rangefinder->busDev, SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max);
busWrite(rangefinder->busDev, SRF10_WRITE_RangeRegister, SRF10_RangeValue6m);
static bool i2c_srf10_send_byte(uint8_t i2cRegister, uint8_t val)
{
isSensorResponding = i2cWrite(SRF10_I2C_INSTANCE, SRF10_AddressI2C, i2cRegister, val);
return isSensorResponding;
}
static uint8_t i2c_srf10_read_byte(uint8_t i2cRegister)
{
uint8_t byte;
isSensorResponding = i2cRead(SRF10_I2C_INSTANCE, SRF10_AddressI2C, i2cRegister, 1, &byte);
return byte;
}
static void srf10_init(rangefinderDev_t *dev)
{
UNUSED(dev);
i2c_srf10_send_byte(SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max);
i2c_srf10_send_byte(SRF10_WRITE_RangeRegister, SRF10_RangeValue6m);
// initiate first ranging command
i2c_srf10_send_command(SRF10_COMMAND_InitiateRangingCm);
busWrite(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm);
timeOfLastMeasurementMs = millis();
}
@ -131,26 +108,33 @@ static void srf10_init(rangefinderDev_t *dev)
* Start a range reading
* Called periodically by the scheduler
*/
static void srf10_start_reading(rangefinderDev_t *dev)
static void srf10_start_reading(rangefinderDev_t * rangefinder)
{
UNUSED(dev);
uint8_t revision;
// check if there is a measurement outstanding, 0xFF is returned if no measurement
const uint8_t revision = i2c_srf10_read_byte(SRF10_READ_SoftwareRevision);
if (revision != 0xFF) {
isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_SoftwareRevision, &revision);
if (isSensorResponding && revision != 0xFF) {
// there is a measurement
const uint8_t lowByte = i2c_srf10_read_byte(SRF10_READ_RangeLowByte);
const uint8_t highByte = i2c_srf10_read_byte(SRF10_READ_RangeHighByte);
uint8_t lowByte, highByte;
isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeLowByte, &lowByte);
isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeHighByte, &highByte);
srf10measurementCm = highByte << 8 | lowByte;
if (srf10measurementCm > SRF10_MAX_RANGE_CM) {
srf10measurementCm = RANGEFINDER_OUT_OF_RANGE;
}
}
const timeMs_t timeNowMs = millis();
if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) {
// measurement repeat interval should be greater than minimumFiringIntervalMs
// to avoid interference between connective measurements.
timeOfLastMeasurementMs = timeNowMs;
i2c_srf10_send_command(SRF10_COMMAND_InitiateRangingCm);
busWrite(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm);
}
}
@ -168,23 +152,44 @@ static int32_t srf10_get_distance(rangefinderDev_t *dev)
}
}
bool srf10Detect(rangefinderDev_t *dev)
static bool deviceDetect(busDevice_t * busDev)
{
const uint8_t value = i2c_srf10_read_byte(SRF10_READ_Unused);
if (value == SRF10_READ_Unused_ReturnValue) {
dev->delayMs = SRF10_MinimumFiringIntervalFor600cmRangeMs + 10; // set up the SRF10 hardware for a range of 6m + margin of 10ms
dev->maxRangeCm = SRF10_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
for (int retry = 0; retry < 5; retry++) {
uint8_t inquiryResult;
dev->init = &srf10_init;
dev->update = &srf10_start_reading;
dev->read = &srf10_get_distance;
delay(150);
bool ack = busRead(busDev, SRF10_READ_Unused, &inquiryResult);
if (ack && inquiryResult == SRF10_READ_Unused_ReturnValue) {
return true;
}
};
return false;
}
bool srf10Detect(rangefinderDev_t * rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_SRF10, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = SRF10_MinimumFiringIntervalFor600cmRangeMs + 10; // set up the SRF10 hardware for a range of 6m + margin of 10ms
rangefinder->maxRangeCm = SRF10_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &srf10_init;
rangefinder->update = &srf10_start_reading;
rangefinder->read = &srf10_get_distance;
return true;
}
else {
return false;
}
}
#endif

0
src/main/drivers/rangefinder_srf10.h → src/main/drivers/rangefinder/rangefinder_srf10.h
View file

566
src/main/drivers/rangefinder_vl53l0x.c → src/main/drivers/rangefinder/rangefinder_vl53l0x.c
View file

@ -108,15 +108,11 @@
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder.h"
#include "drivers/rangefinder_vl53l0x.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_vl53l0x.h"
#include "build/debug.h"
#ifndef RANGEFINDER_VL53L0X_INSTANCE
#define RANGEFINDER_VL53L0X_INSTANCE I2C_DEVICE
#endif
#define VL53L0X_MAX_RANGE_CM 250
#define VL53L0X_DETECTION_CONE_DECIDEGREES 900
@ -258,52 +254,54 @@ static timeMs_t timeoutStartMs = 0;
static timeMs_t timeoutValueMs = 0;
static measurementSteps_e measSteps = MEASUREMENT_START;
static uint8_t readReg(uint8_t reg)
static uint8_t readReg(busDevice_t * busDev, uint8_t reg)
{
uint8_t byte;
isResponding = i2cRead(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, 1, &byte);
isResponding = busRead(busDev, reg, &byte);
return byte;
}
static uint16_t readReg16(uint8_t reg)
static uint16_t readReg16(busDevice_t * busDev, uint8_t reg)
{
uint8_t bytes[2];
isResponding = i2cRead(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, 2, &bytes[0]);
isResponding = busReadBuf(busDev, reg, bytes, 2);
return ((uint16_t)bytes[0] << 8) |
((uint16_t)bytes[1] << 0);
}
static void readMulti(uint8_t reg, uint8_t * dst, uint8_t count)
static void readMulti(busDevice_t * busDev, uint8_t reg, uint8_t * dst, uint8_t count)
{
isResponding = i2cRead(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, count, dst);
isResponding = busReadBuf(busDev, reg, dst, count);
}
static void writeReg(uint8_t reg, uint8_t value)
static void writeReg(busDevice_t * busDev, uint8_t reg, uint8_t value)
{
isResponding = i2cWrite(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, value);
isResponding = busWrite(busDev, reg, value);
}
static void writeReg16(uint8_t reg, uint16_t value)
static void writeReg16(busDevice_t * busDev, uint8_t reg, uint16_t value)
{
uint8_t bytes[2];
bytes[0] = (value >> 8) & 0xFF;
bytes[1] = (value >> 0) & 0xFF;
isResponding = i2cWriteBuffer(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, 2, &bytes[0]);
isResponding = busWriteBuf(busDev, reg, bytes, 2);
}
static void writeReg32(uint8_t reg, uint16_t value)
/*
static void writeReg32(busDevice_t * busDev, uint8_t reg, uint16_t value)
{
uint8_t bytes[4];
bytes[0] = (value >> 24) & 0xFF;
bytes[1] = (value >> 16) & 0xFF;
bytes[2] = (value >> 8) & 0xFF;
bytes[3] = (value >> 0) & 0xFF;
isResponding = i2cWriteBuffer(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, 4, &bytes[0]);
isResponding = busWriteBuf(busDev, reg, bytes, 4);
}
*/
static void writeMulti(uint8_t reg, uint8_t const * src, uint8_t count)
static void writeMulti(busDevice_t * busDev, uint8_t reg, uint8_t const * src, uint8_t count)
{
isResponding = i2cWriteBuffer(RANGEFINDER_VL53L0X_INSTANCE, VL53L0X_I2C_ADDRESS, reg, count, (uint8_t *)src);
isResponding = busWriteBuf(busDev, reg, src, count);
}
@ -367,47 +365,47 @@ uint16_t encodeTimeout(uint16_t timeout_mclks)
// seems to increase the likelihood of getting an inaccurate reading because of
// unwanted reflections from objects other than the intended target.
// Defaults to 0.25 MCPS as initialized by the ST API and this library.
bool setSignalRateLimit(float limit_Mcps)
bool setSignalRateLimit(busDevice_t * busDev, float limit_Mcps)
{
if (limit_Mcps < 0 || limit_Mcps > 511.99) {
return false;
}
// Q9.7 fixed point format (9 integer bits, 7 fractional bits)
writeReg16(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7));
writeReg16(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7));
return true;
}
// Get the return signal rate limit check value in MCPS
float getSignalRateLimit(void)
float getSignalRateLimit(busDevice_t * busDev)
{
return (float)readReg16(VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
return (float)readReg16(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
}
// based on VL53L0X_perform_single_ref_calibration()
bool performSingleRefCalibration(uint8_t vhv_init_byte)
bool performSingleRefCalibration(busDevice_t * busDev, uint8_t vhv_init_byte)
{
writeReg(VL53L0X_REG_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
writeReg(busDev, VL53L0X_REG_SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
startTimeout();
while ((readReg(VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
while ((readReg(busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
if (checkTimeoutExpired()) {
return false;
}
}
writeReg(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
writeReg(busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
writeReg(VL53L0X_REG_SYSRANGE_START, 0x00);
writeReg(busDev, VL53L0X_REG_SYSRANGE_START, 0x00);
return true;
}
// Get sequence step enables
// based on VL53L0X_GetSequenceStepEnables()
void getSequenceStepEnables(sequenceStepEnables_t * enables)
void getSequenceStepEnables(busDevice_t * busDev, sequenceStepEnables_t * enables)
{
uint8_t sequence_config = readReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG);
uint8_t sequence_config = readReg(busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG);
enables->tcc = (sequence_config >> 4) & 0x1;
enables->dss = (sequence_config >> 3) & 0x1;
@ -418,13 +416,13 @@ void getSequenceStepEnables(sequenceStepEnables_t * enables)
// Get the VCSEL pulse period in PCLKs for the given period type.
// based on VL53L0X_get_vcsel_pulse_period()
uint8_t getVcselPulsePeriod(vcselPeriodType_e type)
uint8_t getVcselPulsePeriod(busDevice_t * busDev, vcselPeriodType_e type)
{
if (type == VcselPeriodPreRange) {
return decodeVcselPeriod(readReg(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD));
return decodeVcselPeriod(readReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD));
}
else if (type == VcselPeriodFinalRange) {
return decodeVcselPeriod(readReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD));
return decodeVcselPeriod(readReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD));
}
else {
return 255;
@ -435,19 +433,19 @@ uint8_t getVcselPulsePeriod(vcselPeriodType_e type)
// based on get_sequence_step_timeout(),
// but gets all timeouts instead of just the requested one, and also stores
// intermediate values
void getSequenceStepTimeouts(sequenceStepEnables_t const * enables, sequenceStepTimeouts_t * timeouts)
void getSequenceStepTimeouts(busDevice_t * busDev, sequenceStepEnables_t const * enables, sequenceStepTimeouts_t * timeouts)
{
timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange);
timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(busDev, VcselPeriodPreRange);
timeouts->msrc_dss_tcc_mclks = readReg(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP) + 1;
timeouts->msrc_dss_tcc_mclks = readReg(busDev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP) + 1;
timeouts->msrc_dss_tcc_us = timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks, timeouts->pre_range_vcsel_period_pclks);
timeouts->pre_range_mclks = decodeTimeout(readReg16(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
timeouts->pre_range_mclks = decodeTimeout(readReg16(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI));
timeouts->pre_range_us = timeoutMclksToMicroseconds(timeouts->pre_range_mclks, timeouts->pre_range_vcsel_period_pclks);
timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange);
timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(busDev, VcselPeriodFinalRange);
timeouts->final_range_mclks = decodeTimeout(readReg16(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
timeouts->final_range_mclks = decodeTimeout(readReg16(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI));
if (enables->pre_range) {
timeouts->final_range_mclks -= timeouts->pre_range_mclks;
@ -463,7 +461,7 @@ void getSequenceStepTimeouts(sequenceStepEnables_t const * enables, sequenceStep
// factor of N decreases the range measurement standard deviation by a factor of
// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms.
// based on VL53L0X_set_measurement_timing_budget_micro_seconds()
bool setMeasurementTimingBudget(uint32_t budget_us)
bool setMeasurementTimingBudget(busDevice_t * busDev, uint32_t budget_us)
{
sequenceStepEnables_t enables;
sequenceStepTimeouts_t timeouts;
@ -482,8 +480,8 @@ bool setMeasurementTimingBudget(uint32_t budget_us)
uint32_t used_budget_us = StartOverhead + EndOverhead;
getSequenceStepEnables(&enables);
getSequenceStepTimeouts(&enables, &timeouts);
getSequenceStepEnables(busDev, &enables);
getSequenceStepTimeouts(busDev, &enables, &timeouts);
if (enables.tcc) {
used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
@ -531,7 +529,7 @@ bool setMeasurementTimingBudget(uint32_t budget_us)
final_range_timeout_mclks += timeouts.pre_range_mclks;
}
writeReg16(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(final_range_timeout_mclks));
writeReg16(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(final_range_timeout_mclks));
// set_sequence_step_timeout() end
@ -544,7 +542,7 @@ bool setMeasurementTimingBudget(uint32_t budget_us)
// Get the measurement timing budget in microseconds
// based on VL53L0X_get_measurement_timing_budget_micro_seconds()
// in us
uint32_t getMeasurementTimingBudget(void)
uint32_t getMeasurementTimingBudget(busDevice_t * busDev)
{
sequenceStepEnables_t enables;
sequenceStepTimeouts_t timeouts;
@ -560,8 +558,8 @@ uint32_t getMeasurementTimingBudget(void)
// "Start and end overhead times always present"
uint32_t budget_us = StartOverhead + EndOverhead;
getSequenceStepEnables(&enables);
getSequenceStepTimeouts(&enables, &timeouts);
getSequenceStepEnables(busDev, &enables);
getSequenceStepTimeouts(busDev, &enables, &timeouts);
if (enables.tcc) {
budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead);
@ -593,15 +591,15 @@ uint32_t getMeasurementTimingBudget(void)
// pre: 12 to 18 (initialized default: 14)
// final: 8 to 14 (initialized default: 10)
// based on VL53L0X_set_vcsel_pulse_period()
bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
bool setVcselPulsePeriod(busDevice_t * busDev, vcselPeriodType_e type, uint8_t period_pclks)
{
uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks);
sequenceStepEnables_t enables;
sequenceStepTimeouts_t timeouts;
getSequenceStepEnables(&enables);
getSequenceStepTimeouts(&enables, &timeouts);
getSequenceStepEnables(busDev, &enables);
getSequenceStepTimeouts(busDev, &enables, &timeouts);
// "Apply specific settings for the requested clock period"
// "Re-calculate and apply timeouts, in macro periods"
@ -620,29 +618,30 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
// "Set phase check limits"
switch (period_pclks) {
case 12:
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18);
break;
case 14:
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30);
break;
case 16:
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40);
break;
case 18:
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50);
break;
default:
// invalid period
return false;
}
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
// apply new VCSEL period
writeReg(VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
writeReg(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
// update timeouts
@ -651,7 +650,7 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
uint16_t new_pre_range_timeout_mclks = timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks);
writeReg16(VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(new_pre_range_timeout_mclks));
writeReg16(busDev, VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(new_pre_range_timeout_mclks));
// set_sequence_step_timeout() end
@ -660,50 +659,50 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
uint16_t new_msrc_timeout_mclks = timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks);
writeReg(VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
writeReg(busDev, VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP, (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1));
// set_sequence_step_timeout() end
}
else if (type == VcselPeriodFinalRange) {
switch (period_pclks) {
case 8:
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
writeReg(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
writeReg(0xFF, 0x01);
writeReg(VL53L0X_REG_ALGO_PHASECAL_LIM, 0x30);
writeReg(0xFF, 0x00);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(busDev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x30);
writeReg(busDev, 0xFF, 0x00);
break;
case 10:
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
writeReg(0xFF, 0x01);
writeReg(VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(0xFF, 0x00);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(busDev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(busDev, 0xFF, 0x00);
break;
case 12:
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
writeReg(0xFF, 0x01);
writeReg(VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(0xFF, 0x00);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(busDev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(busDev, 0xFF, 0x00);
break;
case 14:
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
writeReg(0xFF, 0x01);
writeReg(VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(0xFF, 0x00);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08);
writeReg(busDev, VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, VL53L0X_REG_ALGO_PHASECAL_LIM, 0x20);
writeReg(busDev, 0xFF, 0x00);
break;
default:
@ -712,7 +711,7 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
}
// apply new VCSEL period
writeReg(VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
writeReg(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg);
// update timeouts
@ -730,7 +729,7 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
new_final_range_timeout_mclks += timeouts.pre_range_mclks;
}
writeReg16(VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(new_final_range_timeout_mclks));
writeReg16(busDev, VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, encodeTimeout(new_final_range_timeout_mclks));
// set_sequence_step_timeout end
}
else {
@ -739,15 +738,15 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
}
// "Finally, the timing budget must be re-applied"
setMeasurementTimingBudget(measurementTimingBudgetUs);
setMeasurementTimingBudget(busDev, measurementTimingBudgetUs);
// "Perform the phase calibration. This is needed after changing on vcsel period."
// VL53L0X_perform_phase_calibration() begin
uint8_t sequence_config = readReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG);
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
performSingleRefCalibration(0x0);
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config);
uint8_t sequence_config = readReg(busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG);
writeReg(busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
performSingleRefCalibration(busDev, 0x0);
writeReg(busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config);
// VL53L0X_perform_phase_calibration() end
@ -757,87 +756,85 @@ bool setVcselPulsePeriod(vcselPeriodType_e type, uint8_t period_pclks)
// Get reference SPAD (single photon avalanche diode) count and type
// based on VL53L0X_get_info_from_device(),
// but only gets reference SPAD count and type
bool getSpadInfo(uint8_t * count, bool * type_is_aperture)
bool getSpadInfo(busDevice_t * busDev, uint8_t * count, bool * type_is_aperture)
{
uint8_t tmp;
writeReg(0x80, 0x01);
writeReg(0xFF, 0x01);
writeReg(0x00, 0x00);
writeReg(busDev, 0x80, 0x01);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, 0x00, 0x00);
writeReg(0xFF, 0x06);
writeReg(0x83, readReg(0x83) | 0x04);
writeReg(0xFF, 0x07);
writeReg(0x81, 0x01);
writeReg(busDev, 0xFF, 0x06);
writeReg(busDev, 0x83, readReg(busDev, 0x83) | 0x04);
writeReg(busDev, 0xFF, 0x07);
writeReg(busDev, 0x81, 0x01);
writeReg(0x80, 0x01);
writeReg(busDev, 0x80, 0x01);
writeReg(0x94, 0x6b);
writeReg(0x83, 0x00);
writeReg(busDev, 0x94, 0x6b);
writeReg(busDev, 0x83, 0x00);
startTimeout();
while (readReg(0x83) == 0x00)
while (readReg(busDev, 0x83) == 0x00)
{
if (checkTimeoutExpired()) {
return false;
}
}
writeReg(0x83, 0x01);
tmp = readReg(0x92);
writeReg(busDev, 0x83, 0x01);
tmp = readReg(busDev, 0x92);
*count = tmp & 0x7f;
*type_is_aperture = (tmp >> 7) & 0x01;
writeReg(0x81, 0x00);
writeReg(0xFF, 0x06);
writeReg(0x83, readReg(0x83) & ~0x04);
writeReg(0xFF, 0x01);
writeReg(0x00, 0x01);
writeReg(busDev, 0x81, 0x00);
writeReg(busDev, 0xFF, 0x06);
writeReg(busDev, 0x83, readReg(busDev, 0x83) & ~0x04);
writeReg(busDev, 0xFF, 0x01);
writeReg(busDev, 0x00, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x00);
writeReg(busDev, 0xFF, 0x00);
writeReg(busDev, 0x80, 0x00);
return true;
}
static void vl53l0x_Init(rangefinderDev_t *dev)
static void vl53l0x_Init(rangefinderDev_t * rangefinder)
{
UNUSED(dev);
uint8_t byte;
isInitialized = false;
// VL53L0X_DataInit() begin
// Switch sensor to 2.8V mode
byte = readReg(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV);
writeReg(VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, byte | 0x01);
byte = readReg(rangefinder->busDev, VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV);
writeReg(rangefinder->busDev, VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, byte | 0x01);
// Set I2C standard mode
writeReg(0x88, 0x00);
writeReg(rangefinder->busDev, 0x88, 0x00);
writeReg(0x80, 0x01);
writeReg(0xFF, 0x01);
writeReg(0x00, 0x00);
stopVariable = readReg(0x91);
writeReg(0x00, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x00, 0x00);
stopVariable = readReg(rangefinder->busDev, 0x91);
writeReg(rangefinder->busDev, 0x00, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x00);
// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
byte = readReg(VL53L0X_REG_MSRC_CONFIG_CONTROL);
writeReg(VL53L0X_REG_MSRC_CONFIG_CONTROL, byte | 0x12);
byte = readReg(rangefinder->busDev, VL53L0X_REG_MSRC_CONFIG_CONTROL);
writeReg(rangefinder->busDev, VL53L0X_REG_MSRC_CONFIG_CONTROL, byte | 0x12);
// set final range signal rate limit to 0.25 MCPS (million counts per second)
setSignalRateLimit(0.25);
setSignalRateLimit(rangefinder->busDev, 0.25);
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xFF);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xFF);
// VL53L0X_StaticInit() begin
uint8_t spad_count;
bool spad_type_is_aperture;
if (!getSpadInfo(&spad_count, &spad_type_is_aperture)) {
if (!getSpadInfo(rangefinder->busDev, &spad_count, &spad_type_is_aperture)) {
// Init failed
return;
}
@ -846,14 +843,14 @@ static void vl53l0x_Init(rangefinderDev_t *dev)
// the API, but the same data seems to be more easily readable from
// GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
uint8_t ref_spad_map[6];
readMulti(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
readMulti(rangefinder->busDev, VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
// VL53L0X_set_reference_spads() begin (assume NVM values are valid)
writeReg(0xFF, 0x01);
writeReg(VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
writeReg(VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
writeReg(0xFF, 0x00);
writeReg(VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
writeReg(rangefinder->busDev, VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4);
uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad
uint8_t spads_enabled = 0;
@ -872,144 +869,144 @@ static void vl53l0x_Init(rangefinderDev_t *dev)
}
}
writeMulti(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
writeMulti(rangefinder->busDev, VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6);
// -- VL53L0X_load_tuning_settings() begin
// DefaultTuningSettings from vl53l0x_tuning.h
writeReg(0xFF, 0x01);
writeReg(0x00, 0x00);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x00, 0x00);
writeReg(0xFF, 0x00);
writeReg(0x09, 0x00);
writeReg(0x10, 0x00);
writeReg(0x11, 0x00);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x09, 0x00);
writeReg(rangefinder->busDev, 0x10, 0x00);
writeReg(rangefinder->busDev, 0x11, 0x00);
writeReg(0x24, 0x01);
writeReg(0x25, 0xFF);
writeReg(0x75, 0x00);
writeReg(rangefinder->busDev, 0x24, 0x01);
writeReg(rangefinder->busDev, 0x25, 0xFF);
writeReg(rangefinder->busDev, 0x75, 0x00);
writeReg(0xFF, 0x01);
writeReg(0x4E, 0x2C);
writeReg(0x48, 0x00);
writeReg(0x30, 0x20);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x4E, 0x2C);
writeReg(rangefinder->busDev, 0x48, 0x00);
writeReg(rangefinder->busDev, 0x30, 0x20);
writeReg(0xFF, 0x00);
writeReg(0x30, 0x09);
writeReg(0x54, 0x00);
writeReg(0x31, 0x04);
writeReg(0x32, 0x03);
writeReg(0x40, 0x83);
writeReg(0x46, 0x25);
writeReg(0x60, 0x00);
writeReg(0x27, 0x00);
writeReg(0x50, 0x06);
writeReg(0x51, 0x00);
writeReg(0x52, 0x96);
writeReg(0x56, 0x08);
writeReg(0x57, 0x30);
writeReg(0x61, 0x00);
writeReg(0x62, 0x00);
writeReg(0x64, 0x00);
writeReg(0x65, 0x00);
writeReg(0x66, 0xA0);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x30, 0x09);
writeReg(rangefinder->busDev, 0x54, 0x00);
writeReg(rangefinder->busDev, 0x31, 0x04);
writeReg(rangefinder->busDev, 0x32, 0x03);
writeReg(rangefinder->busDev, 0x40, 0x83);
writeReg(rangefinder->busDev, 0x46, 0x25);
writeReg(rangefinder->busDev, 0x60, 0x00);
writeReg(rangefinder->busDev, 0x27, 0x00);
writeReg(rangefinder->busDev, 0x50, 0x06);
writeReg(rangefinder->busDev, 0x51, 0x00);
writeReg(rangefinder->busDev, 0x52, 0x96);
writeReg(rangefinder->busDev, 0x56, 0x08);
writeReg(rangefinder->busDev, 0x57, 0x30);
writeReg(rangefinder->busDev, 0x61, 0x00);
writeReg(rangefinder->busDev, 0x62, 0x00);
writeReg(rangefinder->busDev, 0x64, 0x00);
writeReg(rangefinder->busDev, 0x65, 0x00);
writeReg(rangefinder->busDev, 0x66, 0xA0);
writeReg(0xFF, 0x01);
writeReg(0x22, 0x32);
writeReg(0x47, 0x14);
writeReg(0x49, 0xFF);
writeReg(0x4A, 0x00);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x22, 0x32);
writeReg(rangefinder->busDev, 0x47, 0x14);
writeReg(rangefinder->busDev, 0x49, 0xFF);
writeReg(rangefinder->busDev, 0x4A, 0x00);
writeReg(0xFF, 0x00);
writeReg(0x7A, 0x0A);
writeReg(0x7B, 0x00);
writeReg(0x78, 0x21);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x7A, 0x0A);
writeReg(rangefinder->busDev, 0x7B, 0x00);
writeReg(rangefinder->busDev, 0x78, 0x21);
writeReg(0xFF, 0x01);
writeReg(0x23, 0x34);
writeReg(0x42, 0x00);
writeReg(0x44, 0xFF);
writeReg(0x45, 0x26);
writeReg(0x46, 0x05);
writeReg(0x40, 0x40);
writeReg(0x0E, 0x06);
writeReg(0x20, 0x1A);
writeReg(0x43, 0x40);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x23, 0x34);
writeReg(rangefinder->busDev, 0x42, 0x00);
writeReg(rangefinder->busDev, 0x44, 0xFF);
writeReg(rangefinder->busDev, 0x45, 0x26);
writeReg(rangefinder->busDev, 0x46, 0x05);
writeReg(rangefinder->busDev, 0x40, 0x40);
writeReg(rangefinder->busDev, 0x0E, 0x06);
writeReg(rangefinder->busDev, 0x20, 0x1A);
writeReg(rangefinder->busDev, 0x43, 0x40);
writeReg(0xFF, 0x00);
writeReg(0x34, 0x03);
writeReg(0x35, 0x44);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x34, 0x03);
writeReg(rangefinder->busDev, 0x35, 0x44);
writeReg(0xFF, 0x01);
writeReg(0x31, 0x04);
writeReg(0x4B, 0x09);
writeReg(0x4C, 0x05);
writeReg(0x4D, 0x04);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x31, 0x04);
writeReg(rangefinder->busDev, 0x4B, 0x09);
writeReg(rangefinder->busDev, 0x4C, 0x05);
writeReg(rangefinder->busDev, 0x4D, 0x04);
writeReg(0xFF, 0x00);
writeReg(0x44, 0x00);
writeReg(0x45, 0x20);
writeReg(0x47, 0x08);
writeReg(0x48, 0x28);
writeReg(0x67, 0x00);
writeReg(0x70, 0x04);
writeReg(0x71, 0x01);
writeReg(0x72, 0xFE);
writeReg(0x76, 0x00);
writeReg(0x77, 0x00);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x44, 0x00);
writeReg(rangefinder->busDev, 0x45, 0x20);
writeReg(rangefinder->busDev, 0x47, 0x08);
writeReg(rangefinder->busDev, 0x48, 0x28);
writeReg(rangefinder->busDev, 0x67, 0x00);
writeReg(rangefinder->busDev, 0x70, 0x04);
writeReg(rangefinder->busDev, 0x71, 0x01);
writeReg(rangefinder->busDev, 0x72, 0xFE);
writeReg(rangefinder->busDev, 0x76, 0x00);
writeReg(rangefinder->busDev, 0x77, 0x00);
writeReg(0xFF, 0x01);
writeReg(0x0D, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x0D, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x01);
writeReg(0x01, 0xF8);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x01);
writeReg(rangefinder->busDev, 0x01, 0xF8);
writeReg(0xFF, 0x01);
writeReg(0x8E, 0x01);
writeReg(0x00, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x00);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x8E, 0x01);
writeReg(rangefinder->busDev, 0x00, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x00);
// -- VL53L0X_SetGpioConfig() begin
writeReg(VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
writeReg(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, readReg(VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low
writeReg(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04);
writeReg(rangefinder->busDev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, readReg(rangefinder->busDev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
measurementTimingBudgetUs = getMeasurementTimingBudget();
measurementTimingBudgetUs = getMeasurementTimingBudget(rangefinder->busDev);
// "Disable MSRC and TCC by default"
// MSRC = Minimum Signal Rate Check
// TCC = Target CentreCheck
// -- VL53L0X_SetSequenceStepEnable() begin
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xE8);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xE8);
// "Recalculate timing budget"
setMeasurementTimingBudget(measurementTimingBudgetUs);
setMeasurementTimingBudget(rangefinder->busDev, measurementTimingBudgetUs);
// -- VL53L0X_perform_vhv_calibration() begin
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
if (!performSingleRefCalibration(0x40)) {
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
if (!performSingleRefCalibration(rangefinder->busDev, 0x40)) {
return;
}
// -- VL53L0X_perform_phase_calibration() begin
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
if (!performSingleRefCalibration(0x00)) {
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
if (!performSingleRefCalibration(rangefinder->busDev, 0x00)) {
return;
}
// "restore the previous Sequence Config"
writeReg(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xE8);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xE8);
// Apply range parameters
#if 1 // LONG RANGE
setSignalRateLimit(0.25f);
setVcselPulsePeriod(VcselPeriodPreRange, 18);
setVcselPulsePeriod(VcselPeriodFinalRange, 14);
setSignalRateLimit(rangefinder->busDev, 0.25f);
setVcselPulsePeriod(rangefinder->busDev, VcselPeriodPreRange, 18);
setVcselPulsePeriod(rangefinder->busDev, VcselPeriodFinalRange, 14);
#endif
// Set timeout
@ -1019,10 +1016,8 @@ static void vl53l0x_Init(rangefinderDev_t *dev)
isInitialized = true;
}
void vl53l0x_Update(rangefinderDev_t *dev)
void vl53l0x_Update(rangefinderDev_t * rangefinder)
{
UNUSED(dev);
if (!isInitialized) {
return;
}
@ -1031,14 +1026,14 @@ void vl53l0x_Update(rangefinderDev_t *dev)
switch (measSteps) {
case MEASUREMENT_START:
// Initiate new measurement
writeReg(0x80, 0x01);
writeReg(0xFF, 0x01);
writeReg(0x00, 0x00);
writeReg(0x91, stopVariable);
writeReg(0x00, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x00);
writeReg(VL53L0X_REG_SYSRANGE_START, 0x01);
writeReg(rangefinder->busDev, 0x80, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x00, 0x00);
writeReg(rangefinder->busDev, 0x91, stopVariable);
writeReg(rangefinder->busDev, 0x00, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x00);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START, 0x01);
startTimeout();
measSteps = MEASUREMENT_WAIT;
@ -1046,7 +1041,7 @@ void vl53l0x_Update(rangefinderDev_t *dev)
case MEASUREMENT_WAIT:
// Wait for data and read the measurement
if (readReg(VL53L0X_REG_SYSRANGE_START) & 0x01) {
if (readReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START) & 0x01) {
if (checkTimeoutExpired()) {
lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
measSteps = MEASUREMENT_START;
@ -1059,7 +1054,7 @@ void vl53l0x_Update(rangefinderDev_t *dev)
break;
case MEASUREMENT_READ:
if ((readReg(VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
if ((readReg(rangefinder->busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
if (checkTimeoutExpired()) {
lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
measSteps = MEASUREMENT_START;
@ -1067,8 +1062,8 @@ void vl53l0x_Update(rangefinderDev_t *dev)
}
else {
// assumptions: Linearity Corrective Gain is 1000 (default);
uint16_t raw = readReg16(VL53L0X_REG_RESULT_RANGE_STATUS + 10);
writeReg(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
uint16_t raw = readReg16(rangefinder->busDev, VL53L0X_REG_RESULT_RANGE_STATUS + 10);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
lastMeasurementCm = raw / 10;
lastMeasurementIsNew = true;
@ -1080,24 +1075,24 @@ void vl53l0x_Update(rangefinderDev_t *dev)
switch (measSteps) {
case MEASUREMENT_START:
// Initiate new measurement
writeReg(0x80, 0x01);
writeReg(0xFF, 0x01);
writeReg(0x00, 0x00);
writeReg(0x91, stopVariable);
writeReg(0x00, 0x01);
writeReg(0xFF, 0x00);
writeReg(0x80, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x01);
writeReg(rangefinder->busDev, 0x00, 0x00);
writeReg(rangefinder->busDev, 0x91, stopVariable);
writeReg(rangefinder->busDev, 0x00, 0x01);
writeReg(rangefinder->busDev, 0xFF, 0x00);
writeReg(rangefinder->busDev, 0x80, 0x00);
uint16_t osc_calibrate_val = readReg16(VL53L0X_REG_OSC_CALIBRATE_VAL);
if (osc_calibrate_val != 0) {
writeReg32(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50 * osc_calibrate_val);
writeReg32(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50 * osc_calibrate_val);
}
else {
writeReg32(VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50);
writeReg32(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD, 50);
}
writeReg(VL53L0X_REG_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
writeReg(rangefinder->busDev, VL53L0X_REG_SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK
startTimeout();
measSteps = MEASUREMENT_READ;
@ -1105,7 +1100,7 @@ void vl53l0x_Update(rangefinderDev_t *dev)
case MEASUREMENT_READ:
default:
if ((readReg(VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
if ((readReg(rangefinder->busDev, VL53L0X_REG_RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
if (checkTimeoutExpired()) {
lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
@ -1116,8 +1111,8 @@ void vl53l0x_Update(rangefinderDev_t *dev)
}
else {
// assumptions: Linearity Corrective Gain is 1000 (default);
uint16_t raw = readReg16(VL53L0X_REG_RESULT_RANGE_STATUS + 10);
writeReg(VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
uint16_t raw = readReg16(rangefinder->busDev, VL53L0X_REG_RESULT_RANGE_STATUS + 10);
writeReg(rangefinder->busDev, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
lastMeasurementCm = raw / 10;
@ -1148,24 +1143,45 @@ int32_t vl53l0x_GetDistance(rangefinderDev_t *dev)
}
}
bool vl53l0xDetect(rangefinderDev_t *dev)
static bool deviceDetect(busDevice_t * busDev)
{
uint8_t model_id = readReg(VL53L0X_REG_IDENTIFICATION_MODEL_ID);
for (int retry = 0; retry < 5; retry++) {
uint8_t model_id;
if (isResponding && model_id == 0xEE) {
dev->delayMs = RANGEFINDER_VL53L0X_TASK_PERIOD_MS;
dev->maxRangeCm = VL53L0X_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = VL53L0X_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = VL53L0X_DETECTION_CONE_DECIDEGREES;
delay(150);
dev->init = &vl53l0x_Init;
dev->update = &vl53l0x_Update;
dev->read = &vl53l0x_GetDistance;
bool ack = busRead(busDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &model_id);
if (ack && model_id == 0xEE) {
return true;
}
};
return false;
}
bool vl53l0xDetect(rangefinderDev_t * rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_VL53L0X, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_VL53L0X_TASK_PERIOD_MS;
rangefinder->maxRangeCm = VL53L0X_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = VL53L0X_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = VL53L0X_DETECTION_CONE_DECIDEGREES;
rangefinder->init = &vl53l0x_Init;
rangefinder->update = &vl53l0x_Update;
rangefinder->read = &vl53l0x_GetDistance;
return true;
}
else {
return false;
}
}
#endif

0
src/main/drivers/rangefinder_vl53l0x.h → src/main/drivers/rangefinder/rangefinder_vl53l0x.h
View file

1
src/main/drivers/resource.h
View file

@ -50,6 +50,7 @@ typedef enum {
OWNER_RX_SPI,
OWNER_VTX,
OWNER_SPI_PREINIT,
OWNER_COMPASS,
OWNER_TOTAL_COUNT
} resourceOwner_e;

370
src/main/drivers/serial_softserial.c
View file

@ -15,6 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Cleanflight (or Baseflight): original
* jflyper: Mono-timer and single-wire half-duplex
* jflyper: Ported to iNav
*/
#include <stdbool.h>
#include <stdint.h>
@ -25,16 +31,19 @@
#include "build/build_config.h"
#include "build/atomic.h"
#include "build/debug.h"
#include "common/utils.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "timer.h"
#include "serial.h"
#include "serial_softserial.h"
#include "fc/config.h" //!!TODO remove this dependency
#define RX_TOTAL_BITS 10
#define TX_TOTAL_BITS 10
@ -44,21 +53,34 @@
#define MAX_SOFTSERIAL_PORTS 1
#endif
typedef enum {
TIMER_MODE_SINGLE,
TIMER_MODE_DUAL,
} timerMode_e;
#define ICPOLARITY_RISING true
#define ICPOLARITY_FALLING false
typedef struct softSerial_s {
serialPort_t port;
IO_t rxIO;
IO_t txIO;
const timerHardware_t *rxTimerHardware;
volatile uint8_t rxBuffer[SOFTSERIAL_BUFFER_SIZE];
const timerHardware_t *txTimerHardware;
const timerHardware_t *timerHardware;
#ifdef USE_HAL_DRIVER
const TIM_HandleTypeDef *timerHandle;
#endif
const timerHardware_t *exTimerHardware;
volatile uint8_t rxBuffer[SOFTSERIAL_BUFFER_SIZE];
volatile uint8_t txBuffer[SOFTSERIAL_BUFFER_SIZE];
uint8_t isSearchingForStartBit;
uint8_t rxBitIndex;
uint8_t rxLastLeadingEdgeAtBitIndex;
uint8_t rxEdge;
uint8_t rxActive;
uint8_t isTransmittingData;
int8_t bitsLeftToTransmit;
@ -70,22 +92,20 @@ typedef struct softSerial_s {
uint16_t receiveErrors;
uint8_t softSerialPortIndex;
timerMode_e timerMode;
timerCCHandlerRec_t timerCb;
timerOvrHandlerRec_t overCb;
timerCCHandlerRec_t edgeCb;
} softSerial_t;
extern timerHardware_t* serialTimerHardware;
static const struct serialPortVTable softSerialVTable; // Forward
extern const struct serialPortVTable softSerialVTable[];
static softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void onSerialTimerOverflow(timerOvrHandlerRec_t *cbRec, captureCompare_t capture);
void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture);
void setTxSignal(softSerial_t *softSerial, uint8_t state)
static void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if (softSerial->port.options & SERIAL_INVERTED) {
state = !state;
@ -98,20 +118,80 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state)
}
}
void serialInputPortConfig(ioTag_t pin, uint8_t portIndex)
static void serialEnableCC(softSerial_t *softSerial)
{
IOInit(IOGetByTag(pin), OWNER_SOFTSERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(portIndex));
#ifdef STM32F1
IOConfigGPIO(IOGetByTag(pin), IOCFG_IPU);
#ifdef USE_HAL_DRIVER
TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_ENABLE);
#else
IOConfigGPIO(IOGetByTag(pin), IOCFG_AF_PP_UP);
TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Enable);
#endif
}
static void serialOutputPortConfig(ioTag_t pin, uint8_t portIndex)
static void serialInputPortActivate(softSerial_t *softSerial)
{
IOInit(IOGetByTag(pin), OWNER_SOFTSERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(portIndex));
IOConfigGPIO(IOGetByTag(pin), IOCFG_OUT_PP);
if (softSerial->port.options & SERIAL_INVERTED) {
#ifdef STM32F1
IOConfigGPIO(softSerial->rxIO, IOCFG_IPD);
#else
IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction);
#endif
} else {
#ifdef STM32F1
IOConfigGPIO(softSerial->rxIO, IOCFG_IPU);
#else
IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction);
#endif
}
softSerial->rxActive = true;
softSerial->isSearchingForStartBit = true;
softSerial->rxBitIndex = 0;
// Enable input capture
serialEnableCC(softSerial);
}
static void serialInputPortDeActivate(softSerial_t *softSerial)
{
// Disable input capture
#ifdef USE_HAL_DRIVER
TIM_CCxChannelCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_DISABLE);
#else
TIM_CCxCmd(softSerial->timerHardware->tim, softSerial->timerHardware->channel, TIM_CCx_Disable);
#endif
#ifdef STM32F1
IOConfigGPIO(softSerial->rxIO, IOCFG_IN_FLOATING);
#else
IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->timerHardware->alternateFunction);
#endif
softSerial->rxActive = false;
}
static void serialOutputPortActivate(softSerial_t *softSerial)
{
#ifdef STM32F1
IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
#else
if (softSerial->exTimerHardware)
IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTimerHardware->alternateFunction);
else
IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
#endif
}
static void serialOutputPortDeActivate(softSerial_t *softSerial)
{
#ifdef STM32F1
IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
#else
if (softSerial->exTimerHardware)
IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTimerHardware->alternateFunction);
else
IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
#endif
}
static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
@ -119,10 +199,12 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
return timerPeriod > 0xFFFF;
}
static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud)
{
uint32_t clock = SystemCoreClock;
uint32_t baseClock = SystemCoreClock/timerClockDivisor(timerHardwarePtr->tim);
uint32_t clock = baseClock;
uint32_t timerPeriod;
do {
timerPeriod = clock / baud;
if (isTimerPeriodTooLarge(timerPeriod)) {
@ -135,18 +217,9 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = SystemCoreClock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerChCCHandlerInit(&softSerialPorts[reference].timerCb, onSerialTimer);
timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].timerCb, NULL);
}
uint16_t mhz = baseClock / 1000000; // XXX Prepare for mhz > 255
static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
{
// start bit is usually a FALLING signal
timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED), 0);
timerChCCHandlerInit(&softSerialPorts[reference].edgeCb, onSerialRxPinChange);
timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].edgeCb, NULL);
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
}
static void resetBuffers(softSerial_t *softSerial)
@ -162,80 +235,152 @@ static void resetBuffers(softSerial_t *softSerial)
softSerial->port.txBufferHead = 0;
}
serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portOptions_t options)
serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portMode_t mode, portOptions_t options)
{
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
ioTag_t tagRx;
ioTag_t tagTx;
#ifdef USE_SOFTSERIAL1
if (portIndex == SOFTSERIAL1) {
softSerial->rxTimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY);;
softSerial->txTimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_1_TX_PIN), TIM_USE_ANY);;
tagRx = IO_TAG(SOFTSERIAL_1_RX_PIN);
tagTx = IO_TAG(SOFTSERIAL_1_TX_PIN);
}
#endif
#ifdef USE_SOFTSERIAL2
if (portIndex == SOFTSERIAL2) {
softSerial->rxTimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY);;
softSerial->txTimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_2_TX_PIN), TIM_USE_ANY);;
tagRx = IO_TAG(SOFTSERIAL_2_RX_PIN);
tagTx = IO_TAG(SOFTSERIAL_2_TX_PIN);
}
#endif
softSerial->port.vTable = softSerialVTable;
const timerHardware_t *timerRx = timerGetByTag(tagRx, TIM_USE_ANY);
const timerHardware_t *timerTx = timerGetByTag(tagTx, TIM_USE_ANY);
IO_t rxIO = IOGetByTag(tagRx);
IO_t txIO = IOGetByTag(tagTx);
if (options & SERIAL_BIDIR) {
// If RX and TX pins are both assigned, we CAN use either with a timer.
// However, for consistency with hardware UARTs, we only use TX pin,
// and this pin must have a timer, and it should not be N-Channel.
if (!timerTx || (timerTx->output & TIMER_OUTPUT_N_CHANNEL)) {
return NULL;
}
softSerial->timerHardware = timerTx;
softSerial->txIO = txIO;
softSerial->rxIO = txIO;
IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(portIndex));
} else {
if (mode & MODE_RX) {
// Need a pin & a timer on RX. Channel should not be N-Channel.
if (!timerRx || (timerRx->output & TIMER_OUTPUT_N_CHANNEL)) {
return NULL;
}
softSerial->rxIO = rxIO;
softSerial->timerHardware = timerRx;
IOInit(rxIO, OWNER_SOFTSERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(portIndex));
}
if (mode & MODE_TX) {
// Need a pin on TX
if (!tagTx)
return NULL;
softSerial->txIO = txIO;
if (!(mode & MODE_RX)) {
// TX Simplex, must have a timer
if (!timerTx)
return NULL;
softSerial->timerHardware = timerTx;
} else {
// Duplex
softSerial->exTimerHardware = timerTx;
}
IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(portIndex));
}
}
softSerial->port.vTable = &softSerialVTable;
softSerial->port.baudRate = baud;
softSerial->port.mode = MODE_RXTX;
softSerial->port.mode = mode;
softSerial->port.options = options;
softSerial->port.rxCallback = rxCallback;
resetBuffers(softSerial);
softSerial->isTransmittingData = false;
softSerial->isSearchingForStartBit = true;
softSerial->rxBitIndex = 0;
softSerial->softSerialPortIndex = portIndex;
softSerial->transmissionErrors = 0;
softSerial->receiveErrors = 0;
softSerial->softSerialPortIndex = portIndex;
softSerial->rxActive = false;
softSerial->isTransmittingData = false;
softSerial->txIO = IOGetByTag(softSerial->txTimerHardware->tag);
serialOutputPortConfig(softSerial->txTimerHardware->tag, portIndex);
// Configure master timer (on RX); time base and input capture
softSerial->rxIO = IOGetByTag(softSerial->rxTimerHardware->tag);
serialInputPortConfig(softSerial->rxTimerHardware->tag, portIndex);
serialTimerConfigureTimebase(softSerial->timerHardware, baud);
timerChConfigIC(softSerial->timerHardware, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
// Initialize callbacks
timerChCCHandlerInit(&softSerial->edgeCb, onSerialRxPinChange);
timerChOvrHandlerInit(&softSerial->overCb, onSerialTimerOverflow);
// Configure bit clock interrupt & handler.
// If we have an extra timer (on TX), it is initialized and configured
// for overflow interrupt.
// Receiver input capture is configured when input is activated.
if ((mode & MODE_TX) && softSerial->exTimerHardware && softSerial->exTimerHardware->tim != softSerial->timerHardware->tim) {
softSerial->timerMode = TIMER_MODE_DUAL;
serialTimerConfigureTimebase(softSerial->exTimerHardware, baud);
timerChConfigCallbacks(softSerial->exTimerHardware, NULL, &softSerial->overCb);
timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, NULL);
} else {
softSerial->timerMode = TIMER_MODE_SINGLE;
timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, &softSerial->overCb);
}
#ifdef USE_HAL_DRIVER
softSerial->timerHandle = timerFindTimerHandle(softSerial->timerHardware->tim);
#endif
if (!(options & SERIAL_BIDIR)) {
serialOutputPortActivate(softSerial);
setTxSignal(softSerial, ENABLE);
delay(50);
}
serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud);
serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, options);
serialInputPortActivate(softSerial);
return &softSerial->port;
}
serialPort_t *softSerialLoopbackPort(void)
{
serialPort_t *loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
return loopbackPort;
}
/*********************************************/
/*
* Serial Engine
*/
void processTxState(softSerial_t *softSerial)
{
uint8_t mask;
if (!softSerial->isTransmittingData) {
char byteToSend;
if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
// Transmit buffer empty.
// Start listening if not already in if half-duplex
if (!softSerial->rxActive && softSerial->port.options & SERIAL_BIDIR) {
serialOutputPortDeActivate(softSerial);
serialInputPortActivate(softSerial);
}
return;
}
// data to send
byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
uint8_t byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
softSerial->port.txBufferTail = 0;
}
@ -245,6 +390,12 @@ void processTxState(softSerial_t *softSerial)
softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
softSerial->isTransmittingData = true;
if (softSerial->rxActive && (softSerial->port.options & SERIAL_BIDIR)) {
// Half-duplex: Deactivate receiver, activate transmitter
serialInputPortDeActivate(softSerial);
serialOutputPortActivate(softSerial);
}
return;
}
@ -282,7 +433,8 @@ void prepareForNextRxByte(softSerial_t *softSerial)
softSerial->isSearchingForStartBit = true;
if (softSerial->rxEdge == LEADING) {
softSerial->rxEdge = TRAILING;
timerChConfigIC(softSerial->rxTimerHardware, (softSerial->port.options & SERIAL_INVERTED), 0);
timerChConfigIC(softSerial->timerHardware, (softSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
serialEnableCC(softSerial);
}
}
@ -337,60 +489,85 @@ void processRxState(softSerial_t *softSerial)
}
}
void onSerialTimer(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
void onSerialTimerOverflow(timerOvrHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
softSerial_t *softSerial = container_of(cbRec, softSerial_t, timerCb);
processTxState(softSerial);
processRxState(softSerial);
softSerial_t *self = container_of(cbRec, softSerial_t, overCb);
if (self->port.mode & MODE_TX)
processTxState(self);
if (self->port.mode & MODE_RX)
processRxState(self);
}
void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
{
UNUSED(capture);
softSerial_t *softSerial = container_of(cbRec, softSerial_t, edgeCb);
bool inverted = softSerial->port.options & SERIAL_INVERTED;
softSerial_t *self = container_of(cbRec, softSerial_t, edgeCb);
bool inverted = self->port.options & SERIAL_INVERTED;
if ((softSerial->port.mode & MODE_RX) == 0) {
if ((self->port.mode & MODE_RX) == 0) {
return;
}
if (softSerial->isSearchingForStartBit) {
// synchronise bit counter
// FIXME this reduces functionality somewhat as receiving breaks concurrent transmission on all ports because
// the next callback to the onSerialTimer will happen too early causing transmission errors.
TIM_SetCounter(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->tim->ARR / 2);
if (softSerial->isTransmittingData) {
softSerial->transmissionErrors++;
if (self->isSearchingForStartBit) {
// Synchronize the bit timing so that it will interrupt at the center
// of the bit period.
#ifdef USE_HAL_DRIVER
__HAL_TIM_SetCounter(self->timerHandle, __HAL_TIM_GetAutoreload(self->timerHandle) / 2);
#else
TIM_SetCounter(self->timerHardware->tim, self->timerHardware->tim->ARR / 2);
#endif
// For a mono-timer full duplex configuration, this may clobber the
// transmission because the next callback to the onSerialTimerOverflow
// will happen too early causing transmission errors.
// For a dual-timer configuration, there is no problem.
if ((self->timerMode != TIMER_MODE_DUAL) && self->isTransmittingData) {
self->transmissionErrors++;
}
timerChConfigIC(softSerial->rxTimerHardware, !inverted, 0);
softSerial->rxEdge = LEADING;
timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
#ifdef STM32F7
serialEnableCC(self);
#endif
self->rxEdge = LEADING;
softSerial->rxBitIndex = 0;
softSerial->rxLastLeadingEdgeAtBitIndex = 0;
softSerial->internalRxBuffer = 0;
softSerial->isSearchingForStartBit = false;
self->rxBitIndex = 0;
self->rxLastLeadingEdgeAtBitIndex = 0;
self->internalRxBuffer = 0;
self->isSearchingForStartBit = false;
return;
}
if (softSerial->rxEdge == LEADING) {
softSerial->rxLastLeadingEdgeAtBitIndex = softSerial->rxBitIndex;
if (self->rxEdge == LEADING) {
self->rxLastLeadingEdgeAtBitIndex = self->rxBitIndex;
}
applyChangedBits(softSerial);
applyChangedBits(self);
if (softSerial->rxEdge == TRAILING) {
softSerial->rxEdge = LEADING;
timerChConfigIC(softSerial->rxTimerHardware, !inverted, 0);
if (self->rxEdge == TRAILING) {
self->rxEdge = LEADING;
timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
} else {
softSerial->rxEdge = TRAILING;
timerChConfigIC(softSerial->rxTimerHardware, inverted, 0);
self->rxEdge = TRAILING;
timerChConfigIC(self->timerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
}
#ifdef STM32F7
serialEnableCC(self);
#endif
}
/*
* Standard serial driver API
*/
uint32_t softSerialRxBytesWaiting(const serialPort_t *instance)
{
if ((instance->mode & MODE_RX) == 0) {
@ -445,7 +622,10 @@ void softSerialWriteByte(serialPort_t *s, uint8_t ch)
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
softSerial_t *softSerial = (softSerial_t *)s;
openSoftSerial(softSerial->softSerialPortIndex, s->rxCallback, baudRate, softSerial->port.options);
softSerial->port.baudRate = baudRate;
serialTimerConfigureTimebase(softSerial->timerHardware, baudRate);
}
void softSerialSetMode(serialPort_t *instance, portMode_t mode)
@ -458,8 +638,7 @@ bool isSoftSerialTransmitBufferEmpty(const serialPort_t *instance)
return instance->txBufferHead == instance->txBufferTail;
}
const struct serialPortVTable softSerialVTable[] = {
{
static const struct serialPortVTable softSerialVTable = {
.serialWrite = softSerialWriteByte,
.serialTotalRxWaiting = softSerialRxBytesWaiting,
.serialTotalTxFree = softSerialTxBytesFree,
@ -470,7 +649,6 @@ const struct serialPortVTable softSerialVTable[] = {
.writeBuf = NULL,
.beginWrite = NULL,
.endWrite = NULL
}
};
#endif

4
src/main/drivers/serial_softserial.h
View file

@ -24,8 +24,7 @@ typedef enum {
SOFTSERIAL2
} softSerialPortIndex_e;
serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portOptions_t options);
serialPort_t *softSerialLoopbackPort(void);
serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portMode_t mode, portOptions_t options);
// serialPort API
void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
@ -34,4 +33,3 @@ uint32_t softSerialTxBytesFree(const serialPort_t *instance);
uint8_t softSerialReadByte(serialPort_t *instance);
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isSoftSerialTransmitBufferEmpty(const serialPort_t *s);

3
src/main/drivers/serial_uart_hal.c
View file

@ -170,7 +170,8 @@ static void uartReconfigure(uartPort_t *uartPort)
__HAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
} else {
__HAL_UART_ENABLE_IT(&uartPort->Handle, UART_IT_TXE);
/* Enable the UART Transmit Data Register Empty Interrupt */
SET_BIT(uartPort->USARTx->CR1, USART_CR1_TXEIE);
}
}
return;

73
src/main/drivers/serial_uart_stm32f7xx.c
View file

@ -74,7 +74,9 @@ static uartDevice_t uart1 =
#ifdef USE_UART1_RX_DMA
.rxDMAStream = DMA2_Stream5,
#endif
#ifdef USE_UART1_TX_DMA
.txDMAStream = DMA2_Stream7,
#endif
.dev = USART1,
.rx = IO_TAG(UART1_RX_PIN),
.tx = IO_TAG(UART1_TX_PIN),
@ -97,7 +99,9 @@ static uartDevice_t uart2 =
#ifdef USE_UART2_RX_DMA
.rxDMAStream = DMA1_Stream5,
#endif
#ifdef USE_UART2_TX_DMA
.txDMAStream = DMA1_Stream6,
#endif
.dev = USART2,
.rx = IO_TAG(UART2_RX_PIN),
.tx = IO_TAG(UART2_TX_PIN),
@ -120,7 +124,9 @@ static uartDevice_t uart3 =
#ifdef USE_UART3_RX_DMA
.rxDMAStream = DMA1_Stream1,
#endif
#ifdef USE_UART3_TX_DMA
.txDMAStream = DMA1_Stream3,
#endif
.dev = USART3,
.rx = IO_TAG(UART3_RX_PIN),
.tx = IO_TAG(UART3_TX_PIN),
@ -143,7 +149,9 @@ static uartDevice_t uart4 =
#ifdef USE_UART4_RX_DMA
.rxDMAStream = DMA1_Stream2,
#endif
#ifdef USE_UART4_TX_DMA
.txDMAStream = DMA1_Stream4,
#endif
.dev = UART4,
.rx = IO_TAG(UART4_RX_PIN),
.tx = IO_TAG(UART4_TX_PIN),
@ -166,7 +174,9 @@ static uartDevice_t uart5 =
#ifdef USE_UART5_RX_DMA
.rxDMAStream = DMA1_Stream0,
#endif
#ifdef USE_UART5_TX_DMA
.txDMAStream = DMA1_Stream7,
#endif
.dev = UART5,
.rx = IO_TAG(UART5_RX_PIN),
.tx = IO_TAG(UART5_TX_PIN),
@ -189,7 +199,9 @@ static uartDevice_t uart6 =
#ifdef USE_UART6_RX_DMA
.rxDMAStream = DMA2_Stream1,
#endif
#ifdef USE_UART6_TX_DMA
.txDMAStream = DMA2_Stream6,
#endif
.dev = USART6,
.rx = IO_TAG(UART6_RX_PIN),
.tx = IO_TAG(UART6_TX_PIN),
@ -212,7 +224,9 @@ static uartDevice_t uart7 =
#ifdef USE_UART7_RX_DMA
.rxDMAStream = DMA1_Stream3,
#endif
#ifdef USE_UART7_TX_DMA
.txDMAStream = DMA1_Stream1,
#endif
.dev = UART7,
.rx = IO_TAG(UART7_RX_PIN),
.tx = IO_TAG(UART7_TX_PIN),
@ -234,7 +248,9 @@ static uartDevice_t uart8 =
#ifdef USE_UART8_RX_DMA
.rxDMAStream = DMA1_Stream6,
#endif
#ifdef USE_UART8_TX_DMA
.txDMAStream = DMA1_Stream0,
#endif
.dev = UART8,
.rx = IO_TAG(UART8_RX_PIN),
.tx = IO_TAG(UART8_TX_PIN),
@ -299,8 +315,7 @@ void uartIrqHandler(uartPort_t *s)
{
UART_HandleTypeDef *huart = &s->Handle;
/* UART in mode Receiver ---------------------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_RXNE) != RESET)) {
uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t) 0xff);
if (s->port.rxCallback) {
@ -318,42 +333,51 @@ void uartIrqHandler(uartPort_t *s)
}
/* UART parity error interrupt occurred -------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_PE) != RESET)) {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_PEF);
}
/* UART frame error interrupt occurred --------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_FE) != RESET)) {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_FEF);
}
/* UART noise error interrupt occurred --------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_NE) != RESET)) {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_NEF);
}
/* UART Over-Run interrupt occurred -----------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_ORE) != RESET)) {
__HAL_UART_CLEAR_IT(huart, UART_CLEAR_OREF);
}
/* UART in mode Transmitter ------------------------------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET))
{
HAL_UART_IRQHandler(huart);
if (!s->txDMAStream && (__HAL_UART_GET_IT(huart, UART_IT_TXE) != RESET)) {
/* Check that a Tx process is ongoing */
if (huart->gState != HAL_UART_STATE_BUSY_TX) {
if (s->port.txBufferTail == s->port.txBufferHead) {
huart->TxXferCount = 0;
/* Disable the UART Transmit Data Register Empty Interrupt */
CLEAR_BIT(huart->Instance->CR1, USART_CR1_TXEIE);
} else {
if ((huart->Init.WordLength == UART_WORDLENGTH_9B) && (huart->Init.Parity == UART_PARITY_NONE)) {
huart->Instance->TDR = (((uint16_t) s->port.txBuffer[s->port.txBufferTail]) & (uint16_t) 0x01FFU);
} else {
huart->Instance->TDR = (uint8_t)(s->port.txBuffer[s->port.txBufferTail]);
}
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
}
}
}
/* UART in mode Transmitter (transmission end) -----------------------------*/
if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET))
{
if ((__HAL_UART_GET_IT(huart, UART_IT_TC) != RESET)) {
HAL_UART_IRQHandler(huart);
if (s->txDMAStream) {
handleUsartTxDma(s);
}
}
}
static void handleUsartTxDma(uartPort_t *s)
@ -394,9 +418,16 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
s->rxDMAChannel = uart->DMAChannel;
s->rxDMAStream = uart->rxDMAStream;
}
if (uart->txDMAStream) {
s->txDMAChannel = uart->DMAChannel;
s->txDMAStream = uart->txDMAStream;
// DMA TX Interrupt
dmaInit(uart->txIrq, OWNER_SERIAL, RESOURCE_INDEX(device));
dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart);
}
s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->TDR;
s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->RDR;
@ -421,15 +452,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
}
}
// DMA TX Interrupt
dmaSetHandler(uart->txIrq, dmaIRQHandler, uart->txPriority, (uint32_t)uart);
//HAL_NVIC_SetPriority(uart->txIrq, NVIC_PRIORITY_BASE(uart->txPriority), NVIC_PRIORITY_SUB(uart->txPriority));
//HAL_NVIC_EnableIRQ(uart->txIrq);
if (!s->rxDMAChannel)
{
if (!s->rxDMAChannel) {
HAL_NVIC_SetPriority(uart->rxIrq, NVIC_PRIORITY_BASE(uart->rxPriority), NVIC_PRIORITY_SUB(uart->rxPriority));
HAL_NVIC_EnableIRQ(uart->rxIrq);
}

9
src/main/drivers/system_stm32f4xx.c
View file

@ -34,22 +34,13 @@ void SetSysClock(void);
void systemReset(void)
{
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
NVIC_SystemReset();
}
void systemResetToBootloader(void)
{
if (mpuResetFn) {
mpuResetFn();
}
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
__disable_irq();
NVIC_SystemReset();
}

9
src/main/drivers/system_stm32f7xx.c
View file

@ -33,22 +33,13 @@ void SystemClock_Config(void);
void systemReset(void)
{
if (mpuResetFn) {
mpuResetFn();
}
__disable_irq();
NVIC_SystemReset();
}
void systemResetToBootloader(void)
{
if (mpuResetFn) {
mpuResetFn();
}
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot
__disable_irq();
NVIC_SystemReset();
}

12
src/main/drivers/timer_stm32f4xx.c
View file

@ -72,12 +72,16 @@ void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t
uint8_t timerClockDivisor(TIM_TypeDef *tim)
{
#if defined (STM32F40_41xxx)
if (tim == TIM8) return 1;
#endif
if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
#if defined (STM32F411xE)
UNUSED(tim);
return 1;
#elif defined (STM32F40_41xxx) || defined (STM32F427_437xx)
if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
return 1;
} else {
return 2;
}
#else
#error "No timer clock defined correctly for the MCU"
#endif
}

36
src/main/fc/cli.c
View file

@ -791,6 +791,8 @@ static void cliSerial(char *cmdline)
#ifdef USE_SERIAL_PASSTHROUGH
static void cliSerialPassthrough(char *cmdline)
{
char * saveptr;
if (isEmpty(cmdline)) {
cliShowParseError();
return;
@ -799,7 +801,7 @@ static void cliSerialPassthrough(char *cmdline)
int id = -1;
uint32_t baud = 0;
unsigned mode = 0;
char* tok = strtok(cmdline, " ");
char* tok = strtok_r(cmdline, " ", &saveptr);
int index = 0;
while (tok != NULL) {
@ -818,7 +820,7 @@ static void cliSerialPassthrough(char *cmdline)
break;
}
index++;
tok = strtok(NULL, " ");
tok = strtok_r(NULL, " ", &saveptr);
}
serialPort_t *passThroughPort;
@ -1235,16 +1237,18 @@ static void printModeColor(uint8_t dumpMask, const ledStripConfig_t *ledStripCon
static void cliModeColor(char *cmdline)
{
char * saveptr;
if (isEmpty(cmdline)) {
printModeColor(DUMP_MASTER, ledStripConfig(), NULL);
} else {
enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT};
int args[ARGS_COUNT];
int argNo = 0;
const char* ptr = strtok(cmdline, " ");
const char* ptr = strtok_r(cmdline, " ", &saveptr);
while (ptr && argNo < ARGS_COUNT) {
args[argNo++] = fastA2I(ptr);
ptr = strtok(NULL, " ");
ptr = strtok_r(NULL, " ", &saveptr);
}
if (ptr != NULL || argNo != ARGS_COUNT) {
@ -1428,6 +1432,7 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer
static void cliServoMix(char *cmdline)
{
char * saveptr;
int args[8], check = 0;
uint8_t len = strlen(cmdline);
@ -1476,10 +1481,10 @@ static void cliServoMix(char *cmdline)
return;
}
ptr = strtok(ptr, " ");
ptr = strtok_r(ptr, " ", &saveptr);
while (ptr != NULL && check < ARGS_COUNT - 1) {
args[check++] = fastA2I(ptr);
ptr = strtok(NULL, " ");
ptr = strtok_r(NULL, " ", &saveptr);
}
if (ptr == NULL || check != ARGS_COUNT - 1) {
@ -1500,10 +1505,10 @@ static void cliServoMix(char *cmdline)
cliServoMix("reverse");
} else {
enum {RULE = 0, TARGET, INPUT, RATE, SPEED, ARGS_COUNT};
char *ptr = strtok(cmdline, " ");
char *ptr = strtok_r(cmdline, " ", &saveptr);
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = fastA2I(ptr);
ptr = strtok(NULL, " ");
ptr = strtok_r(NULL, " ", &saveptr);
}
if (ptr != NULL || check != ARGS_COUNT) {
@ -2336,9 +2341,6 @@ static void cliStatus(char *cmdline)
const int sensorHardwareIndex = detectedSensors[i];
const char *sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.dev.revisionCode) {
cliPrintf(".%c", acc.dev.revisionCode);
}
}
}
cliPrintLinefeed();
@ -2419,6 +2421,18 @@ static void cliStatus(char *cmdline)
const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX)));
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintLinef(", cycle time: %d, PID rate: %d, RX rate: %d, System rate: %d", (uint16_t)cycleTime, pidRate, rxRate, systemRate);
#if !defined(CLI_MINIMAL_VERBOSITY)
cliPrint("Arming disabled flags:");
uint32_t flags = armingFlags & ARMING_DISABLED_ALL_FLAGS;
while (flags) {
int bitpos = ffs(flags) - 1;
flags &= ~(1 << bitpos);
if (bitpos > 6) cliPrintf(" %s", armingDisableFlagNames[bitpos - 7]);
}
cliPrintLinefeed();
#else
cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS);
#endif
}
#ifndef SKIP_TASK_STATISTICS

5
src/main/fc/fc_core.c
View file

@ -178,11 +178,14 @@ static void updateArmingStatus(void)
}
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus() != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
}
}
/* CHECK: Angle */
if (!STATE(SMALL_ANGLE)) {
@ -654,7 +657,7 @@ void taskGyro(timeUs_t currentTimeUs) {
// getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
timeUs_t gyroUpdateUs;
timeUs_t gyroUpdateUs = currentTimeUs;
if (gyroConfig()->gyroSync) {
while (true) {

38
src/main/fc/fc_msp.c
View file

@ -379,6 +379,26 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
break;
case MSP2_INAV_STATUS:
{
// Preserves full arming flags and box modes
boxBitmask_t mspBoxModeFlags;
packBoxModeFlags(&mspBoxModeFlags);
sbufWriteU16(dst, (uint16_t)cycleTime);
#ifdef USE_I2C
sbufWriteU16(dst, i2cGetErrorCounter());
#else
sbufWriteU16(dst, 0);
#endif
sbufWriteU16(dst, packSensorStatus());
sbufWriteU16(dst, averageSystemLoadPercent);
sbufWriteU8(dst, getConfigProfile());
sbufWriteU32(dst, armingFlags);
sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags));
}
break;
case MSP_RAW_IMU:
{
// Hack scale due to choice of units for sensor data in multiwii
@ -1120,6 +1140,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
#endif
case MSP_NAME:
{
const char *name = systemConfig()->name;
while (*name) {
sbufWriteU8(dst, *name++);
}
}
break;
case MSP2_COMMON_TZ:
sbufWriteU16(dst, (uint16_t)timeConfig()->tz_offset);
break;
@ -1967,6 +1996,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
case MSP_SET_NAME:
{
char *name = systemConfigMutable()->name;
int len = MIN(MAX_NAME_LENGTH, (int)dataSize);
sbufReadData(src, name, len);
memset(&name[len], '\0', (MAX_NAME_LENGTH + 1) - len);
}
break;
case MSP2_COMMON_SET_TZ:
timeConfigMutable()->tz_offset = (int16_t)sbufReadU16(src);
break;

Some files were not shown because too many files have changed in this diff Show more