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

Merge branch 'softserial'

Browse source 
Conflicts:
	src/board.h
	src/drv_pwm.c
	src/drv_softserial.c
	src/drv_softserial.h
	src/drv_system.c
	src/drv_timer.c
	src/drv_uart.c
	src/drv_uart.h
	src/main.c
	src/sensors.c
This commit is contained in:
Dominic Clifton 2013-10-01 22:16:25 +01:00
commit 750fea4b80
54 changed files with 5720 additions and 5677 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -52,14 +52,17 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
mw.c \
sensors.c \
serial.c \
sbus.c \
spektrum.c \
telemetry.c \
drv_gpio.c \
drv_i2c.c \
drv_i2c_soft.c \
drv_system.c \
drv_serial.c \
drv_uart.c \
printf.c \
utils.c \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)
@ -67,6 +70,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
NAZE_SRC = drv_spi.c \
drv_adc.c \
drv_adxl345.c \
drv_bma280.c \
drv_bmp085.c \
drv_ms5611.c \
drv_hcsr04.c \

60
baseflight.uvproj
View file

@ -462,6 +462,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -657,6 +667,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1293,6 +1313,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1488,6 +1518,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2068,6 +2108,16 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
<File>
<FileName>sbus.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\sbus.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2503,6 +2553,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_softserial.c</FilePath>
</File>
<File>
<FileName>drv_bma280.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_bma280.c</FilePath>
</File>
<File>
<FileName>drv_serial.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_serial.c</FilePath>
</File>
</Files>
</Group>
<Group>

6263
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

76
src/board.h
View file

@ -28,11 +28,11 @@
#endif /* M_PI */
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define RAD (M_PI / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// Chip Unique ID on F103
#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
@ -40,11 +40,12 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,
SENSOR_MAG = 1 << 2,
SENSOR_SONAR = 1 << 3,
SENSOR_GPS = 1 << 4,
SENSOR_GYRO = 1 << 0, // always present
SENSOR_ACC = 1 << 1,
SENSOR_BARO = 1 << 2,
SENSOR_MAG = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_GPS = 1 << 5,
} AvailableSensors;
// Type of accelerometer used/detected
@ -53,13 +54,15 @@ typedef enum AccelSensors {
ACC_ADXL345 = 1,
ACC_MPU6050 = 2,
ACC_MMA8452 = 3,
ACC_BMA280 = 4,
ACC_NONE = 5
} AccelSensors;
typedef enum {
FEATURE_PPM = 1 << 0,
FEATURE_VBAT = 1 << 1,
FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
FEATURE_SPEKTRUM = 1 << 3,
FEATURE_SERIALRX = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_GYRO_SMOOTHING = 1 << 6,
@ -73,16 +76,57 @@ typedef enum {
FEATURE_3D = 1 << 14,
} AvailableFeatures;
typedef enum {
SERIALRX_SPEKTRUM1024 = 0,
SERIALRX_SPEKTRUM2048 = 1,
SERIALRX_SBUS = 2,
} SerialRXType;
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX,
GPS_MTK,
} GPSHardware;
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
typedef enum {
X = 0,
Y,
Z
} sensor_axis_e;
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,
CW90_DEG = 2,
CW180_DEG = 3,
CW270_DEG = 4,
CW0_DEG_FLIP = 5,
CW90_DEG_FLIP = 6,
CW180_DEG_FLIP = 7,
CW270_DEG_FLIP = 8
} sensor_align_e;
enum {
GYRO_UPDATED = 1 << 0,
ACC_UPDATED = 1 << 1,
MAG_UPDATED = 1 << 2,
TEMP_UPDATED = 1 << 3
};
typedef struct sensor_data_t
{
int16_t gyro[3];
int16_t acc[3];
int16_t mag[3];
float temperature;
int updated;
} sensor_data_t;
typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (* baroOpFuncPtr)(void); // baro start operation
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype
@ -90,7 +134,6 @@ typedef struct sensor_t
{
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr align; // sensor align
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor (currently used for gyro only, todo for accel)
} sensor_t;
@ -99,10 +142,10 @@ typedef struct baro_t
{
uint16_t ut_delay;
uint16_t up_delay;
sensorInitFuncPtr start_ut;
sensorInitFuncPtr get_ut;
sensorInitFuncPtr start_up;
sensorInitFuncPtr get_up;
baroOpFuncPtr start_ut;
baroOpFuncPtr get_ut;
baroOpFuncPtr start_up;
baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baro_t;
@ -204,6 +247,8 @@ typedef struct baro_t
#undef SOFT_I2C // enable to test software i2c
#include "utils.h"
#ifdef FY90Q
// FY90Q
#include "drv_adc.h"
@ -223,6 +268,7 @@ typedef struct baro_t
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_serial.h"
#include "drv_uart.h"
#include "drv_softserial.h"
#else
@ -230,6 +276,7 @@ typedef struct baro_t
// AfroFlight32
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bma280.h"
#include "drv_bmp085.h"
#include "drv_ms5611.h"
#include "drv_hmc5883l.h"
@ -242,6 +289,7 @@ typedef struct baro_t
#include "drv_l3g4200d.h"
#include "drv_pwm.h"
#include "drv_timer.h"
#include "drv_serial.h"
#include "drv_uart.h"
#include "drv_softserial.h"
#include "drv_hcsr04.h"

27
src/cli.c
View file

@ -42,7 +42,7 @@ static const char * const mixerNames[] = {
// sync this with AvailableFeatures enum from board.h
static const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
NULL
@ -54,7 +54,7 @@ static const char * const sensorNames[] = {
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", NULL
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "None", NULL
};
typedef struct {
@ -116,21 +116,16 @@ const clivalue_t valueTable[] = {
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
{ "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
{ "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 },
{ "spektrum_hires", VAR_UINT8, &mcfg.spektrum_hires, 0, 1 },
{ "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
{ "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },
{ "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 },
{ "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 },
{ "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 },
{ "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 },
{ "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 },
{ "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 },
{ "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 },
{ "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 },
{ "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 },
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
{ "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
{ "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
@ -152,7 +147,9 @@ const clivalue_t valueTable[] = {
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, 2000 },
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
@ -934,8 +931,8 @@ void cliProcess(void)
cliPrompt();
}
while (isUartAvailable(core.mainport)) {
uint8_t c = uartRead(core.mainport);
while (serialTotalBytesWaiting(core.mainport)) {
uint8_t c = serialRead(core.mainport);
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;

12
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static const uint8_t EEPROM_CONF_VERSION = 49;
static const uint8_t EEPROM_CONF_VERSION = 51;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -159,7 +159,6 @@ void checkFirstTime(bool reset)
static void resetConf(void)
{
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
// Clear all configuration
memset(&mcfg, 0, sizeof(master_t));
@ -178,15 +177,18 @@ static void resetConf(void)
mcfg.accZero[0] = 0;
mcfg.accZero[1] = 0;
mcfg.accZero[2] = 0;
memcpy(&mcfg.align, default_align, sizeof(mcfg.align));
mcfg.gyro_align = ALIGN_DEFAULT;
mcfg.acc_align = ALIGN_DEFAULT;
mcfg.mag_align = ALIGN_DEFAULT;
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.yaw_control_direction = 1;
mcfg.moron_threshold = 32;
mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
mcfg.vbatscale = 110;
mcfg.vbatmaxcellvoltage = 43;
mcfg.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0;
mcfg.spektrum_hires = 0;
mcfg.serialrx_type = 0;
mcfg.midrc = 1500;
mcfg.mincheck = 1100;
mcfg.maxcheck = 1900;
@ -207,6 +209,7 @@ static void resetConf(void)
// serial (USART1) baudrate
mcfg.serial_baudrate = 115200;
mcfg.looptime = 3500;
mcfg.rssi_aux_channel = 0;
cfg.pidController = 0;
cfg.P8[ROLL] = 40;
@ -274,6 +277,7 @@ static void resetConf(void)
// servos
cfg.yaw_direction = 1;
cfg.tri_unarmed_servo = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;

28
src/drv_adxl345.c
View file

@ -31,11 +31,11 @@
extern uint16_t acc_1G;
static void adxl345Init(void);
static void adxl345Init(sensor_align_e align);
static void adxl345Read(int16_t *accelData);
static void adxl345Align(int16_t *accelData);
static bool useFifo = false;
static sensor_align_e accAlign = CW0_DEG;
bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
{
@ -55,11 +55,10 @@ bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
acc->init = adxl345Init;
acc->read = adxl345Read;
acc->align = adxl345Align;
return true;
}
static void adxl345Init(void)
static void adxl345Init(sensor_align_e align)
{
if (useFifo) {
uint8_t fifoDepth = 16;
@ -73,6 +72,9 @@ static void adxl345Init(void)
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
}
acc_1G = 265; // 3.3V operation
if (align > 0)
accAlign = align;
}
uint8_t acc_samples = 0;
@ -80,6 +82,7 @@ uint8_t acc_samples = 0;
static void adxl345Read(int16_t *accelData)
{
uint8_t buf[8];
int16_t data[3];
if (useFifo) {
int32_t x = 0;
@ -96,19 +99,16 @@ static void adxl345Read(int16_t *accelData)
z += (int16_t)(buf[4] + (buf[5] << 8));
samples_remaining = buf[7] & 0x7F;
} while ((i < 32) && (samples_remaining > 0));
accelData[0] = x / i;
accelData[1] = y / i;
accelData[2] = z / i;
data[0] = x / i;
data[1] = y / i;
data[2] = z / i;
acc_samples = i;
} else {
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
accelData[0] = buf[0] + (buf[1] << 8);
accelData[1] = buf[2] + (buf[3] << 8);
accelData[2] = buf[4] + (buf[5] << 8);
data[0] = buf[0] + (buf[1] << 8);
data[1] = buf[2] + (buf[3] << 8);
data[2] = buf[4] + (buf[5] << 8);
}
}
static void adxl345Align(int16_t *accData)
{
// official direction is RPY, nothing to change here.
alignSensors(data, accelData, accAlign);
}

54
src/drv_bma280.c Normal file
View file

@ -0,0 +1,54 @@
#include "board.h"
// BMA280, default I2C address mode 0x18
#define BMA280_ADDRESS 0x18
#define BMA280_ACC_X_LSB 0x02
#define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F
extern uint16_t acc_1G;
static void bma280Init(sensor_align_e align);
static void bma280Read(int16_t *accelData);
static sensor_align_e accAlign = CW0_DEG;
bool bma280Detect(sensor_t *acc)
{
bool ack = false;
uint8_t sig = 0;
ack = i2cRead(BMA280_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xFB)
return false;
acc->init = bma280Init;
acc->read = bma280Read;
return true;
}
static void bma280Init(sensor_align_e align)
{
i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc_1G = 512 * 8;
if (align > 0)
accAlign = align;
}
static void bma280Read(int16_t *accelData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
// Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
data[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
data[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
data[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
alignSensors(data, accelData, accAlign);
}

3
src/drv_bma280.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
bool bma280Detect(sensor_t *acc);

132
src/drv_hmc5883l.c
View file

@ -2,6 +2,62 @@
// HMC5883L, default address 0x1E
// PB12 connected to MAG_DRDY on rev4 hardware
// PC14 connected to MAG_DRDY on rev5 hardware
/* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800?0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800?0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800?0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800?0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800?0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800?0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800?0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800?0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
@ -9,17 +65,18 @@
#define HMC58X3_R_CONFA 0
#define HMC58X3_R_CONFB 1
#define HMC58X3_R_MODE 2
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16) // X axis level when bias current is applied.
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16) // Y axis level when bias current is applied.
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08) // Y axis level when bias current is applied.
#define SELF_TEST_LOW_LIMIT (243.0 / 390.0) // Low limit when gain is 5.
#define SELF_TEST_HIGH_LIMIT (575.0 / 390.0) // High limit when gain is 5.
#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.
#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when gain is 5.
#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when gain is 5.
#define HMC_POS_BIAS 1
#define HMC_NEG_BIAS 2
static int8_t sensor_align[3];
static sensor_align_e magAlign = CW180_DEG;
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
bool hmc5883lDetect(int8_t *align)
bool hmc5883lDetect(sensor_align_e align)
{
bool ack = false;
uint8_t sig = 0;
@ -28,15 +85,15 @@ bool hmc5883lDetect(int8_t *align)
if (!ack || sig != 'H')
return false;
memcpy(sensor_align, align, 3);
if (align > 0)
magAlign = align;
return true;
}
void hmc5883lInit(float *calibrationGain)
void hmc5883lInit(void)
{
gpio_config_t gpio;
float magGain[3];
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
@ -58,7 +115,7 @@ void hmc5883lInit(float *calibrationGain)
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + 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_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
@ -68,12 +125,12 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[0] += magADC[0];
xyz_total[1] += magADC[1];
xyz_total[2] += magADC[2];
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
@ -88,21 +145,21 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged.
xyz_total[0] -= magADC[0];
xyz_total[1] -= magADC[1];
xyz_total[2] -= magADC[2];
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);
magGain[X] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
@ -111,16 +168,9 @@ void hmc5883lInit(float *calibrationGain)
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[0] = 1.0;
magGain[1] = 1.0;
magGain[2] = 1.0;
}
// if parameter was passed, give calibration values back
if (calibrationGain) {
calibrationGain[0] = magGain[0];
calibrationGain[1] = magGain[1];
calibrationGain[2] = magGain[2];
magGain[X] = 1.0f;
magGain[Y] = 1.0f;
magGain[Z] = 1.0f;
}
}
@ -128,18 +178,12 @@ void hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
int16_t mag[3];
int i;
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
mag[0] = buf[0] << 8 | buf[1];
mag[1] = buf[2] << 8 | buf[3];
mag[2] = buf[4] << 8 | buf[5];
for (i = 0; i < 3; i++) {
int8_t axis = sensor_align[i];
if (axis > 0)
magData[axis - 1] = mag[i];
else
magData[-axis - 1] = -mag[i];
}
// 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.
mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(mag, magData, magAlign);
}

4
src/drv_hmc5883l.h
View file

@ -1,5 +1,5 @@
#pragma once
bool hmc5883lDetect(int8_t *align);
void hmc5883lInit(float *calibrationGain);
bool hmc5883lDetect(sensor_align_e align);
void hmc5883lInit(void);
void hmc5883lRead(int16_t *magData);

32
src/drv_l3g4200d.c
View file

@ -3,6 +3,7 @@
// L3G4200D, Standard address 0x68
#define L3G4200D_ADDRESS 0x68
#define L3G4200D_ID 0xD3
#define L3G4200D_AUTOINCR 0x80
// Registers
#define L3G4200D_WHO_AM_I 0x0F
@ -13,7 +14,7 @@
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_GYRO_OUT 0xA8
#define L3G4200D_GYRO_OUT 0x28
// Bits
#define L3G4200D_POWER_ON 0x0F
@ -24,10 +25,10 @@
#define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static void l3g4200dInit(void);
static void l3g4200dInit(sensor_align_e align);
static void l3g4200dRead(int16_t *gyroData);
static void l3g4200dAlign(int16_t *gyroData);
bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
{
@ -41,7 +42,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
gyro->init = l3g4200dInit;
gyro->read = l3g4200dRead;
gyro->align = l3g4200dAlign;
// 14.2857dps/lsb scalefactor
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
@ -65,7 +66,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
return true;
}
static void l3g4200dInit(void)
static void l3g4200dInit(sensor_align_e align)
{
bool ack;
@ -77,21 +78,20 @@ static void l3g4200dInit(void)
delay(5);
i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
}
static void l3g4200dAlign(int16_t *gyroData)
{
gyroData[0] = -gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
if (align > 0)
gyroAlign = align;
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void l3g4200dRead(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
gyroData[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
int16_t data[3];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

24
src/drv_mma845x.c
View file

@ -49,10 +49,10 @@
extern uint16_t acc_1G;
static uint8_t device_id;
static sensor_align_e accAlign = CW90_DEG;
static void mma8452Init(void);
static void mma8452Init(sensor_align_e align);
static void mma8452Read(int16_t *accelData);
static void mma8452Align(int16_t *accelData);
bool mma8452Detect(sensor_t *acc)
{
@ -69,12 +69,11 @@ bool mma8452Detect(sensor_t *acc)
acc->init = mma8452Init;
acc->read = mma8452Read;
acc->align = mma8452Align;
device_id = sig;
return true;
}
static void mma8452Init(void)
static void mma8452Init(sensor_align_e align)
{
gpio_config_t gpio;
@ -95,21 +94,20 @@ static void mma8452Init(void)
i2cWrite(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.
acc_1G = 256;
if (align > 0)
accAlign = align;
}
static void mma8452Read(int16_t *accelData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
}
data[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
static void mma8452Align(int16_t *accelData)
{
accelData[0] = -accelData[0];
accelData[1] = -accelData[1];
accelData[2] = accelData[2];
alignSensors(data, accelData, accAlign);
}

26
src/drv_mpu3050.c
View file

@ -25,10 +25,10 @@
#define MPU3050_CLK_SEL_PLL_GX 0x01
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static void mpu3050Init(void);
static void mpu3050Init(sensor_align_e align);
static void mpu3050Read(int16_t *gyroData);
static void mpu3050Align(int16_t *gyroData);
static void mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
@ -43,7 +43,6 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
gyro->init = mpu3050Init;
gyro->read = mpu3050Read;
gyro->align = mpu3050Align;
gyro->temperature = mpu3050ReadTemp;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
@ -74,7 +73,7 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
return true;
}
static void mpu3050Init(void)
static void mpu3050Init(sensor_align_e align)
{
bool ack;
@ -88,24 +87,23 @@ static void mpu3050Init(void)
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static void mpu3050Align(int16_t *gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
if (align > 0)
gyroAlign = align;
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void mpu3050Read(int16_t *gyroData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
static void mpu3050ReadTemp(int16_t *tempData)

599
src/drv_mpu6050.c
View file

@ -4,26 +4,9 @@
// MPU_INT on PB13 on rev4 hardware
#define MPU6050_ADDRESS 0x68
// Experimental DMP support
// #define MPU6050_DMP
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
#define INV_MAX_NUM_ACCEL_SAMPLES (8)
#define DMP_REF_QUATERNION (0)
#define DMP_REF_GYROS (DMP_REF_QUATERNION + 4) // 4
#define DMP_REF_CONTROL (DMP_REF_GYROS + 3) // 7
#define DMP_REF_RAW (DMP_REF_CONTROL + 4) // 11
#define DMP_REF_RAW_EXTERNAL (DMP_REF_RAW + 8) // 19
#define DMP_REF_ACCEL (DMP_REF_RAW_EXTERNAL + 6) // 25
#define DMP_REF_QUANT_ACCEL (DMP_REF_ACCEL + 3) // 28
#define DMP_REF_QUATERNION_6AXIS (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) // 36
#define DMP_REF_EIS (DMP_REF_QUATERNION_6AXIS + 4) // 40
#define DMP_REF_DMP_PACKET (DMP_REF_EIS + 3) // 43
#define DMP_REF_GARBAGE (DMP_REF_DMP_PACKET + 1) // 44
#define DMP_REF_LAST (DMP_REF_GARBAGE + 1) // 45
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
@ -113,7 +96,7 @@
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75
#define MPU6050_SMPLRT_DIV 0 //8000Hz
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
#define MPU6050_LPF_256HZ 0
#define MPU6050_LPF_188HZ 1
@ -124,19 +107,13 @@
#define MPU6050_LPF_5HZ 6
static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static sensor_align_e accAlign = CW0_DEG;
static void mpu6050AccInit(void);
static void mpu6050AccInit(sensor_align_e align);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050AccAlign(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroInit(sensor_align_e align);
static void mpu6050GyroRead(int16_t *gyroData);
static void mpu6050GyroAlign(int16_t *gyroData);
#ifdef MPU6050_DMP
static void mpu6050DmpInit(void);
float dmpdata[2];
int16_t dmpGyroData[3];
#endif
extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;
@ -186,12 +163,11 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
acc->align = mpu6050AccAlign;
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
gyro->align = mpu6050GyroAlign;
// 16.4 dps/lsb scalefactor
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
// give halfacc (old revision) back to system
if (scale)
@ -223,48 +199,34 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
break;
}
#ifdef MPU6050_DMP
mpu6050DmpInit();
#endif
return true;
}
static void mpu6050AccInit(void)
static void mpu6050AccInit(sensor_align_e align)
{
if (mpuAccelHalf)
acc_1G = 255 * 8;
else
acc_1G = 512 * 8;
if (align > 0)
accAlign = align;
}
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
int16_t data[3];
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
alignSensors(data, accData, accAlign);
}
static void mpu6050AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1];
accData[1] = -temp[0];
accData[2] = accData[2];
}
static void mpu6050GyroInit(void)
static void mpu6050GyroInit(sensor_align_e align)
{
gpio_config_t gpio;
@ -277,7 +239,6 @@ static void mpu6050GyroInit(void)
else if (hse_value == 12000000)
gpioInit(GPIOC, &gpio);
#ifndef MPU6050_DMP
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
@ -289,522 +250,20 @@ static void mpu6050GyroInit(void)
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
#endif
if (align > 0)
gyroAlign = align;
}
static void mpu6050GyroRead(int16_t * gyroData)
static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
#ifndef MPU6050_DMP
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
#else
gyroData[0] = dmpGyroData[0] / 4 ;
gyroData[1] = dmpGyroData[1] / 4;
gyroData[2] = dmpGyroData[2] / 4;
#endif
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
static void mpu6050GyroAlign(int16_t * gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
#ifdef MPU6050_DMP
//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
//library only supports 32 byte transmissions, including the register address to which you're writing,
//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
//
//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
//directly from that code. That is true of all transmissions in this sketch, and any documentation has
//been added after the fact by referencing the Invensense code.
const unsigned char dmpMem[8][16][16] = {
{
{0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00},
{0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01},
{0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00},
{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
{0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82},
{0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC},
{0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4},
{0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10}
},
{
{0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00},
{0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8},
{0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C},
{0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C},
{0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0}
},
{
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
},
{
{0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F},
{0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2},
{0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF},
{0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C},
{0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1},
{0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01},
{0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80},
{0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C},
{0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80},
{0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E},
{0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9},
{0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24},
{0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0},
{0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86},
{0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1},
{0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86}
},
{
{0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA},
{0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C},
{0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8},
{0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3},
{0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84},
{0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5},
{0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3},
{0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1},
{0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5},
{0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A},
{0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8},
{0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87}
},
{
{0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8},
{0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68},
{0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D},
{0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94},
{0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA},
{0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56},
{0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9},
{0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA},
{0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A},
{0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60},
{0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97},
{0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04},
{0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78},
{0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79},
{0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68},
{0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68}
},
{
{0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04},
{0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66},
{0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31},
{0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60},
{0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76},
{0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56},
{0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD},
{0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91},
{0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8},
{0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE},
{0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9},
{0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD},
{0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E},
{0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8},
{0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89},
{0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79}
},
{
{0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8},
{0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA},
{0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB},
{0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3},
{0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3},
{0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3},
{0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3},
{0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC},
{0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF}
}
};
//DMP update transmissions (Bank, Start Address, Update Length, Update Data...)
const uint8_t dmp_updates[29][9] = {
{0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C}, //FCFG_1 inv_set_gyro_calibration
{0x03, 0xAB, 0x03, 0x36, 0x56, 0x76}, //FCFG_3 inv_set_gyro_calibration
{0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2}, //D_0_104 inv_set_gyro_calibration
{0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1}, //D_0_24 inv_set_gyro_calibration
{0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00}, //D_1_152 inv_set_accel_calibration
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_accel_calibration
{0x03, 0x89, 0x03, 0x26, 0x46, 0x66}, //FCFG_7 inv_set_accel_calibration
{0x00, 0x6C, 0x02, 0x20, 0x00}, //D_0_108 inv_set_accel_calibration
{0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_00 inv_set_compass_calibration
{0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_01
{0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_02
{0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_10
{0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_11
{0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_12
{0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_20
{0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_21
{0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_22
{0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00}, //D_1_236 inv_apply_endian_accel
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_mpu_sensors
{0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D}, //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
{0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D}, //FCFG_5 inv_set_bias_update
{0x00, 0xA3, 0x01, 0x00}, //D_0_163 inv_set_dead_zone
//SET INT_ENABLE at i=22
{0x07, 0x86, 0x01, 0xFE}, //CFG_6 inv_set_fifo_interupt
{0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38}, //CFG_8 inv_send_quaternion
{0x07, 0x7E, 0x01, 0x30}, //CFG_16 inv_set_footer
{0x07, 0x46, 0x01, 0x9A}, //CFG_GYRO_SOURCE inv_send_gyro
{0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
{0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
{0x02, 0x16, 0x02, 0x00, 0x00}, //D_0_22 inv_set_fifo_rate
};
static long dmp_lastRead = 0;
static uint8_t dmp_processed_packet[8];
static uint8_t dmp_received_packet[50];
static uint8_t dmp_temp = 0;
uint8_t dmp_fifoCountL = 0;
static uint8_t dmp_fifoCountL2 = 0;
static uint8_t dmp_packetCount = 0x00;
static bool dmp_longPacket = false;
static bool dmp_firstPacket = true;
static void mpu6050DmpMemInit(void);
static void mpu6050DmpBankSelect(uint8_t bank);
static bool mpu6050DmpFifoReady(void);
static void mpu6050DmpGetPacket(void);
static void mpu6050DmpProcessQuat(void);
void mpu6050DmpResetFifo(void);
static void mpu6050DmpInit(void)
{
uint8_t temp = 0;
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0xC0); // device reset
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
delay(10);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x70);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x06);
i2cRead(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x00);
/*
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_XG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_YG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_ZG_OFFS_TC);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL);
*/
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0x32); // I2C bypass enabled, latch int enabled, int read clear
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
delay(5);
mpu6050DmpMemInit();
}
void mpu6050DmpLoop(void)
{
uint8_t temp;
uint8_t buf[2];
if (mpu6050DmpFifoReady()) {
LED1_ON;
mpu6050DmpGetPacket();
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
if (dmp_firstPacket) {
delay(1);
mpu6050DmpBankSelect(0x00);
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x62);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 2, buf);
dmp_firstPacket = false;
mpu6050DmpFifoReady();
}
if (dmp_fifoCountL == 42) {
mpu6050DmpProcessQuat();
}
LED1_OFF;
}
}
#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
extern int16_t angle[2];
static void mpu6050DmpProcessQuat(void)
{
float quat0, quat1, quat2, quat3;
int32_t quatl0, quatl1, quatl2, quatl3;
quatl0 = dmp_quatTake32(dmp_received_packet, 0);
quatl1 = dmp_quatTake32(dmp_received_packet, 1);
quatl2 = dmp_quatTake32(dmp_received_packet, 2);
quatl3 = dmp_quatTake32(dmp_received_packet, 3);
if (quatl0 > 32767)
quatl0 -= 65536;
if (quatl1 > 32767)
quatl1 -= 65536;
if (quatl2 > 32767)
quatl2 -= 65536;
if (quatl3 > 32767)
quatl3 -= 65536;
quat0 = ((float) quatl0) / 16384.0f;
quat1 = ((float) quatl1) / 16384.0f;
quat2 = ((float) quatl2) / 16384.0f;
quat3 = ((float) quatl3) / 16384.0f;
dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
angle[0] = dmpdata[0] * 10;
angle[1] = dmpdata[1] * 10;
dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
}
void mpu6050DmpResetFifo(void)
{
uint8_t ctrl;
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &ctrl);
ctrl |= 0x04;
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, ctrl);
}
static void mpu6050DmpGetPacket(void)
{
if (dmp_fifoCountL > 32) {
dmp_fifoCountL2 = dmp_fifoCountL - 32;
dmp_longPacket = true;
}
if (dmp_longPacket) {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, 32, dmp_received_packet);
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet + 32);
dmp_longPacket = false;
} else {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet);
}
}
uint16_t dmpFifoLevel = 0;
static bool mpu6050DmpFifoReady(void)
{
uint8_t buf[2];
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmp_fifoCountL = buf[1];
dmpFifoLevel = buf[0] << 8 | buf[1];
if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
return true;
else {
// lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
if (dmpFifoLevel > 100) {
// clear out fifo
uint8_t crap[16];
do {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmpFifoLevel = buf[0] << 8 | buf[1];
} while (dmpFifoLevel);
}
}
return false;
}
static void mpu6050DmpBankSelect(uint8_t bank)
{
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, bank);
}
static void mpu6050DmpBankInit(void)
{
uint8_t i, j;
uint8_t incoming[9];
for (i = 0; i < 7; i++) {
mpu6050DmpBankSelect(i);
for (j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
}
}
mpu6050DmpBankSelect(7);
for (j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
}
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, 0x80);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 8, incoming);
}
static void mpu6050DmpMemInit(void)
{
uint8_t i;
uint8_t temp;
mpu6050DmpBankInit();
// Bank, Start Address, Update Length, Update Data...
for (i = 0; i < 22; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x32);
for (i = 22; i < 29; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
/*
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_1);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_2);
*/
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x02); // ??
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x04);
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); // full scale 2000 deg/s
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_1, 0x03);
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_2, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_XG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_YG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZG_OFFS_TC, 0x00);
// clear offsets
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0xB2);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x90);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
// Insert FIFO count read?
mpu6050DmpFifoReady();
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_THR, 0x02);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_THR, 0x9C);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_DUR, 0x50);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_DUR, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0xC8); // fifo enable
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x6A);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
}
#else /* MPU6050_DMP */
void mpu6050DmpLoop(void)
{
}
void mpu6050DmpResetFifo(void)
{
}
#endif /* MPU6050_DMP */

26
src/drv_ms5611.c
View file

@ -154,29 +154,27 @@ static void ms5611_get_up(void)
static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
{
int32_t temp, off2 = 0, sens2 = 0, delt;
int32_t press;
int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
temp = 2000 + ((dT * ms5611_c[6]) >> 23);
uint32_t press;
int64_t temp;
int64_t delt;
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
if (temp < 2000) { // temperature lower than 20degC
delt = temp - 2000;
delt = 5 * delt * delt;
off2 = delt >> 1;
sens2 = delt >> 2;
off -= delt >> 1;
sens -= delt >> 2;
if (temp < -1500) { // temperature lower than -15degC
delt = temp + 1500;
delt = delt * delt;
off2 += 7 * delt;
sens2 += (11 * delt) >> 1;
off -= 7 * delt;
sens -= (11 * delt) >> 1;
}
}
off -= off2;
sens -= sens2;
press = (((ms5611_up * sens ) >> 21) - off) >> 15;
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
if (pressure)
*pressure = press;

2
src/drv_pwm.c
View file

@ -325,7 +325,7 @@ bool pwmInit(drv_pwm_config_t *init)
#ifdef SOFTSERIAL_19200_LOOPBACK
// skip softSerial ports
if ((port == PWM5 || port == PWM6))
if ((port == PWM5 || port == PWM6 || port == PWM7 || port == PWM8))
continue;
#endif

40
src/drv_serial.c Normal file
View file

@ -0,0 +1,40 @@
#include "board.h"
void serialPrint(serialPort_t *instance, const char *str)
{
uint8_t ch;
while ((ch = *(str++))) {
serialWrite(instance, ch);
}
}
inline uint32_t serialGetBaudRate(serialPort_t *instance)
{
return instance->baudRate;
}
inline void serialWrite(serialPort_t *instance, uint8_t ch)
{
instance->vTable->serialWrite(instance, ch);
}
inline uint8_t serialTotalBytesWaiting(serialPort_t *instance)
{
return instance->vTable->serialTotalBytesWaiting(instance);
}
inline uint8_t serialRead(serialPort_t *instance)
{
return instance->vTable->serialRead(instance);
}
inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
instance->vTable->serialSetBaudRate(instance, baudRate);
}
inline bool isSerialTransmitBufferEmpty(serialPort_t *instance)
{
return instance->vTable->isSerialTransmitBufferEmpty(instance);
}

48
src/drv_serial.h Normal file
View file

@ -0,0 +1,48 @@
#pragma once
typedef enum portMode_t {
MODE_RX = 1,
MODE_TX = 2,
MODE_RXTX = MODE_RX | MODE_TX
} portMode_t;
typedef struct serialPort {
const struct serialPortVTable *vTable;
portMode_t mode;
uint32_t baudRate;
uint32_t rxBufferSize;
uint32_t txBufferSize;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
uint32_t rxBufferHead;
uint32_t rxBufferTail;
uint32_t txBufferHead;
uint32_t txBufferTail;
// FIXME rename member to rxCallback
serialReceiveCallbackPtr callback;
} serialPort_t;
struct serialPortVTable {
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance);
uint8_t (*serialRead)(serialPort_t *instance);
// Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
};
inline void serialWrite(serialPort_t *instance, uint8_t ch);
inline uint8_t serialTotalBytesWaiting(serialPort_t *instance);
inline uint8_t serialRead(serialPort_t *instance);
inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
inline bool isSerialTransmitBufferEmpty(serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance);

346
src/drv_softserial.c
View file

@ -1,12 +1,34 @@
#include "board.h"
enum serialBitStatus {
WAITING_FOR_START_BIT = -1, BIT_0, BIT_1, BIT_2, BIT_3, BIT_4, BIT_5, BIT_6, BIT_7, WAITING_FOR_STOP_BIT
};
// There needs to be a table of timerMhz per baud rate so the system can set the timer correctly.
// See http://www.wormfood.net/avrbaudcalc.php?postbitrate=19200&postclock=72
// Currently defaulting to 3Mhz to support 19200.
#define SOFT_SERIAL_TIMER_MHZ 3
#define SOFT_SERIAL_1_TIMER_RX_HARDWARE 4
#define SOFT_SERIAL_1_TIMER_TX_HARDWARE 5
#define RX_TOTAL_BITS 8
#define TX_TOTAL_BITS 10
#define MAX_SOFTSERIAL_PORTS 2
softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
void onSerialTimer(uint8_t portIndex, uint16_t capture);
uint8_t readRxSignal(softSerial_t *softSerial)
{
return GPIO_ReadInputDataBit(softSerial->rxTimerHardware->gpio, softSerial->rxTimerHardware->pin);
}
void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if (state) {
digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
} else {
digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
}
}
softSerial_t* lookupSoftSerial(uint8_t reference)
{
assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS);
@ -14,50 +36,87 @@ softSerial_t* lookupSoftSerial(uint8_t reference)
return &(softSerialPorts[reference]);
}
void resetSerialTimer(softSerial_t *softSerial)
{
//uint16_t counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
TIM_SetCounter(softSerial->rxTimerHardware->tim, 0);
//counter = TIM_GetCounter(softSerial->rxTimerHardware->tim);
}
void stopSerialTimer(softSerial_t *softSerial)
{
TIM_Cmd(softSerial->timerHardware->tim, DISABLE);
TIM_SetCounter(softSerial->timerHardware->tim, 0);
TIM_Cmd(softSerial->rxTimerHardware->tim, DISABLE);
}
void startSerialTimer(softSerial_t *softSerial)
{
TIM_SetCounter(softSerial->timerHardware->tim, 0);
TIM_Cmd(softSerial->timerHardware->tim, ENABLE);
TIM_Cmd(softSerial->rxTimerHardware->tim, ENABLE);
}
static void waitForFirstBit(softSerial_t *softSerial)
static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{
softSerial->state = BIT_0;
startSerialTimer(softSerial);
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
static void onSerialPinChange(uint8_t reference, uint16_t capture)
void serialInputPortConfig(const timerHardware_t *timerHardwarePtr)
{
softSerial_t *softSerial = lookupSoftSerial(reference);
if (softSerial->state == WAITING_FOR_START_BIT) {
waitForFirstBit(softSerial);
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
}
}
uint8_t readSerialSignal(softSerial_t *softSerial)
#define TICKS_PER_BIT 3
void serialTimerConfig(const timerHardware_t *timerHardwarePtr, uint32_t baud, uint8_t reference, timerCCCallbackPtr callback)
{
return GPIO_ReadInputDataBit(softSerial->timerHardware->gpio, softSerial->timerHardware->pin);
uint16_t timerPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / (baud * TICKS_PER_BIT);
timerInConfig(timerHardwarePtr, timerPeriod, SOFT_SERIAL_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
}
void mergeSignalWithCurrentByte(softSerial_t *softSerial, uint8_t serialSignal)
void serialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
{
softSerial->rxBuffer[softSerial->port.rxBufferTail] |= (serialSignal << softSerial->state);
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_Out_PP);
}
inline void initialiseCurrentByteWithFirstSignal(softSerial_t *softSerial, uint8_t serialSignal)
void setupSoftSerial1(uint32_t baud)
{
softSerial->rxBuffer[softSerial->port.rxBufferTail] = serialSignal;
}
int portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
inline void prepareForNextSignal(softSerial_t *softSerial) {
softSerial->state++;
softSerial->port.vTable = softSerialVTable;
softSerial->port.mode = MODE_RXTX;
softSerial->port.baudRate = baud;
softSerial->rxTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_RX_HARDWARE]);
softSerial->txTimerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_TX_HARDWARE]);
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = softSerial->txBuffer;
softSerial->port.txBufferSize = SOFT_SERIAL_BUFFER_SIZE,
softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0;
softSerial->isTransmittingData = false;
softSerial->isSearchingForStartBit = true;
softSerial->isSearchingForStopBit = false;
softSerial->timerRxCounter = 1;
serialInputPortConfig(softSerial->rxTimerHardware);
serialOutputPortConfig(softSerial->txTimerHardware);
setTxSignal(softSerial, 1);
delay(50);
serialTimerConfig(softSerial->rxTimerHardware, baud, portIndex, onSerialTimer);
}
void updateBufferIndex(softSerial_t *softSerial)
@ -70,104 +129,135 @@ void updateBufferIndex(softSerial_t *softSerial)
}
}
void prepareForNextByte(softSerial_t *softSerial)
/*********************************************/
void searchForStartBit(softSerial_t *softSerial)
{
stopSerialTimer(softSerial);
softSerial->state = WAITING_FOR_START_BIT;
char rxSignal = readRxSignal(softSerial);
if (rxSignal == 1) {
// start bit not found
softSerial->timerRxCounter = 1; // process on next timer event
return;
}
// timer is aligned to falling signal of start bit.
// three ticks per bit.
softSerial->isSearchingForStartBit = false;
softSerial->internalRxBuffer = 0;
softSerial->timerRxCounter = TICKS_PER_BIT + 1; // align to middle of next bit
softSerial->bitsLeftToReceive = RX_TOTAL_BITS;
softSerial->rxBitSelectionMask = 1;
}
void searchForStopBit(softSerial_t *softSerial)
{
softSerial->timerRxCounter = 1;
char rxSignal = readRxSignal(softSerial);
if (rxSignal != 1) {
// not found
return;
}
softSerial->isSearchingForStopBit = false;
softSerial->isSearchingForStartBit = true;
softSerial->internalRxBuffer &= 0xFF;
softSerial->port.rxBuffer[softSerial->port.rxBufferTail] = softSerial->internalRxBuffer;
updateBufferIndex(softSerial);
}
void readDataBit(softSerial_t *softSerial)
{
softSerial->timerRxCounter = TICKS_PER_BIT; // keep aligned to middle of bit
char rxSignal = readRxSignal(softSerial);
if (rxSignal) {
softSerial->internalRxBuffer |= softSerial->rxBitSelectionMask;
}
softSerial->rxBitSelectionMask <<= 1;
if (--softSerial->bitsLeftToReceive <= 0) {
softSerial->isSearchingForStopBit = true;
softSerial->timerRxCounter = 2;
}
}
void processRxState(softSerial_t *softSerial)
{
//digitalToggle(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
if (--softSerial->timerRxCounter > 0) {
return;
}
if (softSerial->isSearchingForStartBit) {
searchForStartBit(softSerial);
return;
}
if (softSerial->isSearchingForStopBit) {
searchForStopBit(softSerial);
return;
}
readDataBit(softSerial);
}
void processTxState(softSerial_t *softSerial)
{
char mask;
if (!softSerial->isTransmittingData) {
if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
return;
}
// data to send
char byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
softSerial->port.txBufferTail = 0;
}
// build internal buffer, start bit(1) + data bits + stop bit(0)
softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
// start immediately
softSerial->timerTxCounter = 1;
softSerial->isTransmittingData = true;
return;
}
if (--softSerial->timerTxCounter <= 0) {
mask = softSerial->internalTxBuffer & 1;
softSerial->internalTxBuffer >>= 1;
setTxSignal(softSerial, mask);
softSerial->timerTxCounter = TICKS_PER_BIT;
if (--softSerial->bitsLeftToTransmit <= 0) {
softSerial->isTransmittingData = false;
}
}
}
void onSerialTimer(uint8_t portIndex, uint16_t capture)
{
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
uint8_t serialSignal = readSerialSignal(softSerial);
switch (softSerial->state) {
case BIT_0:
initialiseCurrentByteWithFirstSignal(softSerial, serialSignal);
prepareForNextSignal(softSerial);
break;
case BIT_1:
case BIT_2:
case BIT_3:
case BIT_4:
case BIT_5:
case BIT_6:
case BIT_7:
mergeSignalWithCurrentByte(softSerial, serialSignal);
prepareForNextSignal(softSerial);
break;
case WAITING_FOR_STOP_BIT:
prepareForNextByte(softSerial);
break;
}
processTxState(softSerial);
processRxState(softSerial);
}
#define SOFT_SERIAL_TIMER_MHZ 1
#define SOFT_SERIAL_1_TIMER_HARDWARE 4
static void softSerialGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
{
gpio_config_t cfg;
cfg.pin = pin;
cfg.mode = mode;
cfg.speed = Speed_2MHz;
gpioInit(gpio, &cfg);
}
void serialInputPortConfig(const timerHardware_t *timerHardwarePtr, uint16_t baud, uint8_t reference, timerCCCallbackPtr callback)
{
softSerialGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, Mode_IPU);
pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
int oneBitPeriod = (SOFT_SERIAL_TIMER_MHZ * 1000000) / baud;
timerInConfig(timerHardwarePtr, oneBitPeriod, SOFT_SERIAL_TIMER_MHZ);
configureTimerCaptureCompareInterrupt(timerHardwarePtr, reference, callback);
}
void setupSoftSerial1(uint32_t baud)
{
int portIndex = 0;
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
softSerial->timerHardware = &(timerHardware[SOFT_SERIAL_1_TIMER_HARDWARE]);
softSerial->timerIndex = SOFT_SERIAL_1_TIMER_HARDWARE;
softSerial->state = WAITING_FOR_START_BIT;
softSerial->port.rxBufferSize = SOFT_SERIAL_BUFFER_SIZE;
softSerial->port.rxBuffer = softSerial->rxBuffer;
softSerial->port.rxBufferTail = 0;
softSerial->port.rxBufferHead = 0;
softSerial->port.txBuffer = 0;
softSerial->port.txBufferSize = 0;
softSerial->port.txBufferTail = 0;
softSerial->port.txBufferHead = 0;
softSerial->port.baudRate = baud;
softSerial->port.mode = MODE_RX;
// FIXME this uart specific initialisation doesn't belong really here
softSerial->port.txDMAChannel = 0;
softSerial->port.txDMAChannel = 0;
configureTimerChannelCallback(softSerial->timerHardware->tim, TIM_Channel_2, portIndex, onSerialTimer);
TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
stopSerialTimer(softSerial);
serialInputPortConfig(softSerial->timerHardware, baud, portIndex, onSerialPinChange);
}
bool serialAvailable(softSerial_t *softSerial)
uint8_t softSerialTotalBytesWaiting(serialPort_t *instance)
{
softSerial_t *softSerial = (softSerial_t *)instance;
if (softSerial->port.rxBufferTail == softSerial->port.rxBufferHead) {
return 0;
}
@ -190,14 +280,42 @@ static void moveHeadToNextByte(softSerial_t *softSerial)
}
}
uint8_t serialReadByte(softSerial_t *softSerial)
uint8_t softSerialReadByte(serialPort_t *instance)
{
if (serialAvailable(softSerial) == 0) {
if (softSerialTotalBytesWaiting(instance) == 0) {
return 0;
}
char b = softSerial->port.rxBuffer[softSerial->port.rxBufferHead];
char b = instance->rxBuffer[instance->rxBufferHead];
moveHeadToNextByte(softSerial);
moveHeadToNextByte((softSerial_t *)instance);
return b;
}
void softSerialWriteByte(serialPort_t *s, uint8_t ch)
{
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
}
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
{
// not implemented.
}
bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance)
{
return instance->txBufferHead == instance->txBufferTail;
}
const struct serialPortVTable softSerialVTable[] = {
{
softSerialWriteByte,
softSerialTotalBytesWaiting,
softSerialReadByte,
softSerialSetBaudRate,
isSoftSerialTransmitBufferEmpty
}
};

38
src/drv_softserial.h
View file

@ -10,16 +10,38 @@
#define SOFT_SERIAL_BUFFER_SIZE 256
typedef struct softSerial_s {
const timerHardware_t *timerHardware;
uint8_t timerIndex;
serialPort_t port;
volatile int state;
volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
} softSerial_t;
void setupSoftSerial1(uint32_t baud);
uint8_t serialReadByte(softSerial_t *softSerial);
bool serialAvailable(softSerial_t *softSerial);
const timerHardware_t *rxTimerHardware;
volatile uint8_t rxBuffer[SOFT_SERIAL_BUFFER_SIZE];
const timerHardware_t *txTimerHardware;
volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE];
uint8_t isSearchingForStopBit;
uint8_t rxBitSelectionMask;
uint8_t isSearchingForStartBit;
uint8_t isTransmittingData;
uint8_t timerRxCounter;
uint8_t timerTxCounter;
uint8_t bitsLeftToReceive;
uint8_t bitsLeftToTransmit;
uint16_t internalRxBuffer; // excluding start/stop bits
uint16_t internalTxBuffer; // includes start and stop bits
} softSerial_t;
extern timerHardware_t* serialTimerHardware;
extern softSerial_t softSerialPorts[];
extern const struct serialPortVTable softSerialVTable[];
void setupSoftSerial1(uint32_t baud);
// serialPort API
void softSerialWriteByte(serialPort_t *instance, uint8_t ch);
uint8_t softSerialTotalBytesWaiting(serialPort_t *instance);
uint8_t softSerialReadByte(serialPort_t *instance);
void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isSoftSerialTransmitBufferEmpty(serialPort_t *s);

47
src/drv_system.c
View file

@ -182,3 +182,50 @@ void systemReset(bool toBootloader)
// Generate system reset
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
{
switch (rotation) {
case CW0_DEG:
dest[X] = src[X];
dest[Y] = src[Y];
dest[Z] = src[Z];
break;
case CW90_DEG:
dest[X] = src[Y];
dest[Y] = -src[X];
dest[Z] = src[Z];
break;
case CW180_DEG:
dest[X] = -src[X];
dest[Y] = -src[Y];
dest[Z] = src[Z];
break;
case CW270_DEG:
dest[X] = -src[Y];
dest[Y] = src[X];
dest[Z] = src[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -src[X];
dest[Y] = src[Y];
dest[Z] = -src[Z];
break;
case CW90_DEG_FLIP:
dest[X] = src[Y];
dest[Y] = src[X];
dest[Z] = -src[Z];
break;
case CW180_DEG_FLIP:
dest[X] = src[X];
dest[Y] = -src[Y];
dest[Z] = -src[Z];
break;
case CW270_DEG_FLIP:
dest[X] = -src[Y];
dest[Y] = -src[X];
dest[Z] = -src[Z];
break;
default:
break;
}
}

3
src/drv_system.h
View file

@ -15,3 +15,6 @@ void systemReset(bool toBootloader);
// current crystal frequency - 8 or 12MHz
extern uint32_t hse_value;
// sensor orientation
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);

161
src/drv_uart.c
View file

@ -4,23 +4,28 @@
Copyright © 2011 Bill Nesbitt
*/
static serialPort_t serialPort1;
static serialPort_t serialPort2;
static uartPort_t uartPort1;
static uartPort_t uartPort2;
// USART1 - Telemetry (RX/TX by DMA)
serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
{
serialPort_t *s;
uartPort_t *s;
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
static volatile uint8_t tx1Buffer[UART1_TX_BUFFER_SIZE];
gpio_config_t gpio;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort1;
s->rxBufferSize = UART1_RX_BUFFER_SIZE;
s->txBufferSize = UART1_TX_BUFFER_SIZE;
s->rxBuffer = rx1Buffer;
s->txBuffer = tx1Buffer;
s = &uartPort1;
s->port.vTable = uartVTable;
s->port.baudRate = baudRate;
s->port.rxBuffer = rx1Buffer;
s->port.txBuffer = tx1Buffer;
s->port.rxBufferSize = UART1_RX_BUFFER_SIZE;
s->port.txBufferSize = UART1_TX_BUFFER_SIZE;
s->rxDMAChannel = DMA1_Channel5;
s->txDMAChannel = DMA1_Channel4;
@ -48,20 +53,24 @@ serialPort_t *serialUSART1(uint32_t baudRate, portmode_t mode)
}
// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
{
serialPort_t *s;
uartPort_t *s;
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
static volatile uint8_t tx2Buffer[UART2_TX_BUFFER_SIZE];
gpio_config_t gpio;
NVIC_InitTypeDef NVIC_InitStructure;
s = &serialPort2;
s->baudRate = baudRate;
s->rxBufferSize = UART2_RX_BUFFER_SIZE;
s->txBufferSize = UART2_TX_BUFFER_SIZE;
s->rxBuffer = rx2Buffer;
s->txBuffer = tx2Buffer;
s = &uartPort2;
s->port.vTable = uartVTable;
s->port.baudRate = baudRate;
s->port.rxBufferSize = UART2_RX_BUFFER_SIZE;
s->port.txBufferSize = UART2_TX_BUFFER_SIZE;
s->port.rxBuffer = rx2Buffer;
s->port.txBuffer = tx2Buffer;
s->USARTx = USART2;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
@ -87,11 +96,12 @@ serialPort_t *serialUSART2(uint32_t baudRate, portmode_t mode)
return s;
}
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode)
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode)
{
DMA_InitTypeDef DMA_InitStructure;
USART_InitTypeDef USART_InitStructure;
serialPort_t *s = NULL;
uartPort_t *s = NULL;
if (USARTx == USART1)
s = serialUSART1(baudRate, mode);
@ -99,11 +109,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
s = serialUSART2(baudRate, mode);
s->USARTx = USARTx;
s->rxBufferHead = s->rxBufferTail = 0;
s->txBufferHead = s->txBufferTail = 0;
// common serial initialisation code should move to serialPort::init()
s->port.rxBufferHead = s->port.rxBufferTail = 0;
s->port.txBufferHead = s->port.txBufferTail = 0;
// callback for IRQ-based RX ONLY
s->callback = callback;
s->mode = mode;
s->port.callback = callback;
s->port.mode = mode;
s->port.baudRate = baudRate;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
@ -130,10 +143,10 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
// Receive DMA or IRQ
if (mode & MODE_RX) {
if (s->rxDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->rxBufferSize;
DMA_InitStructure.DMA_BufferSize = s->port.rxBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->rxBuffer;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)s->port.rxBuffer;
DMA_DeInit(s->rxDMAChannel);
DMA_Init(s->rxDMAChannel, &DMA_InitStructure);
DMA_Cmd(s->rxDMAChannel, ENABLE);
@ -147,7 +160,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
// Transmit DMA or IRQ
if (mode & MODE_TX) {
if (s->txDMAChannel) {
DMA_InitStructure.DMA_BufferSize = s->txBufferSize;
DMA_InitStructure.DMA_BufferSize = s->port.txBufferSize;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_DeInit(s->txDMAChannel);
@ -161,12 +174,13 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, u
}
}
return s;
return (serialPort_t *)s;
}
void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
USART_InitTypeDef USART_InitStructure;
uartPort_t *s = (uartPort_t *)instance;
USART_InitStructure.USART_BaudRate = baudRate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
@ -174,64 +188,71 @@ void uartChangeBaud(serialPort_t *s, uint32_t baudRate)
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = 0;
if (s->mode & MODE_RX)
if (s->port.mode & MODE_RX)
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
if (s->mode & MODE_TX)
if (s->port.mode & MODE_TX)
USART_InitStructure.USART_Mode |= USART_Mode_Tx;
USART_Init(s->USARTx, &USART_InitStructure);
s->port.baudRate = baudRate;
}
static void uartStartTxDMA(serialPort_t *s)
static void uartStartTxDMA(uartPort_t *s)
{
s->txDMAChannel->CMAR = (uint32_t)&s->txBuffer[s->txBufferTail];
if (s->txBufferHead > s->txBufferTail) {
s->txDMAChannel->CNDTR = s->txBufferHead - s->txBufferTail;
s->txBufferTail = s->txBufferHead;
s->txDMAChannel->CMAR = (uint32_t)&s->port.txBuffer[s->port.txBufferTail];
if (s->port.txBufferHead > s->port.txBufferTail) {
s->txDMAChannel->CNDTR = s->port.txBufferHead - s->port.txBufferTail;
s->port.txBufferTail = s->port.txBufferHead;
} else {
s->txDMAChannel->CNDTR = s->txBufferSize - s->txBufferTail;
s->txBufferTail = 0;
s->txDMAChannel->CNDTR = s->port.txBufferSize - s->port.txBufferTail;
s->port.txBufferTail = 0;
}
s->txDMAEmpty = false;
DMA_Cmd(s->txDMAChannel, ENABLE);
}
bool isUartAvailable(serialPort_t *s)
uint8_t uartTotalBytesWaiting(serialPort_t *instance)
{
uartPort_t *s = (uartPort_t*)instance;
// FIXME always returns 1 or 0, not the amount of bytes waiting
if (s->rxDMAChannel)
return s->rxDMAChannel->CNDTR != s->rxDMAPos;
else
return s->rxBufferTail != s->rxBufferHead;
return s->port.rxBufferTail != s->port.rxBufferHead;
}
// BUGBUG TODO TODO FIXME
bool isUartTransmitEmpty(serialPort_t *s)
// BUGBUG TODO TODO FIXME - What is the bug?
bool isUartTransmitBufferEmpty(serialPort_t *instance)
{
uartPort_t *s = (uartPort_t *)instance;
if (s->txDMAChannel)
return s->txDMAEmpty;
else
return s->txBufferTail == s->txBufferHead;
return s->port.txBufferTail == s->port.txBufferHead;
}
uint8_t uartRead(serialPort_t *s)
uint8_t uartRead(serialPort_t *instance)
{
uint8_t ch;
uartPort_t *s = (uartPort_t *)instance;
if (s->rxDMAChannel) {
ch = s->rxBuffer[s->rxBufferSize - s->rxDMAPos];
ch = s->port.rxBuffer[s->port.rxBufferSize - s->rxDMAPos];
if (--s->rxDMAPos == 0)
s->rxDMAPos = s->rxBufferSize;
s->rxDMAPos = s->port.rxBufferSize;
} else {
ch = s->rxBuffer[s->rxBufferTail];
s->rxBufferTail = (s->rxBufferTail + 1) % s->rxBufferSize;
ch = s->port.rxBuffer[s->port.rxBufferTail];
s->port.rxBufferTail = (s->port.rxBufferTail + 1) % s->port.rxBufferSize;
}
return ch;
}
void uartWrite(serialPort_t *s, uint8_t ch)
void uartWrite(serialPort_t *instance, uint8_t ch)
{
s->txBuffer[s->txBufferHead] = ch;
s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
uartPort_t *s = (uartPort_t *)instance;
s->port.txBuffer[s->port.txBufferHead] = ch;
s->port.txBufferHead = (s->port.txBufferHead + 1) % s->port.txBufferSize;
if (s->txDMAChannel) {
if (!(s->txDMAChannel->CCR & 1))
@ -241,24 +262,26 @@ void uartWrite(serialPort_t *s, uint8_t ch)
}
}
void uartPrint(serialPort_t *s, const char *str)
const struct serialPortVTable uartVTable[] = {
{
uint8_t ch;
while ((ch = *(str++))) {
uartWrite(s, ch);
uartWrite,
uartTotalBytesWaiting,
uartRead,
uartSetBaudRate,
isUartTransmitBufferEmpty
}
}
};
// Handlers
// USART1 Tx DMA Handler
void DMA1_Channel4_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
uartPort_t *s = &uartPort1;
DMA_ClearITPendingBit(DMA1_IT_TC4);
DMA_Cmd(s->txDMAChannel, DISABLE);
if (s->txBufferHead != s->txBufferTail)
if (s->port.txBufferHead != s->port.txBufferTail)
uartStartTxDMA(s);
else
s->txDMAEmpty = true;
@ -267,13 +290,13 @@ void DMA1_Channel4_IRQHandler(void)
// USART1 Tx IRQ Handler
void USART1_IRQHandler(void)
{
serialPort_t *s = &serialPort1;
uartPort_t *s = &uartPort1;
uint16_t SR = s->USARTx->SR;
if (SR & USART_FLAG_TXE) {
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
if (s->port.txBufferTail != s->port.txBufferHead) {
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}
@ -283,22 +306,22 @@ void USART1_IRQHandler(void)
// USART2 Rx/Tx IRQ Handler
void USART2_IRQHandler(void)
{
serialPort_t *s = &serialPort2;
uartPort_t *s = &uartPort2;
uint16_t SR = s->USARTx->SR;
if (SR & USART_FLAG_RXNE) {
// If we registered a callback, pass crap there
if (s->callback) {
s->callback(s->USARTx->DR);
if (s->port.callback) {
s->port.callback(s->USARTx->DR);
} else {
s->rxBuffer[s->rxBufferHead] = s->USARTx->DR;
s->rxBufferHead = (s->rxBufferHead + 1) % s->rxBufferSize;
s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR;
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
}
}
if (SR & USART_FLAG_TXE) {
if (s->txBufferTail != s->txBufferHead) {
s->USARTx->DR = s->txBuffer[s->txBufferTail];
s->txBufferTail = (s->txBufferTail + 1) % s->txBufferSize;
if (s->port.txBufferTail != s->port.txBufferHead) {
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
} else {
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
}

40
src/drv_uart.h
View file

@ -8,29 +8,10 @@
#define UART2_TX_BUFFER_SIZE 64
#define MAX_SERIAL_PORTS 2
// This is a bitmask
typedef enum portmode_t {
MODE_RX = 1,
MODE_TX = 2,
MODE_RXTX = 3
} portmode_t;
// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
typedef struct {
portmode_t mode;
uint32_t baudRate;
uint32_t rxBufferSize;
uint32_t txBufferSize;
volatile uint8_t *rxBuffer;
volatile uint8_t *txBuffer;
uint32_t rxBufferHead;
uint32_t rxBufferTail;
uint32_t txBufferHead;
uint32_t txBufferTail;
// FIXME rename callback type so something more generic
uartReceiveCallbackPtr callback;
serialPort_t port;
// FIXME these are uart specific and do not belong in here
DMA_Channel_TypeDef *rxDMAChannel;
@ -43,12 +24,15 @@ typedef struct {
bool txDMAEmpty;
USART_TypeDef *USARTx;
} serialPort_t;
} uartPort_t;
serialPort_t *uartOpen(USART_TypeDef *USARTx, uartReceiveCallbackPtr callback, uint32_t baudRate, portmode_t mode);
void uartChangeBaud(serialPort_t *s, uint32_t baudRate);
bool isUartAvailable(serialPort_t *s);
bool isUartTransmitEmpty(serialPort_t *s);
uint8_t uartRead(serialPort_t *s);
void uartWrite(serialPort_t *s, uint8_t ch);
void uartPrint(serialPort_t *s, const char *str);
extern const struct serialPortVTable uartVTable[];
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode);
// serialPort API
void uartWrite(serialPort_t *instance, uint8_t ch);
uint8_t uartTotalBytesWaiting(serialPort_t *instance);
uint8_t uartRead(serialPort_t *instance);
void uartSetBaudRate(serialPort_t *s, uint32_t baudRate);
bool isUartTransmitBufferEmpty(serialPort_t *s);

19
src/gps.c
View file

@ -10,6 +10,9 @@ const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
static void GPS_NewData(uint16_t c);
static void gpsPrint(const char *str);
#define UBX_INIT_STRING_INDEX 0
#define MTK_INIT_STRING_INDEX 4
static const char * const gpsInitStrings[] = {
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3
"$PUBX,41,1,0003,0001,38400,0*26\r\n",
@ -45,13 +48,15 @@ void gpsInit(uint32_t baudrate)
core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX);
if (mcfg.gps_type == GPS_UBLOX)
offset = 0;
offset = UBX_INIT_STRING_INDEX;
else if (mcfg.gps_type == GPS_MTK)
offset = 4;
offset = MTK_INIT_STRING_INDEX;
if (mcfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
uartChangeBaud(core.gpsport, init_speed[i]);
serialSetBaudRate(core.gpsport, init_speed[i]);
// verify the requested change took effect.
baudrate = serialGetBaudRate(core.gpsport);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
@ -70,10 +75,10 @@ void gpsInit(uint32_t baudrate)
}
}
uartChangeBaud(core.gpsport, baudrate);
serialSetBaudRate(core.gpsport, baudrate);
if (mcfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
uartWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
serialWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
delay(4);
}
} else if (mcfg.gps_type == GPS_MTK) {
@ -90,13 +95,13 @@ void gpsInit(uint32_t baudrate)
static void gpsPrint(const char *str)
{
while (*str) {
uartWrite(core.gpsport, *str);
serialWrite(core.gpsport, *str);
if (mcfg.gps_type == GPS_UBLOX)
delay(4);
str++;
}
// wait to send all
while (!isUartTransmitEmpty(core.gpsport));
while (!isSerialTransmitBufferEmpty(core.gpsport));
delay(30);
}

201
src/imu.c
View file

@ -5,10 +5,10 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int32_t accSum[3];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
int16_t acc_25deg = 0;
int16_t accZ_25deg = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroPressureSum = 0;
uint32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
int32_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
@ -26,12 +26,13 @@ float accVelScale;
int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0, 0 }; // absolute angle inclination in radians
static void getEstimatedAttitude(void);
void imuInit(void)
{
acc_25deg = acc_1G * 0.423f;
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
accVelScale = 9.80665f / acc_1G / 10000.0f;
#ifdef MAG
@ -41,42 +42,19 @@ void imuInit(void)
#endif
}
void computeIMU(void)
{
uint32_t axis;
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
Gyro_getADC();
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave = micros();
annexCode();
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
if (feature(FEATURE_GYRO_SMOOTHING)) {
@ -95,6 +73,9 @@ void computeIMU(void)
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
} else {
for (axis = 0; axis < 3; axis++)
gyroData[axis] = gyroADC[axis];
}
}
@ -106,24 +87,15 @@ void computeIMU(void)
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Modified: 19/04/2011 by ziss_dm
// Version: V1.1
//
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
// **************************************************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
typedef struct fp_vector {
float X;
float Y;
@ -137,6 +109,19 @@ typedef union {
t_fp_vector EstG;
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
@ -147,10 +132,10 @@ void rotateV(struct fp_vector *v, float *delta)
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(-delta[PITCH]);
sinx = sinf(-delta[PITCH]);
cosy = cosf(delta[ROLL]);
siny = sinf(delta[ROLL]);
cosx = cosf(delta[ROLL]);
sinx = sinf(delta[ROLL]);
cosy = cosf(delta[PITCH]);
siny = sinf(delta[PITCH]);
cosz = cosf(delta[YAW]);
sinz = sinf(delta[YAW]);
@ -161,13 +146,13 @@ void rotateV(struct fp_vector *v, float *delta)
sinzsinx = sinx * sinz;
mat[0][0] = coszcosy;
mat[0][1] = sinz * cosy;
mat[0][2] = -siny;
mat[1][0] = (coszsinx * siny) - sinzcosx;
mat[1][1] = (sinzsinx * siny) + (coszcosx);
mat[1][2] = cosy * sinx;
mat[2][0] = (coszcosx * siny) + (sinzsinx);
mat[2][1] = (sinzcosx * siny) - (coszsinx);
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
@ -175,12 +160,6 @@ void rotateV(struct fp_vector *v, float *delta)
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}
static int16_t _atan2f(float y, float x)
{
// no need for aidsy inaccurate shortcuts on a proper platform
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
int32_t applyDeadband(int32_t value, int32_t deadband)
{
if (abs(value) < deadband) {
@ -201,9 +180,9 @@ void acc_calc(uint32_t deltaT)
t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float) angle[ROLL] * RADX10;
rpy[1] = -(float) angle[PITCH] * RADX10;
rpy[2] = -(float) heading * RADX10 * 10;
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RADX10 * 10.0f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
@ -243,10 +222,6 @@ void accSum_reset(void)
accTimeSum = 0;
}
// Use original baseflight angle calculation
// #define BASEFLIGHT_CALC
static float invG;
static void getEstimatedAttitude(void)
{
uint32_t axis;
@ -257,13 +232,6 @@ static void getEstimatedAttitude(void)
uint32_t currentT = micros();
uint32_t deltaT;
float scale, deltaGyroAngle[3];
#ifndef BASEFLIGHT_CALC
float sqGZ;
float sqGX;
float sqGY;
float sqGX_sqGZ;
float invmagXZ;
#endif
deltaT = currentT - previousT;
scale = deltaT * gyro.scale;
previousT = currentT;
@ -285,11 +253,6 @@ static void getEstimatedAttitude(void)
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
@ -303,50 +266,30 @@ static void getEstimatedAttitude(void)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
}
if (abs(EstG.A[Z]) > accZ_25deg)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
// Attitude of the estimated vector
#ifdef BASEFLIGHT_CALC
// This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
#else
// MW2.2 version
sqGZ = EstG.V.Z * EstG.V.Z;
sqGX = EstG.V.X * EstG.V.X;
sqGY = EstG.V.Y * EstG.V.Y;
sqGX_sqGZ = sqGX + sqGZ;
invmagXZ = 1.0f / sqrtf(sqGX_sqGZ);
invG = 1.0f / sqrtf(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, invmagXZ * sqGX_sqGZ);
#endif
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));
#ifdef MAG
if (sensors(SENSOR_MAG)) {
#ifdef BASEFLIGHT_CALC
// baseflight calculation
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = -(float)angle[PITCH] * RADX10;
float magX = EstM.A[1]; // Swap X/Y
float magY = EstM.A[0]; // Swap X/Y
float magZ = EstM.A[2];
float cr = cosf(rollRAD);
float sr = sinf(rollRAD);
float cp = cosf(pitchRAD);
float sp = sinf(pitchRAD);
float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
float Yh = magY * cr - magZ * sr;
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = hd;
#else
// MW 2.2 calculation
heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * sqGX_sqGZ - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
heading = heading + magneticDeclination;
heading = heading / 10;
#endif
if (heading > 180)
heading = heading - 360;
else if (heading < -180)
heading = heading + 360;
// baseflight calculation by Luggi09 originates from arducopter
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll;
float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = lrintf(hd);
if (heading < 0)
heading += 360;
}
#endif
@ -372,6 +315,9 @@ int getEstimatedAltitude(void)
int32_t baroVel;
int32_t vel_tmp;
int32_t BaroAlt_tmp;
float dt;
float PressureScaling;
float vel_acc;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
@ -388,19 +334,22 @@ int getEstimatedAltitude(void)
accAlt = 0;
}
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt_tmp = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
BaroAlt = BaroAlt * cfg.baro_noise_lpf + BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
PressureScaling = (float)baroPressureSum / ((float)baroGroundPressure * (float)(cfg.baro_tab_size - 1));
BaroAlt_tmp = 153.8462f * (baroTemperature + 27315) * (1.0f - expf(0.190259f * logf(PressureScaling))); // in cm
BaroAlt = (float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
dt = accTimeSum * 1e-6; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
vel += (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
// Integrator - Altitude in cm
accAlt += vel * ((float) accTimeSum * 0.0000005f); // integrate velocity to get distance (x= a/2 * t^2)
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt;
vel += vel_acc;
#if 0
debug[0] = accSum[2] / accSumCount; // acceleration
@ -413,13 +362,12 @@ int getEstimatedAltitude(void)
//P
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150);
BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -200, +200);
//I
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI / 512; // I in range +/-60
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
BaroPID += errorAltitudeI / 512; // I in range +/-100
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
@ -430,6 +378,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D
vel_tmp = vel;

46
src/main.c
View file

@ -5,23 +5,22 @@ core_t core;
extern rcReadRawDataPtr rcReadRawFunc;
// two receiver read functions
// receiver read function
extern uint16_t pwmReadRawRC(uint8_t chan);
extern uint16_t spektrumReadRawRC(uint8_t chan);
#ifdef USE_LAME_PRINTF
// gcc/GNU version
static void _putc(void *p, char c)
{
uartWrite(core.mainport, c);
serialWrite(core.mainport, c);
}
#else
// keil/armcc version
int fputc(int c, FILE *f)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!isUartTransmitEmpty(core.mainport));
uartWrite(core.mainport, c);
while (!isSerialTransmitBufferEmpty(core.mainport));
serialWrite(core.mainport, c);
return c;
}
#endif
@ -59,9 +58,9 @@ int main(void)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
pwm_params.useServos = core.useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
@ -81,12 +80,20 @@ int main(void)
pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum below will override that
// configure PWM/CPPM read function. spektrum or sbus below will override that
rcReadRawFunc = pwmReadRawRC;
if (feature(FEATURE_SPEKTRUM)) {
spektrumInit();
rcReadRawFunc = spektrumReadRawRC;
if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
spektrumInit(&rcReadRawFunc);
break;
case SERIALRX_SBUS:
sbusInit(&rcReadRawFunc);
break;
}
} else {
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
@ -122,13 +129,11 @@ int main(void)
if (feature(FEATURE_VBAT))
batteryInit();
#ifdef SOFTSERIAL_19200_LOOPBACK
serialInit(19200);
setupSoftSerial1(19200);
uartPrint(core.mainport, "LOOPBACK 19200 ENABLED");
#else
serialInit(mcfg.serial_baudrate);
#ifdef SOFTSERIAL_19200_LOOPBACK
setupSoftSerial1(19200);
serialPort_t* loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
serialPrint(loopbackPort, "LOOPBACK 19200 ENABLED\r\n");
#endif
previousTime = micros();
@ -142,10 +147,11 @@ int main(void)
while (1) {
loop();
#ifdef SOFTSERIAL_19200_LOOPBACK
while (serialAvailable(&softSerialPorts[0])) {
while (serialTotalBytesWaiting(loopbackPort)) {
uint8_t b = serialReadByte(&softSerialPorts[0]);
uartWrite(core.mainport, b);
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
//serialWrite(core.mainport, b);
};
#endif
}

17
src/mixer.c
View file

@ -206,7 +206,16 @@ void writeServos(void)
break;
case MULTITYPE_TRI:
if (cfg.tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(0, servo[5]);
} else {
// otherwise, only move servo when copter is armed
if (f.ARMED)
pwmWriteServo(0, servo[5]);
else
pwmWriteServo(0, 0); // kill servo signal completely.
}
break;
case MULTITYPE_AIRPLANE:
@ -312,17 +321,17 @@ void mixTable(void)
// motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
servo[5] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break;
case MULTITYPE_GIMBAL:

43
src/mw.c
View file

@ -11,7 +11,6 @@ uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t headFreeModeHold;
int16_t annex650_overrun_count = 0;
uint8_t vbat; // battery voltage in 0.1V steps
int16_t telemTemperature1; // gyro sensor temperature
@ -85,7 +84,6 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
#define BREAKPOINT 1500
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
@ -125,6 +123,7 @@ void annexCode(void)
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
tmp *= -mcfg.yaw_control_direction; //change control direction for yaw needed with new gyro orientation
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
@ -147,7 +146,7 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if(f.HEADFREE_MODE) {
if (f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
@ -437,14 +436,26 @@ void loop(void)
static uint8_t GPSNavReset = 1;
bool isThrottleLow = false;
// this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete())
// calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_SERIALRX)) {
bool ready = false;
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
ready = spektrumFrameComplete();
break;
case SERIALRX_SBUS:
ready = sbusFrameComplete();
break;
}
if (ready)
computeRC();
}
if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
rcTime = currentTime + 20000;
// TODO clean this up. computeRC should handle this check
if (!feature(FEATURE_SPEKTRUM))
if (!feature(FEATURE_SERIALRX))
computeRC();
// in 3D mode, we need to be able to disarm by switch at any time
@ -453,6 +464,14 @@ void loop(void)
mwDisarm();
}
// Read value of AUX channel as rssi
// 0 is disable, 1-4 is AUX{1..4}
if (mcfg.rssi_aux_channel > 0) {
const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1];
// Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
}
// Failsafe routine
if (feature(FEATURE_FAILSAFE)) {
if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
@ -777,6 +796,7 @@ void loop(void)
loopTime = currentTime + mcfg.looptime;
computeIMU();
annexCode();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
@ -823,24 +843,23 @@ void loop(void)
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if (abs(AltHoldCorr) > 500) {
AltHold += AltHoldCorr / 500;
AltHoldCorr %= 500;
}
errorAltitudeI = 0;
AltHold += AltHoldCorr / 2000;
AltHoldCorr %= 2000;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHoldCorr = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
}
}
}
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE]+= throttleAngleCorrection;
rcCommand[THROTTLE] += throttleAngleCorrection;
}
if (sensors(SENSOR_GPS)) {

20
src/mw.h
View file

@ -189,6 +189,7 @@ typedef struct config_t {
// mixer-related configuration
int8_t yaw_direction;
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
uint16_t tri_yaw_min; // tail servo min
uint16_t tri_yaw_max; // tail servo max
@ -250,7 +251,10 @@ typedef struct master_t {
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
// global sensor-related stuff
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
sensor_align_e gyro_align; // gyro alignment
sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
@ -269,17 +273,18 @@ typedef struct master_t {
// Radio/ESC-related configuration
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint8_t serialrx_type; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_SERIALRX first.
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ??
uint32_t gps_baudrate; // GPS baudrate
// serial(uart1) baudrate
uint32_t serial_baudrate;
config_t profile[3]; // 3 separate profiles
@ -339,10 +344,9 @@ extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;
extern uint32_t baroPressureSum;
extern int32_t BaroAlt;
extern int32_t sonarAlt;
extern int32_t EstAlt;
@ -444,9 +448,13 @@ void featureClearAll(void);
uint32_t featureMask(void);
// spektrum
void spektrumInit(void);
void spektrumInit(rcReadRawDataPtr *callback);
bool spektrumFrameComplete(void);
// sbus
void sbusInit(rcReadRawDataPtr *callback);
bool sbusFrameComplete(void);
// buzzer
void buzzer(uint8_t warn_vbat);

2
src/printf.c
View file

@ -228,7 +228,7 @@ void tfp_printf(char *fmt, ...)
va_start(va, fmt);
tfp_format(stdout_putp, stdout_putf, fmt, va);
va_end(va);
while (!isUartTransmitEmpty(core.mainport));
while (!isSerialTransmitBufferEmpty(core.mainport));
}
static void putcp(void *p, char c)

108
src/sbus.c Normal file
View file

@ -0,0 +1,108 @@
#include "board.h"
#include "mw.h"
// driver for SBUS receiver using UART2
#define SBUS_MAX_CHANNEL 8
#define SBUS_FRAME_SIZE 25
#define SBUS_SYNCBYTE 0x0F
#define SBUS_ENDBYTE 0x00
#define SBUS_OFFSET 988
static bool sbusFrameDone = false;
static void sbusDataReceive(uint16_t c);
static uint16_t sbusReadRawRC(uint8_t chan);
// external vars (ugh)
extern int16_t failsafeCnt;
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
void sbusInit(rcReadRawDataPtr *callback)
{
int b;
for (b = 0; b < SBUS_MAX_CHANNEL; b ++)
sbusChannelData[b] = 2 * (mcfg.midrc - SBUS_OFFSET);
core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, MODE_RX);
if (callback)
*callback = sbusReadRawRC;
}
struct sbus_dat
{
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
} __attribute__ ((__packed__));
typedef union
{
uint8_t in[SBUS_FRAME_SIZE];
struct sbus_dat msg;
} sbus_msg;
static sbus_msg sbus;
// Receive ISR callback
static void sbusDataReceive(uint16_t c)
{
uint32_t sbusTime;
static uint32_t sbusTimeLast;
static uint8_t sbusFramePosition;
sbusTime = micros();
if ((sbusTime - sbusTimeLast) > 4000)
sbusFramePosition = 0;
sbusTimeLast = sbusTime;
if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE)
return;
sbusFrameDone = false; // lazy main loop didnt fetch the stuff
if (sbusFramePosition != 0)
sbus.in[sbusFramePosition - 1] = (uint8_t)c;
if (sbusFramePosition == SBUS_FRAME_SIZE - 1) {
if (sbus.in[sbusFramePosition - 1] == SBUS_ENDBYTE)
sbusFrameDone = true;
sbusFramePosition = 0;
} else {
sbusFramePosition++;
}
}
bool sbusFrameComplete(void)
{
if (sbusFrameDone) {
if (!((sbus.in[22] >> 3) & 0x0001)) { // failsave flag
failsafeCnt = 0; // clear FailSafe counter
sbusChannelData[0] = sbus.msg.chan0;
sbusChannelData[1] = sbus.msg.chan1;
sbusChannelData[2] = sbus.msg.chan2;
sbusChannelData[3] = sbus.msg.chan3;
sbusChannelData[4] = sbus.msg.chan4;
sbusChannelData[5] = sbus.msg.chan5;
sbusChannelData[6] = sbus.msg.chan6;
sbusChannelData[7] = sbus.msg.chan7;
// need more channels? No problem. Add them.
sbusFrameDone = false;
return true;
}
sbusFrameDone = false;
}
return false;
}
static uint16_t sbusReadRawRC(uint8_t chan)
{
return sbusChannelData[mcfg.rcmap[chan]] / 2 + SBUS_OFFSET;
}

86
src/sensors.c
View file

@ -50,8 +50,11 @@ void sensorsAutodetect(void)
// Accelerometer. Fuck it. Let user break shit.
retry:
switch (mcfg.acc_hardware) {
case 0: // autodetect
case 1: // ADXL345
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
@ -59,7 +62,7 @@ retry:
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
case 2: // MPU6050
case ACC_MPU6050: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
@ -68,12 +71,19 @@ retry:
}
; // fallthrough
#ifndef OLIMEXINO
case 3: // MMA8452
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
; // fallthrough
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
if (mcfg.acc_hardware == ACC_BMA280)
break;
}
#endif
}
@ -102,12 +112,12 @@ retry:
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init();
acc.init(mcfg.acc_align);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
gyro.init(mcfg.gyro_align);
#ifdef MAG
if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
if (!hmc5883lDetect(mcfg.mag_align))
sensorsClear(SENSOR_MAG);
#endif
@ -147,28 +157,6 @@ void batteryInit(void)
batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
}
// ALIGN_GYRO = 0,
// ALIGN_ACCEL = 1,
// ALIGN_MAG = 2
static void alignSensors(uint8_t type, int16_t *data)
{
int i;
int16_t tmp[3];
// make a copy :(
tmp[0] = data[0];
tmp[1] = data[1];
tmp[2] = data[2];
for (i = 0; i < 3; i++) {
int8_t axis = mcfg.align[type][i];
if (axis > 0)
data[axis - 1] = tmp[i];
else
data[-axis - 1] = -tmp[i];
}
}
static void ACC_Common(void)
{
static int32_t a[3];
@ -254,12 +242,6 @@ static void ACC_Common(void)
void ACC_getADC(void)
{
acc.read(accADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (mcfg.align[ALIGN_ACCEL][0])
alignSensors(ALIGN_ACCEL, accADC);
else
acc.align(accADC);
ACC_Common();
}
@ -346,7 +328,6 @@ static float devStandardDeviation(stdev_t *dev)
static void GYRO_Common(void)
{
int axis;
static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
static stdev_t var[3];
@ -380,42 +361,25 @@ static void GYRO_Common(void)
}
calibratingG--;
}
for (axis = 0; axis < 3; axis++) {
for (axis = 0; axis < 3; axis++)
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
previousGyroADC[axis] = gyroADC[axis];
}
}
void Gyro_getADC(void)
{
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (mcfg.align[ALIGN_GYRO][0])
alignSensors(ALIGN_GYRO, gyroADC);
else
gyro.align(gyroADC);
GYRO_Common();
}
#ifdef MAG
static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0;
static void Mag_getRawADC(void)
{
// MAG driver will align itself, so no need to alignSensors()
hmc5883lRead(magADC);
}
void Mag_init(void)
{
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON;
hmc5883lInit(magCal);
hmc5883lInit();
LED1_OFF;
magInit = 1;
}
@ -432,11 +396,7 @@ int Mag_getADC(void)
t = currentTime + 100000;
// Read mag sensor
Mag_getRawADC();
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
hmc5883lRead(magADC);
if (f.CALIBRATE_MAG) {
tCal = t;
@ -449,9 +409,9 @@ int Mag_getADC(void)
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[ROLL] -= mcfg.magZero[ROLL];
magADC[PITCH] -= mcfg.magZero[PITCH];
magADC[YAW] -= mcfg.magZero[YAW];
magADC[X] -= mcfg.magZero[X];
magADC[Y] -= mcfg.magZero[Y];
magADC[Z] -= mcfg.magZero[Z];
}
if (tCal != 0) {

22
src/serial.c
View file

@ -157,16 +157,16 @@ void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 16;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 24;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
}
@ -174,16 +174,16 @@ void serialize16(int16_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
t = a >> 8 & 0xff;
uartWrite(core.mainport, t);
serialWrite(core.mainport, t);
checksum ^= t;
}
void serialize8(uint8_t a)
{
uartWrite(core.mainport, a);
serialWrite(core.mainport, a);
checksum ^= a;
}
@ -672,14 +672,14 @@ void serialCom(void)
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
while (isUartAvailable(core.mainport)) {
c = uartRead(core.mainport);
while (serialTotalBytesWaiting(core.mainport)) {
c = serialRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
@ -715,7 +715,7 @@ void serialCom(void)
c_state = IDLE;
}
}
if (!cliMode && !isUartAvailable(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
if (!cliMode && !serialTotalBytesWaiting(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
return;
}

21
src/spektrum.c
View file

@ -8,26 +8,35 @@
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
static bool spekHiRes = false;
static bool spekDataIncoming = false;
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);
static uint16_t spektrumReadRawRC(uint8_t chan);
// external vars (ugh)
extern int16_t failsafeCnt;
void spektrumInit(void)
void spektrumInit(rcReadRawDataPtr *callback)
{
if (mcfg.spektrum_hires) {
switch (mcfg.serialrx_type) {
case SERIALRX_SPEKTRUM2048:
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
} else {
spekHiRes = true;
break;
case SERIALRX_SPEKTRUM1024:
// 10 bit frames
spek_chan_shift = 2;
spek_chan_mask = 0x03;
spekHiRes = false;
break;
}
core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
if (callback)
*callback = spektrumReadRawRC;
}
// Receive ISR callback
@ -57,9 +66,7 @@ bool spektrumFrameComplete(void)
return rcFrameComplete;
}
// static const uint8_t spekRcChannelMap[SPEK_MAX_CHANNEL] = {1, 2, 3, 0, 4, 5, 6};
uint16_t spektrumReadRawRC(uint8_t chan)
static uint16_t spektrumReadRawRC(uint8_t chan)
{
uint16_t data;
static uint32_t spekChannelData[SPEK_MAX_CHANNEL];
@ -77,7 +84,7 @@ uint16_t spektrumReadRawRC(uint8_t chan)
if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) {
data = mcfg.midrc;
} else {
if (mcfg.spektrum_hires)
if (spekHiRes)
data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode
else
data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode

16
src/telemetry.c
View file

@ -50,26 +50,26 @@ extern uint8_t batteryCellCount;
static void sendDataHead(uint8_t id)
{
uartWrite(core.telemport, PROTOCOL_HEADER);
uartWrite(core.telemport, id);
serialWrite(core.telemport, PROTOCOL_HEADER);
serialWrite(core.telemport, id);
}
static void sendTelemetryTail(void)
{
uartWrite(core.telemport, PROTOCOL_TAIL);
serialWrite(core.telemport, PROTOCOL_TAIL);
}
static void serializeFrsky(uint8_t data)
{
// take care of byte stuffing
if (data == 0x5e) {
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3e);
serialWrite(core.telemport, 0x5d);
serialWrite(core.telemport, 0x3e);
} else if (data == 0x5d) {
uartWrite(core.telemport, 0x5d);
uartWrite(core.telemport, 0x3d);
serialWrite(core.telemport, 0x5d);
serialWrite(core.telemport, 0x3d);
} else
uartWrite(core.telemport, data);
serialWrite(core.telemport, data);
}
static void serialize16(int16_t a)

12
src/utils.c Normal file
View file

@ -0,0 +1,12 @@
#include "board.h"
#include "mw.h"
int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}

3
src/utils.h Normal file
View file

@ -0,0 +1,3 @@
#pragma once
int constrain(int amt, int low, int high);