diff --git a/baseflight.uvopt b/baseflight.uvopt
index a588256d7..e72f7e5c9 100755
--- a/baseflight.uvopt
+++ b/baseflight.uvopt
@@ -504,8 +504,8 @@
0
0
0
- 0
- 0
+ 1
+ 1
0
.\src\config.c
config.c
@@ -558,10 +558,10 @@
1
0
0
- 45
+ 0
0
- 55
- 55
+ 124
+ 129
0
.\src\mw.c
mw.c
@@ -614,10 +614,10 @@
5
0
0
- 0
+ 20
0
- 0
- 0
+ 151
+ 167
0
.\src\mw.h
mw.h
@@ -649,10 +649,10 @@
1
0
0
- 16
+ 0
0
44
- 67
+ 72
0
.\src\drv_adxl345.c
drv_adxl345.c
@@ -691,10 +691,10 @@
1
0
0
- 43
+ 11
0
- 122
- 149
+ 121
+ 121
0
.\src\drv_i2c.c
drv_i2c.c
@@ -755,6 +755,20 @@
.\src\drv_uart.c
drv_uart.c
+
+ 2
+ 0
+ 1
+ 0
+ 0
+ 7
+ 0
+ 17
+ 52
+ 0
+ .\src\drv_ledring.c
+ drv_ledring.c
+
@@ -939,7 +953,7 @@
0
0
133
- 133
+ 136
0
.\src\baseflight_startups\startup_stm32f10x_md.s
startup_stm32f10x_md.s
diff --git a/baseflight.uvproj b/baseflight.uvproj
index c895db426..6cf1721f5 100755
--- a/baseflight.uvproj
+++ b/baseflight.uvproj
@@ -486,6 +486,11 @@
1
.\src\drv_uart.c
+
+ drv_ledring.c
+ 1
+ .\src\drv_ledring.c
+
@@ -1040,6 +1045,11 @@
1
.\src\drv_uart.c
+
+ drv_ledring.c
+ 1
+ .\src\drv_ledring.c
+
diff --git a/src/board.h b/src/board.h
index daedee56d..af63c37c1 100755
--- a/src/board.h
+++ b/src/board.h
@@ -15,6 +15,7 @@
#include "drv_bmp085.h"
#include "drv_hmc5883l.h"
#include "drv_i2c.h"
+#include "drv_ledring.h"
#include "drv_mpu3050.h"
#include "drv_pwm.h"
#include "drv_uart.h"
@@ -36,6 +37,7 @@ typedef enum {
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_CAMTRIG = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7,
+ FEATURE_LED_RING = 1 << 8,
} AvailableFeatures;
#define digitalHi(p, i) { p->BSRR = i; }
diff --git a/src/config.c b/src/config.c
index a6abec52e..b813f03cf 100755
--- a/src/config.c
+++ b/src/config.c
@@ -8,7 +8,7 @@
config_t cfg;
static uint32_t enabledSensors = 0;
-static uint8_t checkNewConf = 2;
+static uint8_t checkNewConf = 3;
void readEEPROM(void)
{
@@ -65,7 +65,7 @@ void checkFirstTime(void)
cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
- featureSet(FEATURE_VBAT | FEATURE_PPM);
+ featureSet(FEATURE_VBAT); // | FEATURE_PPM); // sadly, this is for hackers only
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
@@ -102,8 +102,9 @@ void checkFirstTime(void)
cfg.accTrim[1] = 0;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.powerTrigger1 = 0;
-
+
// Radio/ESC
+ cfg.deadband = 0;
cfg.midrc = 1500;
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
diff --git a/src/drv_ledring.c b/src/drv_ledring.c
new file mode 100644
index 000000000..71e345299
--- /dev/null
+++ b/src/drv_ledring.c
@@ -0,0 +1,53 @@
+#include "board.h"
+#include "mw.h"
+
+// Driver for DFRobot I2C Led Ring
+#define LED_RING_ADDRESS 0x6D
+
+bool ledringDetect(void)
+{
+ bool ack = false;
+ uint8_t sig = 'e';
+
+ ack = 0; // i2cWrite(LED_RING_ADDRESS, 0xFF, 1, &sig);
+ if (!ack)
+ return false;
+ return true;
+}
+
+void ledringState(void)
+{
+ uint8_t b[10];
+ static uint8_t state;
+
+ if (state == 0) {
+ b[0] = 'z';
+ b[1] = (180 - heading) / 2; // 1 unit = 2 degrees;
+ // i2cWrite(LED_RING_ADDRESS, 0xFF, 2, b);
+ state = 1;
+ } else if (state == 1) {
+ b[0] = 'y';
+ b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180);
+ b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180);
+ // i2cWrite(LED_RING_ADDRESS, 0xFF, 3, b);
+ state = 2;
+ } else if (state == 2) {
+ b[0] = 'd'; // all unicolor GREEN
+ b[1] = 1;
+ if (armed)
+ b[2] = 1;
+ else
+ b[2] = 0;
+ // i2cWrite(LED_RING_ADDRESS, 0xFF, 3, b);
+ state = 0;
+ }
+}
+
+void ledringBlink(void)
+{
+ uint8_t b[3];
+ b[0] = 'k';
+ b[1] = 10;
+ b[2] = 10;
+ // i2cWrite(LED_RING_ADDRESS, 0xFF, 3, b);
+}
diff --git a/src/drv_ledring.h b/src/drv_ledring.h
new file mode 100644
index 000000000..579f0ba25
--- /dev/null
+++ b/src/drv_ledring.h
@@ -0,0 +1,5 @@
+#pragma once
+
+bool ledringDetect(void);
+void ledringState(void);
+void ledringBlink(void);
diff --git a/src/mw.c b/src/mw.c
index ca98a690a..2abe8ff52 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -126,13 +126,13 @@ void annexCode(void)
for (axis = 0; axis < 3; axis++) {
uint16_t tmp = min(abs(rcData[axis] - cfg.midrc), 500);
-#if defined(DEADBAND)
- if (tmp > DEADBAND) {
- tmp -= DEADBAND;
- } else {
- tmp = 0;
+ if (cfg.deadband > 0) {
+ if (tmp > cfg.deadband) {
+ tmp -= cfg.deadband;
+ } else {
+ tmp = 0;
+ }
}
-#endif
if (axis != 2) { //ROLL & PITCH
uint16_t tmp2 = tmp / 100;
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
diff --git a/src/mw.h b/src/mw.h
index 9bc58eb66..375457f7f 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -164,6 +164,7 @@ typedef struct config_t {
uint8_t powerTrigger1; // trigger for alarm based on power consumption
// Radio/ESC-related configuration
+ uint8_t deadband; // introduce a deadband around the stick center. Must be greater than zero
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000