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

Merge branch 'master' into xbus_jr01

Browse source
This commit is contained in:
Stefan Grufman 2015-01-24 13:54:58 +01:00
commit 12f45b4f04
73 changed files with 1436 additions and 443 deletions
Download patch file Download diff file Expand all files Collapse all files

168
src/main/blackbox/blackbox.c
View file

@ -74,7 +74,6 @@
#include "config/config_profile.h"
#include "config/config_master.h"
#include "blackbox_fielddefs.h"
#include "blackbox.h"
#define BLACKBOX_BAUDRATE 115200
@ -83,6 +82,9 @@
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
#define STATIC_ASSERT(condition, name ) \
typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
// Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)
@ -148,6 +150,7 @@ typedef struct blackboxGPSFieldDefinition_t {
uint8_t isSigned;
uint8_t predict;
uint8_t encode;
uint8_t condition; // Decide whether this field should appear in the log
} blackboxGPSFieldDefinition_t;
/**
@ -179,6 +182,7 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
{"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
{"amperageLatest",UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE},
#ifdef MAG
{"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
{"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
@ -211,18 +215,19 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
#ifdef GPS
// GPS position/vel frame
static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
{"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
{"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
{"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}
{"time", UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
{"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_ground_course",UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}
};
// GPS home frame
static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = {
{"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
{"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
{"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}
};
#endif
@ -235,7 +240,8 @@ typedef enum BlackboxState {
BLACKBOX_STATE_SEND_GPS_G_HEADERS,
BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_PRERUN,
BLACKBOX_STATE_RUNNING
BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN
} BlackboxState;
typedef struct gpsState_t {
@ -267,8 +273,11 @@ static struct {
} u;
} xmitState;
// Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
static uint32_t blackboxConditionCache;
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions);
static uint32_t blackboxIteration;
static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
@ -625,6 +634,12 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_VBAT:
return feature(FEATURE_VBAT);
case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE:
return feature(FEATURE_CURRENT_METER);
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
return false;
default:
@ -671,6 +686,9 @@ static void blackboxSetState(BlackboxState newState)
blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0;
break;
case BLACKBOX_STATE_SHUTTING_DOWN:
xmitState.u.startTime = millis();
break;
default:
;
}
@ -712,6 +730,11 @@ static void writeIntraframe(void)
writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) {
// 12bit value directly from ADC
writeUnsignedVB(blackboxCurrent->amperageLatest);
}
#ifdef MAG
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
for (x = 0; x < XYZ_AXIS_COUNT; x++)
@ -763,8 +786,8 @@ static void writeInterframe(void)
//No need to store iteration count since its delta is always 1
/*
* Since the difference between the difference between successive times will be nearly zero, use
* second-order differences.
* Since the difference between the difference between successive times will be nearly zero (due to consistent
* looptime spacing), use second-order differences.
*/
writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
@ -797,13 +820,17 @@ static void writeInterframe(void)
writeTag8_4S16(deltas);
//Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO
//Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO
int optionalFieldCount = 0;
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
}
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) {
deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->amperageLatest - blackboxLast->amperageLatest;
}
#ifdef MAG
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
for (x = 0; x < XYZ_AXIS_COUNT; x++)
@ -894,6 +921,14 @@ static void releaseBlackboxPort(void)
serialSetBaudRate(blackboxPort, previousBaudRate);
endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
/*
* Normally this would be handled by mw.c, but since we take an unknown amount
* of time to shut down asynchronously, we're the only ones that know when to call it.
*/
if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) {
mspAllocateSerialPorts(&masterConfig.serialConfig);
}
}
void startBlackbox(void)
@ -914,7 +949,7 @@ void startBlackbox(void)
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatest;
vbatReference = vbatLatestADC;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
@ -931,10 +966,18 @@ void startBlackbox(void)
void finishBlackbox(void)
{
if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
blackboxSetState(BLACKBOX_STATE_STOPPED);
if (blackboxState == BLACKBOX_STATE_RUNNING) {
blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
} else if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED
&& blackboxState != BLACKBOX_STATE_SHUTTING_DOWN) {
/*
* We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event.
* Just give the port back and stop immediately.
*/
releaseBlackboxPort();
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
}
@ -955,6 +998,17 @@ static void writeGPSFrame()
{
blackboxWrite('G');
/*
* If we're logging every frame, then a GPS frame always appears just after a frame with the
* currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
*
* If we're not logging every frame, we need to store the time of this GPS frame.
*/
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
// Predict the time of the last frame in the main log
writeUnsignedVB(currentTime - blackboxHistory[1]->time);
}
writeUnsignedVB(GPS_numSat);
writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
@ -997,7 +1051,8 @@ static void loadBlackboxState(void)
for (i = 0; i < motorCount; i++)
blackboxCurrent->motor[i] = motor[i];
blackboxCurrent->vbatLatest = vbatLatest;
blackboxCurrent->vbatLatest = vbatLatestADC;
blackboxCurrent->amperageLatest = amperageLatestADC;
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++)
@ -1141,7 +1196,6 @@ static bool blackboxWriteSysinfo()
blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
xmitState.u.serialBudget -= strlen("H P interval:%d/%d\n");
break;
case 5:
blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
@ -1163,10 +1217,10 @@ static bool blackboxWriteSysinfo()
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
xmitState.u.serialBudget -= strlen("H gyro.scale:0x%x\n") + 6;
break;
break;
case 9:
blackboxPrintf("H acc_1G:%u\n", acc_1G);
xmitState.u.serialBudget -= strlen("H acc_1G:%u\n");
break;
case 10:
@ -1185,6 +1239,10 @@ static bool blackboxWriteSysinfo()
xmitState.u.serialBudget -= strlen("H vbatref:%u\n");
break;
case 13:
blackboxPrintf("H currentMeter:%i,%i\n", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale);
xmitState.u.serialBudget -= strlen("H currentMeter:%i,%i\n");
default:
return true;
}
@ -1193,10 +1251,49 @@ static bool blackboxWriteSysinfo()
return false;
}
/**
* Write the given event to the log immediately
*/
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
{
if (blackboxState != BLACKBOX_STATE_RUNNING)
return;
//Shared header for event frames
blackboxWrite('E');
blackboxWrite(event);
//Now serialize the data for this specific frame type
switch (event) {
case FLIGHT_LOG_EVENT_SYNC_BEEP:
writeUnsignedVB(data->syncBeep.time);
break;
case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START:
blackboxWrite(data->autotuneCycleStart.phase);
blackboxWrite(data->autotuneCycleStart.cycle);
blackboxWrite(data->autotuneCycleStart.p);
blackboxWrite(data->autotuneCycleStart.i);
blackboxWrite(data->autotuneCycleStart.d);
break;
case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT:
blackboxWrite(data->autotuneCycleResult.overshot);
blackboxWrite(data->autotuneCycleStart.p);
blackboxWrite(data->autotuneCycleStart.i);
blackboxWrite(data->autotuneCycleStart.d);
break;
case FLIGHT_LOG_EVENT_LOG_END:
blackboxPrint("End of log");
blackboxWrite(0);
break;
}
}
// Beep the buzzer and write the current time to the log as a synchronization point
static void blackboxPlaySyncBeep()
{
uint32_t now = micros();
flightLogEvent_syncBeep_t eventData;
eventData.time = micros();
/*
* The regular beep routines aren't going to work for us, because they queue up the beep to be executed later.
@ -1207,10 +1304,7 @@ static void blackboxPlaySyncBeep()
// Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive
queueConfirmationBeep(1);
blackboxWrite('E');
blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP);
writeUnsignedVB(now);
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
}
void handleBlackbox(void)
@ -1249,14 +1343,14 @@ void handleBlackbox(void)
case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1,
ARRAY_LENGTH(blackboxGpsHFields), NULL, NULL)) {
ARRAY_LENGTH(blackboxGpsHFields), &blackboxGpsHFields[0].condition, &blackboxGpsHFields[1].condition)) {
blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS);
}
break;
case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1,
ARRAY_LENGTH(blackboxGpsGFields), NULL, NULL)) {
ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
}
break;
@ -1269,9 +1363,9 @@ void handleBlackbox(void)
blackboxSetState(BLACKBOX_STATE_PRERUN);
break;
case BLACKBOX_STATE_PRERUN:
blackboxPlaySyncBeep();
blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxPlaySyncBeep();
break;
case BLACKBOX_STATE_RUNNING:
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
@ -1317,6 +1411,20 @@ void handleBlackbox(void)
blackboxIFrameIndex++;
}
break;
case BLACKBOX_STATE_SHUTTING_DOWN:
//On entry of this state, startTime is set
/*
* Wait for the log we've transmitted to make its way to the logger before we release the serial port,
* since releasing the port clears the Tx buffer.
*
* Don't wait longer than it could possibly take if something funky happens.
*/
if (millis() > xmitState.u.startTime + 200 || isSerialTransmitBufferEmpty(blackboxPort)) {
releaseBlackboxPort();
blackboxSetState(BLACKBOX_STATE_STOPPED);
}
break;
default:
break;
}

8
src/main/blackbox/blackbox.h
View file

@ -17,9 +17,12 @@
#pragma once
#include "common/axis.h"
#include <stdint.h>
#include "common/axis.h"
#include "flight/mixer.h"
#include "blackbox/blackbox_fielddefs.h"
typedef struct blackboxValues_t {
uint32_t time;
@ -32,6 +35,7 @@ typedef struct blackboxValues_t {
int16_t servo[MAX_SUPPORTED_SERVOS];
uint16_t vbatLatest;
uint16_t amperageLatest;
#ifdef BARO
int32_t BaroAlt;
@ -41,6 +45,8 @@ typedef struct blackboxValues_t {
#endif
} blackboxValues_t;
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);
void handleBlackbox(void);
void startBlackbox(void);

43
src/main/blackbox/blackbox_fielddefs.h
View file

@ -32,11 +32,14 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_MAG,
FLIGHT_LOG_FIELD_CONDITION_BARO,
FLIGHT_LOG_FIELD_CONDITION_VBAT,
FLIGHT_LOG_FIELD_CONDITION_AMPERAGE,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2,
FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME,
FLIGHT_LOG_FIELD_CONDITION_NEVER,
FLIGHT_LOG_FIELD_CONDITION_FIRST = FLIGHT_LOG_FIELD_CONDITION_ALWAYS,
@ -72,7 +75,10 @@ typedef enum FlightLogFieldPredictor {
FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8,
//Predict vbatref, the reference ADC level stored in the header
FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9
FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9,
//Predict the last time value written in the main stream
FLIGHT_LOG_FIELD_PREDICTOR_LAST_MAIN_FRAME_TIME = 10
} FlightLogFieldPredictor;
@ -93,5 +99,40 @@ typedef enum FlightLogFieldSign {
typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10,
FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
FLIGHT_LOG_EVENT_LOG_END = 255
} FlightLogEvent;
typedef struct flightLogEvent_syncBeep_t {
uint32_t time;
} flightLogEvent_syncBeep_t;
typedef struct flightLogEvent_autotuneCycleStart_t {
uint8_t phase;
uint8_t cycle;
uint8_t p;
uint8_t i;
uint8_t d;
} flightLogEvent_autotuneCycleStart_t;
typedef struct flightLogEvent_autotuneCycleResult_t {
uint8_t overshot;
uint8_t p;
uint8_t i;
uint8_t d;
} flightLogEvent_autotuneCycleResult_t;
typedef union flightLogEventData_t
{
flightLogEvent_syncBeep_t syncBeep;
flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
} flightLogEventData_t;
typedef struct flightLogEvent_t
{
FlightLogEvent event;
flightLogEventData_t data;
} flightLogEvent_t;

49
src/main/common/maths.c
View file

@ -92,3 +92,52 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return ((a / b) - (destMax - destMin)) + destMax;
}
// 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 a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = cosz * cosy;
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];
v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}

41
src/main/common/maths.h
View file

@ -21,16 +21,10 @@
#define sq(x) ((x)*(x))
#endif
#ifdef M_PI
// M_PI should be float, but previous definition may be double
# undef M_PI
#endif
#define M_PI 3.14159265358979323846f
// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define RAD (M_PI / 180.0f)
#define DEG2RAD(degrees) (degrees * RAD)
#define RAD (M_PIf / 180.0f)
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
@ -42,6 +36,31 @@ typedef struct stdev_t
int m_n;
} stdev_t;
// Floating point 3 vector.
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
// Floating point Euler angles.
// Be carefull, could be either of degrees or radians.
typedef struct fp_angles {
float roll;
float pitch;
float yaw;
} fp_angles_def;
typedef union {
float raw[3];
fp_angles_def angles;
} fp_angles_t;
int32_t applyDeadband(int32_t value, int32_t deadband);
int constrain(int amt, int low, int high);
@ -54,3 +73,7 @@ float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);
void rotateV(struct fp_vector *v, fp_angles_t *delta);

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

@ -92,7 +92,7 @@ int a2d(char ch)
char a2i(char ch, const char **src, int base, int *nump)
{
char *p = *src;
const char *p = *src;
int num = 0;
int digit;
while ((digit = a2d(ch)) >= 0) {

5
src/main/config/config.c
View file

@ -108,7 +108,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 88;
static const uint8_t EEPROM_CONF_VERSION = 89;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -159,6 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D_f[YAW] = 0.05f;
pidProfile->A_level = 5.0f;
pidProfile->H_level = 3.0f;
pidProfile->H_sensitivity = 75;
}
#ifdef GPS
@ -612,7 +613,7 @@ void activateConfig(void)
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
configureImu(
configureIMU(
&imuRuntimeConfig,
&currentProfile->pidProfile,
&currentProfile->accDeadband

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

@ -20,6 +20,8 @@
#include "platform.h"
#include "build_config.h"
#include "common/maths.h"
#include "system.h"
@ -110,7 +112,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
GPIO_SetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
SPI_I2S_DeInit(SPI1);
SPI_I2S_DeInit(SPIx);
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
@ -122,11 +124,11 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI1, &SPI_InitStructure);
SPI_Init(SPIx, &SPI_InitStructure);
SPI_RxFIFOThresholdConfig(L3GD20_SPI, SPI_RxFIFOThreshold_QF);
SPI_RxFIFOThresholdConfig(SPIx, SPI_RxFIFOThreshold_QF);
SPI_Cmd(SPI1, ENABLE);
SPI_Cmd(SPIx, ENABLE);
}
void l3gd20GyroInit(void)
@ -194,6 +196,8 @@ static void l3gd20GyroRead(int16_t *gyroData)
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
{
UNUSED(lpf);
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

14
src/main/drivers/accgyro_mpu6050.c
View file

@ -178,9 +178,17 @@ static const mpu6050Config_t *mpu6050Config = NULL;
void mpu6050GpioInit(void) {
gpio_config_t gpio;
if (mpu6050Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
}
#ifdef STM32F303
if (mpu6050Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (mpu6050Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = mpu6050Config->gpioPin;
gpio.speed = Speed_2MHz;

5
src/main/drivers/accgyro_mpu6050.h
View file

@ -18,7 +18,12 @@
#pragma once
typedef struct mpu6050Config_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
} mpu6050Config_t;

2
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -319,7 +319,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
gyro->read = mpu6000SpiGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}

2
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -128,7 +128,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
// default lpf is 42Hz
if (lpf >= 188)

55
src/main/drivers/adc_stm32f10x.c
View file

@ -48,7 +48,7 @@ extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
void adcInit(drv_adc_config_t *init)
{
#ifdef CC3D
#if defined(CJMCU) || defined(CC3D)
UNUSED(init);
#endif
@ -64,58 +64,49 @@ void adcInit(drv_adc_config_t *init)
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
#ifdef CC3D
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_0;
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_BATTERY].enabled = true;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5;
#else
// configure always-present battery index (ADC4)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
#ifdef VBAT_ADC_GPIO_PIN
GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN;
GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_BATTERY].enabled = true;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5;
#endif
#ifdef EXTERNAL1_ADC_GPIO
if (init->enableExternal1) {
#ifdef OLIMEXINO
GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5;
adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5;
GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN;
GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL;
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
#endif
#ifdef NAZE
GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_5;
adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5;
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_239Cycles5;
#endif
}
#endif // !CC3D
#endif
#ifdef RSSI_ADC_GPIO
if (init->enableRSSI) {
GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1;
adcConfig[ADC_RSSI].adcChannel = ADC_Channel_1;
GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN;
GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL;
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_RSSI].enabled = true;
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_239Cycles5;
}
#endif
GPIO_Init(GPIOA, &GPIO_InitStructure);
#ifdef CURRENT_METER_ADC_GPIO
if (init->enableCurrentMeter) {
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_9;
GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN;
GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL;
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
adcConfig[ADC_CURRENT].enabled = true;
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5;
}
#endif
RCC_ADCCLKConfig(RCC_PCLK2_Div8); // 9MHz from 72MHz APB2 clock(HSE), 8MHz from 64MHz (HSI)
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);

8
src/main/drivers/compass_hmc5883l.c
View file

@ -142,10 +142,16 @@ void hmc5883lInit(void)
gpio_config_t gpio;
if (hmc5883Config) {
#ifdef STM32F303
if (hmc5883Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (hmc5883Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = hmc5883Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;

5
src/main/drivers/compass_hmc5883l.h
View file

@ -18,7 +18,12 @@
#pragma once
typedef struct hmc5883Config_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
} hmc5883Config_t;

77
src/main/drivers/pwm_mapping.c
View file

@ -69,7 +69,7 @@ enum {
MAP_TO_SERVO_OUTPUT,
};
#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(MASSIVEF3) || defined(PORT103R)
#if defined(NAZE) || defined(OLIMEXINO) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(EUSTM32F103RC) || defined(PORT103R)
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
@ -267,15 +267,64 @@ static const uint16_t multiPWM[] = {
};
static const uint16_t airPPM[] = {
// TODO
0xFFFF
};
static const uint16_t airPWM[] = {
// TODO
0xFFFF
};
#endif
#ifdef SPRACINGF3
static const uint16_t multiPPM[] = {
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t multiPWM[] = {
PWM1 | (MAP_TO_PWM_INPUT << 8),
PWM2 | (MAP_TO_PWM_INPUT << 8),
PWM3 | (MAP_TO_PWM_INPUT << 8),
PWM4 | (MAP_TO_PWM_INPUT << 8),
PWM5 | (MAP_TO_PWM_INPUT << 8),
PWM6 | (MAP_TO_PWM_INPUT << 8),
PWM7 | (MAP_TO_PWM_INPUT << 8),
PWM8 | (MAP_TO_PWM_INPUT << 8),
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
0xFFFF
};
static const uint16_t airPPM[] = {
// TODO
0xFFFF
};
static const uint16_t airPWM[] = {
// TODO
0xFFFF
};
#endif
static const uint16_t * const hardwareMaps[] = {
multiPWM,
multiPPM,
@ -345,24 +394,20 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
#endif
#ifdef STM32F10X
// skip ADC for RSSI
if (init->useRSSIADC && timerIndex == PWM2)
#ifdef VBAT_ADC_GPIO
if (init->useVbat && timerHardwarePtr->gpio == VBAT_ADC_GPIO && timerHardwarePtr->pin == VBAT_ADC_GPIO_PIN) {
continue;
}
#endif
#ifdef CC3D
if (init->useVbat && timerIndex == Vbat_TIMER) {
#ifdef RSSI_ADC_GPIO
if (init->useRSSIADC && timerHardwarePtr->gpio == RSSI_ADC_GPIO && timerHardwarePtr->pin == RSSI_ADC_GPIO_PIN) {
continue;
}
#endif
#ifdef CC3D
if (init->useCurrentMeterADC && timerIndex == CurrentMeter_TIMER) {
continue;
}
#endif
#ifdef CC3D
if (init->useRSSIADC && timerIndex == RSSI_TIMER) {
#ifdef CURRENT_METER_ADC_GPIO
if (init->useCurrentMeterADC && timerHardwarePtr->gpio == CURRENT_METER_ADC_GPIO && timerHardwarePtr->pin == CURRENT_METER_ADC_GPIO_PIN) {
continue;
}
#endif
@ -387,6 +432,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
type = MAP_TO_SERVO_OUTPUT;
#endif
#if defined(SPRACINGF3)
// remap PWM15+16 as servos
if ((timerIndex == PWM15 || timerIndex == PWM16) && timerHardwarePtr->tim == TIM15)
type = MAP_TO_SERVO_OUTPUT;
#endif
#if defined(NAZE32PRO) || (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3))
// remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer
if (init->useSoftSerial) {

6
src/main/drivers/pwm_mapping.h
View file

@ -65,7 +65,7 @@ typedef struct pwmOutputConfiguration_s {
uint8_t motorCount;
} pwmOutputConfiguration_t;
// This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data.
// This indexes into the read-only hardware definition structure, timerHardware_t
enum {
PWM1 = 0,
PWM2,
@ -80,5 +80,7 @@ enum {
PWM11,
PWM12,
PWM13,
PWM14
PWM14,
PWM15,
PWM16
};

21
src/main/drivers/serial_usb_vcp.c
View file

@ -17,17 +17,17 @@
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include "platform.h"
#include "build_config.h"
#include "usb_core.h"
#include "usb_init.h"
#include "hw_config.h"
#include <stdbool.h>
#include <string.h>
#include "drivers/system.h"
#include "serial.h"
@ -40,26 +40,37 @@ static vcpPort_t vcpPort;
void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{
UNUSED(instance);
UNUSED(baudRate);
// TODO implement
}
void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
{
UNUSED(instance);
UNUSED(mode);
// TODO implement
}
bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance)
{
UNUSED(instance);
return true;
}
uint8_t usbVcpAvailable(serialPort_t *instance)
{
UNUSED(instance);
return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere
}
uint8_t usbVcpRead(serialPort_t *instance)
{
UNUSED(instance);
uint8_t buf[1];
uint32_t rxed = 0;
@ -73,6 +84,8 @@ uint8_t usbVcpRead(serialPort_t *instance)
void usbVcpWrite(serialPort_t *instance, uint8_t c)
{
UNUSED(instance);
uint32_t txed;
uint32_t start = millis();

6
src/main/drivers/sound_beeper_stm32f30x.c
View file

@ -21,13 +21,17 @@
#include "platform.h"
#include "build_config.h"
#include "gpio.h"
#include "sound_beeper.h"
void initBeeperHardware(beeperConfig_t *config)
{
#ifdef BEEPER
#ifndef BEEPER
UNUSED(config);
#else
gpio_config_t gpioConfig = {
config->gpioPin,
config->gpioMode,

31
src/main/drivers/timer.c
View file

@ -220,6 +220,37 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
#endif
#ifdef SPRACINGF3
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
{ TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // RC_CH1 - PA0 - *TIM2_CH1
{ TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N
{ TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // RC_CH3 - PB10 - *TIM2_CH3, USART3_TX (AF7)
{ TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // RC_CH4 - PB11 - *TIM2_CH4, USART3_RX (AF7)
{ TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH1
{ TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH2
{ TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // RC_CH6 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
{ TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // RC_CH7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
{ TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
{ TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
{ TIM4, GPIOA, Pin_11, TIM_Channel_1, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_10}, // PWM3 - PA11
{ TIM4, GPIOA, Pin_12, TIM_Channel_2, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource12, GPIO_AF_10}, // PWM4 - PA12
{ TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM5 - PB8
{ TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM6 - PB9
{ TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM7 - PA2
{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM8 - PA3
{ TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO_TIMER / LED_STRIP
};
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17))
#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4)
#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15 | RCC_APB2Periph_TIM16 | RCC_APB2Periph_TIM17)
#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB)
#endif
#define USED_TIMER_COUNT BITCOUNT(USED_TIMERS)
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4

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

@ -17,18 +17,6 @@
#pragma once
#ifdef SPARKY
#define USABLE_TIMER_CHANNEL_COUNT 11
#endif
#ifdef CHEBUZZF3
#define USABLE_TIMER_CHANNEL_COUNT 18
#endif
#ifdef CC3D
#define USABLE_TIMER_CHANNEL_COUNT 12
#endif
#if !defined(USABLE_TIMER_CHANNEL_COUNT)
#define USABLE_TIMER_CHANNEL_COUNT 14
#endif

130
src/main/flight/autotune.c
View file

@ -31,6 +31,9 @@
#include "flight/flight.h"
#include "config/config.h"
#include "blackbox/blackbox.h"
extern int16_t debug[4];
/*
@ -97,6 +100,12 @@ typedef enum {
PHASE_SAVE_OR_RESTORE_PIDS,
} autotunePhase_e;
typedef enum {
CYCLE_TUNE_I = 0,
CYCLE_TUNE_PD,
CYCLE_TUNE_PD2
} autotuneCycle_e;
static const pidIndex_e angleIndexToPidIndexMap[] = {
PIDROLL,
PIDPITCH
@ -112,7 +121,7 @@ static pidProfile_t pidBackup;
static uint8_t pidController;
static uint8_t pidIndex;
static bool rising;
static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability.
static autotuneCycle_e cycle;
static uint32_t timeoutAt;
static angle_index_t autoTuneAngleIndex;
static autotunePhase_e phase = PHASE_IDLE;
@ -140,10 +149,33 @@ bool isAutotuneIdle(void)
return phase == PHASE_IDLE;
}
#ifdef BLACKBOX
static void autotuneLogCycleStart()
{
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleStart_t eventData;
eventData.phase = phase;
eventData.cycle = cycle;
eventData.p = pid.p * MULTIWII_P_MULTIPLIER;
eventData.i = pid.i * MULTIWII_I_MULTIPLIER;
eventData.d = pid.d;
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData);
}
}
#endif
static void startNewCycle(void)
{
rising = !rising;
secondPeakAngle = firstPeakAngle = 0;
#ifdef BLACKBOX
autotuneLogCycleStart();
#endif
}
static void updatePidIndex(void)
@ -155,8 +187,7 @@ static void updateTargetAngle(void)
{
if (rising) {
targetAngle = AUTOTUNE_TARGET_ANGLE;
}
else {
} else {
targetAngle = -AUTOTUNE_TARGET_ANGLE;
}
@ -210,30 +241,48 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
} else if (secondPeakAngle > 0) {
if (cycleCount == 0) {
// when checking the I value, we would like to overshoot the target position by half of the max oscillation.
if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) {
pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
} else {
pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
pid.i = AUTOTUNE_MINIMUM_I_VALUE;
switch (cycle) {
case CYCLE_TUNE_I:
// when checking the I value, we would like to overshoot the target position by half of the max oscillation.
if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) {
pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
} else {
pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
pid.i = AUTOTUNE_MINIMUM_I_VALUE;
}
}
}
// go back to checking P and D
cycleCount = 1;
pidProfile->I8[pidIndex] = 0;
startNewCycle();
} else {
// we are checking P and D values
// set up to look for the 2nd peak
firstPeakAngle = currentAngle;
timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleResult_t eventData;
eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1;
eventData.p = pidProfile->P8[pidIndex];
eventData.i = pidProfile->I8[pidIndex];
eventData.d = pidProfile->D8[pidIndex];
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
// go back to checking P and D
cycle = CYCLE_TUNE_PD;
pidProfile->I8[pidIndex] = 0;
startNewCycle();
break;
case CYCLE_TUNE_PD:
case CYCLE_TUNE_PD2:
// we are checking P and D values
// set up to look for the 2nd peak
firstPeakAngle = currentAngle;
timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
break;
}
}
} else {
// we saw the first peak. looking for the second
// We saw the first peak while tuning PD, looking for the second
if (currentAngle < firstPeakAngle) {
firstPeakAngle = currentAngle;
@ -266,8 +315,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
}
#else
pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
#endif
} else {
// undershot
@ -286,15 +335,30 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
pidProfile->D8[pidIndex] = pid.d;
// switch to the other direction and start a new cycle
startNewCycle();
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleResult_t eventData;
if (++cycleCount == 3) {
// switch to testing I value
cycleCount = 0;
eventData.overshot = secondPeakAngle > targetAngleAtPeak ? 1 : 0;
eventData.p = pidProfile->P8[pidIndex];
eventData.i = pidProfile->I8[pidIndex];
eventData.d = pidProfile->D8[pidIndex];
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
if (cycle == CYCLE_TUNE_PD2) {
// switch to testing I value
cycle = CYCLE_TUNE_I;
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
} else {
cycle = CYCLE_TUNE_PD2;
}
// switch to the other direction for the new cycle
startNewCycle();
}
}
@ -344,7 +408,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
}
rising = true;
cycleCount = 1;
cycle = CYCLE_TUNE_PD;
secondPeakAngle = firstPeakAngle = 0;
pidProfile = pidProfileToTune;
@ -360,6 +424,10 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
pidProfile->D8[pidIndex] = pid.d;
pidProfile->I8[pidIndex] = 0;
#ifdef BLACKBOX
autotuneLogCycleStart();
#endif
}
void autotuneEndPhase(void)

40
src/main/flight/flight.c
View file

@ -57,10 +57,10 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
@ -97,18 +97,42 @@ bool shouldAutotune(void)
#endif
static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, errorAngle, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastGyroRate[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
float horizonLevelStrength = 1;
dT = (float)cycleTime * 0.000001f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
if(abs(stickPosAil) > abs(stickPosEle)){
mostDeflectedPos = abs(stickPosAil);
}
else {
mostDeflectedPos = abs(stickPosEle);
}
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
if(pidProfile->H_sensitivity == 0){
horizonLevelStrength = 0;
} else {
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
@ -139,7 +163,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level;
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
}
}
}
@ -181,8 +205,10 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int axis, prop;
int32_t error, errorAngle;
int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
@ -264,8 +290,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#define GYRO_I_MAX 256
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim)
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
int32_t errorAngle;
int axis;
int32_t delta, deltaSum;

23
src/main/flight/flight.h
View file

@ -42,6 +42,7 @@ typedef struct pidProfile_s {
float D_f[3];
float A_level;
float H_level;
uint8_t H_sensitivity;
} pidProfile_t;
typedef enum {
@ -60,28 +61,6 @@ typedef enum {
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
typedef struct fp_vector {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
typedef struct fp_angles {
float roll;
float pitch;
float yaw;
} fp_angles_def;
typedef union {
float raw[3];
fp_angles_def angles;
} fp_angles_t;
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;
int16_t pitch;

91
src/main/flight/imu.c
View file

@ -79,28 +79,28 @@ imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
accDeadband_t *accDeadband;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
{
imuRuntimeConfig = initialImuRuntimeConfig;
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
}
void imuInit()
void initIMU()
{
smallAngle = lrintf(acc_1G * cosf(RAD * imuRuntimeConfig->small_angle));
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
accVelScale = 9.80665f / acc_1G / 10000.0f;
gyroScaleRad = gyro.scale * (M_PI / 180.0f) * 0.000001f;
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
}
void calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle);
throttleAngleScale = (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
}
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
{
fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
fc_acc = 0.5f / (M_PIf * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
@ -145,56 +145,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
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, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->angles.roll);
sinx = sinf(delta->angles.roll);
cosy = cosf(delta->angles.pitch);
siny = sinf(delta->angles.pitch);
cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = cosz * cosy;
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];
v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}
void accSum_reset(void)
{
accSum[0] = 0;
@ -248,7 +198,13 @@ void acc_calc(uint32_t deltaT)
accSumCount++;
}
// baseflight calculation by Luggi09 originates from arducopter
/*
* Baseflight calculation by Luggi09 originates from arducopter
* ============================================================
*
* Calculate the heading of the craft (in degrees clockwise from North)
* when given a 3-vector representing the direction of North.
*/
static int16_t calculateHeading(t_fp_vector *vec)
{
int16_t head;
@ -259,8 +215,12 @@ static int16_t calculateHeading(t_fp_vector *vec)
float sinePitch = sinf(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
// or handle the case in which they are and (atan2f(0, 0) is undefined.
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
head = lrintf(hd);
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
if (head < 0)
head += 360;
@ -318,8 +278,8 @@ static void getEstimatedAttitude(void)
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, &deltaGyroAngle);
@ -338,16 +298,21 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame
}
// correction of throttle in lateral wind,
// Correction of throttle in lateral wind.
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
if (cosZ <= 0.015f) { // we are inverted, vertical or with a small angle < 0.86 deg
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (cosZ <= 0.015f) {
return 0;
}
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f)));
}

2
src/main/flight/imu.h
View file

@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s {
int8_t small_angle;
} imuRuntimeConfig_t;
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void configureIMU(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
void calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);

2
src/main/flight/navigation.c
View file

@ -163,7 +163,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// Low pass filter cut frequency for derivative calculation
// Set to "1 / ( 2 * PI * gps_lpf )
float pidFilter = (1.0f / (2.0f * M_PI * (float)gpsProfile->gps_lpf));
float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);

201
src/main/io/ledstrip.c
View file

@ -48,9 +48,14 @@
#include "io/ledstrip.h"
static bool ledStripInitialised = false;
static bool ledStripEnabled = true;
static failsafe_t* failsafe;
static void ledStripDisable(void);
//#define USE_LED_ANIMATION
//#define USE_LED_RING_DEFAULT_CONFIG
// timers
#ifdef USE_LED_ANIMATION
@ -59,6 +64,7 @@ static uint32_t nextAnimationUpdateAt = 0;
static uint32_t nextIndicatorFlashAt = 0;
static uint32_t nextWarningFlashAt = 0;
static uint32_t nextRotationUpdateAt = 0;
#define LED_STRIP_20HZ ((1000 * 1000) / 20)
#define LED_STRIP_10HZ ((1000 * 1000) / 10)
@ -221,23 +227,44 @@ static const modeColorIndexes_t baroModeColors = {
uint8_t ledGridWidth;
uint8_t ledGridHeight;
uint8_t ledCount;
uint8_t ledsInRingCount;
ledConfig_t *ledConfigs;
hsvColor_t *colors;
#ifdef USE_LED_RING_DEFAULT_CONFIG
const ledConfig_t defaultLedStripConfig[] = {
{ CALCULATE_LED_XY( 2, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 2, 1), LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 2, 0), LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 1, 0), LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
{ CALCULATE_LED_XY( 0, 0), LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 0, 1), LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 0, 2), LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 1, 2), LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING},
{ CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING},
};
#else
const ledConfig_t defaultLedStripConfig[] = {
{ CALCULATE_LED_XY( 2, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 2, 1), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 2, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 1, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE },
{ CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 0, 1), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 0, 2), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE },
{ CALCULATE_LED_XY( 1, 2), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
{ CALCULATE_LED_XY( 1, 1), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING },
};
#endif
/*
@ -254,12 +281,13 @@ typedef enum {
X_COORDINATE,
Y_COORDINATE,
DIRECTIONS,
FUNCTIONS
FUNCTIONS,
RING_COLORS
} parseState_e;
#define PARSE_STATE_COUNT 4
#define PARSE_STATE_COUNT 5
static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':', '\0' };
static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' };
static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' };
#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0]))
@ -272,14 +300,15 @@ static const uint8_t directionMappings[DIRECTION_COUNT] = {
LED_DIRECTION_DOWN
};
static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T' };
static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R' };
#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0]))
static const uint16_t functionMappings[FUNCTION_COUNT] = {
LED_FUNCTION_INDICATOR,
LED_FUNCTION_WARNING,
LED_FUNCTION_FLIGHT_MODE,
LED_FUNCTION_ARM_STATE,
LED_FUNCTION_THROTTLE
LED_FUNCTION_THROTTLE,
LED_FUNCTION_THRUST_RING
};
// grid offsets
@ -323,17 +352,28 @@ void determineOrientationLimits(void)
void updateLedCount(void)
{
const ledConfig_t *ledConfig;
uint8_t ledIndex;
ledCount = 0;
ledsInRingCount = 0;
for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) {
if (ledConfigs[ledIndex].flags == 0 && ledConfigs[ledIndex].xy == 0) {
ledConfig = &ledConfigs[ledIndex];
if (ledConfig->flags == 0 && ledConfig->xy == 0) {
break;
}
ledCount++;
if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) {
ledsInRingCount++;
}
}
}
static void reevalulateLedConfig(void)
void reevalulateLedConfig(void)
{
updateLedCount();
determineLedStripDimensions();
@ -406,6 +446,15 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config)
}
}
break;
case RING_COLORS:
if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) {
ledConfig->color = atoi(chunk);
} else {
ledConfig->color = 0;
}
break;
default :
break;
}
parseState++;
@ -429,6 +478,7 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz
char directions[DIRECTION_COUNT];
uint8_t index;
uint8_t mappingIndex;
ledConfig_t *ledConfig = &ledConfigs[ledIndex];
memset(ledConfigBuffer, 0, bufferSize);
@ -447,7 +497,7 @@ void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSiz
}
}
sprintf(ledConfigBuffer, "%u,%u:%s:%s", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions);
sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color);
}
void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors)
@ -525,7 +575,9 @@ void applyLedModeLayer(void)
ledConfig = &ledConfigs[ledIndex];
setLedHsv(ledIndex, &hsv_black);
if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) {
setLedHsv(ledIndex, &hsv_black);
}
if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) {
if (ledConfig->flags & LED_FUNCTION_ARM_STATE) {
@ -678,7 +730,7 @@ void applyLedThrottleLayer()
uint8_t ledIndex;
for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
ledConfig = &ledConfigs[ledIndex];
if(!(ledConfig->flags & LED_FUNCTION_THROTTLE)) {
if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) {
continue;
}
@ -691,15 +743,63 @@ void applyLedThrottleLayer()
}
}
#ifdef USE_LED_ANIMATION
static uint8_t frameCounter = 0;
#define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off
#define ROTATION_SEQUENCE_LED_WIDTH 2
static uint8_t previousRow;
static uint8_t currentRow;
static uint8_t nextRow;
static void updateLedAnimationState(void)
void applyLedThrustRingLayer(void)
{
uint8_t ledIndex;
static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT;
bool nextLedOn = false;
hsvColor_t ringColor;
const ledConfig_t *ledConfig;
uint8_t ledRingIndex = 0;
for (ledIndex = 0; ledIndex < ledCount; ledIndex++) {
ledConfig = &ledConfigs[ledIndex];
if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) {
continue;
}
bool applyColor = false;
if (ARMING_FLAG(ARMED)) {
if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) {
applyColor = true;
}
} else {
if (nextLedOn == false) {
applyColor = true;
}
nextLedOn = !nextLedOn;
}
if (applyColor) {
ringColor = colors[ledConfig->color];
} else {
ringColor = hsv_black;
}
setLedHsv(ledIndex, &ringColor);
ledRingIndex++;
}
rotationPhase--;
if (rotationPhase == 0) {
rotationPhase = ROTATION_SEQUENCE_LED_COUNT;
}
}
#ifdef USE_LED_ANIMATION
void updateLedAnimationState(void)
{
static uint8_t frameCounter = 0;
static uint8_t previousRow;
static uint8_t currentRow;
static uint8_t nextRow;
uint8_t animationFrames = ledGridHeight;
previousRow = (frameCounter + animationFrames - 1) % animationFrames;
@ -737,19 +837,36 @@ static void applyLedAnimationLayer(void)
void updateLedStrip(void)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;
}
} else {
ledStripEnabled = true;
}
if (!ledStripEnabled){
return;
}
uint32_t now = micros();
bool indicatorFlashNow = indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L;
bool warningFlashNow =warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
bool warningFlashNow = warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L;
bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L;
#ifdef USE_LED_ANIMATION
bool animationUpdateNow = animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L;
#endif
if (!(
indicatorFlashNow ||
rotationUpdateNow ||
warningFlashNow
#ifdef USE_LED_ANIMATION
|| animationUpdateNow
@ -794,10 +911,22 @@ void updateLedStrip(void)
nextAnimationUpdateAt = now + LED_STRIP_20HZ;
updateLedAnimationState();
}
applyLedAnimationLayer();
#endif
if (rotationUpdateNow) {
applyLedThrustRingLayer();
uint8_t animationSpeedScale = 1;
if (ARMING_FLAG(ARMED)) {
animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10);
}
nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale;
}
ws2811UpdateStrip();
}
@ -817,6 +946,7 @@ bool parseColor(uint8_t index, const char *colorConfig)
if (val > HSV_HUE_MAX) {
ok = false;
continue;
}
colors[index].h = val;
break;
@ -883,4 +1013,11 @@ void ledStripEnable(void)
ws2811LedStripInit();
}
static void ledStripDisable(void)
{
setStripColor(&hsv_black);
ws2811UpdateStrip();
}
#endif

29
src/main/io/ledstrip.h
View file

@ -18,6 +18,7 @@
#pragma once
#define MAX_LED_STRIP_LENGTH 32
#define CONFIGURABLE_COLOR_COUNT 16
#define LED_X_BIT_OFFSET 4
#define LED_Y_BIT_OFFSET 0
@ -30,6 +31,7 @@
#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET)
#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET)
#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y))
typedef enum {
@ -44,27 +46,43 @@ typedef enum {
LED_FUNCTION_WARNING = (1 << 7),
LED_FUNCTION_FLIGHT_MODE = (1 << 8),
LED_FUNCTION_ARM_STATE = (1 << 9),
LED_FUNCTION_THROTTLE = (1 << 10)
LED_FUNCTION_THROTTLE = (1 << 10),
LED_FUNCTION_THRUST_RING = (1 << 11),
} ledFlag_e;
#define LED_DIRECTION_BIT_OFFSET 0
#define LED_DIRECTION_MASK 0x3F
#define LED_DIRECTION_MASK ( \
LED_DIRECTION_NORTH | \
LED_DIRECTION_EAST | \
LED_DIRECTION_SOUTH | \
LED_DIRECTION_WEST | \
LED_DIRECTION_UP | \
LED_DIRECTION_DOWN \
)
#define LED_FUNCTION_BIT_OFFSET 6
#define LED_FUNCTION_MASK 0x7C0
#define LED_FUNCTION_MASK ( \
LED_FUNCTION_INDICATOR | \
LED_FUNCTION_WARNING | \
LED_FUNCTION_FLIGHT_MODE | \
LED_FUNCTION_ARM_STATE | \
LED_FUNCTION_THROTTLE | \
LED_FUNCTION_THRUST_RING \
)
typedef struct ledConfig_s {
uint8_t xy; // see LED_X/Y_MASK defines
uint8_t xy; // see LED_X/Y_MASK defines
uint8_t color; // see colors (config_master)
uint16_t flags; // see ledFlag_e
} ledConfig_t;
extern uint8_t ledCount;
#define CONFIGURABLE_COLOR_COUNT 16
bool parseLedStripConfig(uint8_t ledIndex, const char *config);
void updateLedStrip(void);
void updateLedRing(void);
void applyDefaultLedStripConfig(ledConfig_t *ledConfig);
void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize);
@ -73,4 +91,5 @@ bool parseColor(uint8_t index, const char *colorConfig);
void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount);
void ledStripEnable(void);
void reevalulateLedConfig(void);

4
src/main/io/rc_controls.c
View file

@ -517,6 +517,10 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges)
}
}
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return min(abs(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
{
uint8_t index;

2
src/main/io/rc_controls.h
View file

@ -214,3 +214,5 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
bool isUsingSticksForArming(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);

4
src/main/io/serial.c
View file

@ -73,7 +73,9 @@ static serialPort_t *serialPorts[SERIAL_PORT_COUNT];
#ifdef STM32F303xC
static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
#ifdef USE_VCP
{SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
#endif
{SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
{SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
#if (SERIAL_PORT_COUNT > 3)
@ -85,7 +87,9 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
};
const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
#ifdef USE_VCP
{SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE },
#endif
{SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
{SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
#if (SERIAL_PORT_COUNT > 3)

1
src/main/io/serial_cli.c
View file

@ -393,6 +393,7 @@ const clivalue_t valueTable[] = {
{ "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 },
{ "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 },
{ "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 },
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 },

50
src/main/io/serial_msp.c
View file

@ -121,7 +121,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 2 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 4 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
@ -617,6 +617,12 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
#ifdef LED_STRIP
if (feature(FEATURE_LED_STRIP)) {
activeBoxIds[activeBoxIdCount++] = BOXLEDLOW;
}
#endif
if (feature(FEATURE_INFLIGHT_ACC_CAL))
activeBoxIds[activeBoxIdCount++] = BOXCALIB;
@ -746,6 +752,7 @@ static bool processOutCommand(uint8_t cmdMSP)
IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
@ -846,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
if (i == PIDLEVEL) {
serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250));
serialize8(0);
serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250));
} else {
serialize8(currentProfile->pidProfile.P8[i]);
serialize8(currentProfile->pidProfile.I8[i]);
@ -1097,13 +1104,14 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_LED_STRIP_CONFIG:
headSerialReply(MAX_LED_STRIP_LENGTH * 6);
headSerialReply(MAX_LED_STRIP_LENGTH * 7);
for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET);
serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET);
serialize8(GET_LED_X(ledConfig));
serialize8(GET_LED_Y(ledConfig));
serialize8(ledConfig->color);
}
break;
#endif
@ -1171,7 +1179,7 @@ static bool processInCommand(void)
if (i == PIDLEVEL) {
currentProfile->pidProfile.A_level = (float)read8() / 10.0f;
currentProfile->pidProfile.H_level = (float)read8() / 10.0f;
read8();
currentProfile->pidProfile.H_sensitivity = read8();
} else {
currentProfile->pidProfile.P8[i] = read8();
currentProfile->pidProfile.I8[i] = read8();
@ -1442,28 +1450,30 @@ static bool processInCommand(void)
case MSP_SET_LED_STRIP_CONFIG:
{
uint8_t ledCount = currentPort->dataSize / 6;
if (ledCount != MAX_LED_STRIP_LENGTH) {
i = read8();
if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) {
headSerialError(0);
break;
}
for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
uint16_t mask;
// currently we're storing directions and functions in a uint16 (flags)
// the msp uses 2 x uint16_t to cater for future expansion
mask = read16();
ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
uint16_t mask;
// currently we're storing directions and functions in a uint16 (flags)
// the msp uses 2 x uint16_t to cater for future expansion
mask = read16();
ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
mask = read16();
ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
mask = read16();
ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
mask = read8();
ledConfig->xy = CALCULATE_LED_X(mask);
mask = read8();
ledConfig->xy = CALCULATE_LED_X(mask);
mask = read8();
ledConfig->xy |= CALCULATE_LED_Y(mask);
}
mask = read8();
ledConfig->xy |= CALCULATE_LED_Y(mask);
ledConfig->color = read8();
reevalulateLedConfig();
}
break;
#endif

13
src/main/main.c
View file

@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void);
void initIMU(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
void loop(void);
@ -206,17 +206,20 @@ void init(void)
#endif
#ifdef USE_I2C
#ifdef NAZE
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
// Configure the rest of the stuff
i2cInit(I2C_DEVICE);
#endif
#endif
#if !defined(SPARKY)
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
@ -264,7 +267,7 @@ void init(void)
LED0_OFF;
LED1_OFF;
imuInit();
initIMU();
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG

9
src/main/mw.c
View file

@ -97,7 +97,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
extern failsafe_t *failsafe;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
extern pidControllerFuncPtr pid_controller;
@ -311,9 +312,6 @@ void mwDisarm(void)
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
finishBlackbox();
if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) {
mspAllocateSerialPorts(&masterConfig.serialConfig);
}
}
#endif
}
@ -712,7 +710,8 @@ void loop(void)
&currentProfile->pidProfile,
currentControlRateProfile,
masterConfig.max_angle_inclination,
&currentProfile->accelerometerTrims
&currentProfile->accelerometerTrims,
&masterConfig.rxConfig
);
mixTable();

2
src/main/rx/rx.c
View file

@ -292,7 +292,7 @@ void processRxChannels(void)
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
failsafe->vTable->checkPulse(rawChannel, sample);
failsafe->vTable->checkPulse(chan, sample);
}
// validate the range

2
src/main/rx/sbus.c
View file

@ -121,7 +121,7 @@ static void sbusDataReceive(uint16_t c)
static uint8_t sbusFramePosition = 0;
static uint32_t sbusTimeoutAt = 0;
uint32_t now = millis();
uint32_t now = micros();
if ((int32_t)(sbusTimeoutAt - now) < 0) {
sbusFramePosition = 0;

7
src/main/sensors/battery.c
View file

@ -31,7 +31,8 @@ uint16_t batteryWarningVoltage;
uint16_t batteryCriticalVoltage;
uint8_t vbat = 0; // battery voltage in 0.1V steps
uint16_t vbatLatest = 0; // most recent unsmoothed raw reading from vbat adc
uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC
uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
@ -55,7 +56,7 @@ void updateBatteryVoltage(void)
uint16_t vbatSampleTotal = 0;
// store the battery voltage with some other recent battery voltage readings
vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY);
vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatestADC = adcGetChannel(ADC_BATTERY);
// calculate vbat based on the average of recent readings
for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) {
@ -114,7 +115,7 @@ void updateCurrentMeter(int32_t lastUpdateAt)
static int64_t mAhdrawnRaw = 0;
amperageRaw -= amperageRaw / 8;
amperageRaw += adcGetChannel(ADC_CURRENT);
amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));
amperage = currentSensorToCentiamps(amperageRaw / 8);
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;

3
src/main/sensors/battery.h
View file

@ -42,9 +42,10 @@ typedef enum {
} batteryState_e;
extern uint8_t vbat;
extern uint16_t vbatLatest;
extern uint16_t vbatLatestADC;
extern uint8_t batteryCellCount;
extern uint16_t batteryWarningVoltage;
extern uint16_t amperageLatestADC;
extern int32_t amperage;
extern int32_t mAhDrawn;

24
src/main/sensors/initialisation.c
View file

@ -94,6 +94,15 @@ const mpu6050Config_t *selectMPU6050Config(void)
return &nazeRev5MPU6050Config;
}
#endif
#ifdef SPRACINGF3
static const mpu6050Config_t spRacingF3MPU6050Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13
};
return &spRacingF3MPU6050Config;
#endif
return NULL;
}
@ -164,8 +173,8 @@ bool detectGyro(uint16_t gyroLpf)
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_GYRO_L3GD20_ALIGN
gyroAlign = GYRO_GYRO_L3GD20_ALIGN;
#ifdef GYRO_L3GD20_ALIGN
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
return true;
}
@ -406,6 +415,17 @@ static void detectMag(uint8_t magHardwareToUse)
hmc5883Config = &nazeHmc5883Config;
#endif
#ifdef SPRACINGF3
hmc5883Config_t spRacingF3Hmc5883Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPin = Pin_14,
.gpioPort = GPIOC
};
hmc5883Config = &spRacingF3Hmc5883Config;
#endif
#endif
retry:

34
src/main/target/CC3D/target.h
View file

@ -35,6 +35,8 @@
#define MPU6000_CS_PIN GPIO_Pin_4
#define MPU6000_SPI_INSTANCE SPI1
#define USABLE_TIMER_CHANNEL_COUNT 12
#define GYRO
#define USE_GYRO_SPI_MPU6000
@ -45,8 +47,18 @@
#define ACC_SPI_MPU6000_ALIGN CW270_DEG
// External I2C BARO
#define BARO
#define USE_BARO_MS5611
#define USE_BARO_BMP085
// External I2C MAG
#define MAG
#define USE_MAG_HMC5883
#define INVERTER
#define BEEPER
#define DISPLAY
#define USE_USART1
#define USE_USART3
@ -57,10 +69,6 @@
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3
#define CurrentMeter_TIMER 3 // PWM4
#define Vbat_TIMER 4 // PWM5
#define RSSI_TIMER 5 // PWM6
#define USART3_RX_PIN Pin_11
#define USART3_TX_PIN Pin_10
#define USART3_GPIO GPIOB
@ -71,6 +79,24 @@
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11
#define USE_ADC
#define CURRENT_METER_ADC_GPIO GPIOB
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
#define VBAT_ADC_CHANNEL ADC_Channel_0
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define SENSORS_SET (SENSOR_ACC)
#define GPS

5
src/main/target/CHEBUZZF3/target.h
View file

@ -33,10 +33,13 @@
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE
#define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 18
#define GYRO
#define USE_GYRO_L3GD20
#define USE_GYRO_MPU6050
#define GYRO_L3GD20_ALIGN CW90_DEG
#define GYRO_MPU6050_ALIGN CW0_DEG
#define ACC
@ -65,6 +68,8 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define USE_ADC
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS

18
src/main/target/EUSTM32F103RC/target.h
View file

@ -98,6 +98,24 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define USE_ADC
#define CURRENT_METER_ADC_GPIO GPIOB
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_4
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define EXTERNAL1_ADC_GPIO GPIOA
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS

63
src/main/target/MASSIVEF3/target.h
View file

@ -1,63 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "MF3A"
#define LED0_GPIO GPIOE
#define LED0_PIN Pin_8|Pin_12 // Blue LEDs - PE8/PE12
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOE
#define LED0_INVERTED
#define LED1_GPIO GPIOE
#define LED1_PIN Pin_10|Pin_14 // Orange LEDs - PE10/PE14
#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOE
#define LED1_INVERTED
#define BEEP_GPIO GPIOE
#define BEEP_PIN Pin_9|Pin_13 // Red LEDs - PE9/PE13
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOE
#define BEEPER_INVERTED
#define BEEPER_INVERTED
#define GYRO
#define ACC
#define BARO
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define BEEPER
#define LED0
#define LED1
#define USE_VCP
#define USE_USART1
#define USE_USART2
#define SERIAL_PORT_COUNT 3
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE

17
src/main/target/NAZE/target.h
View file

@ -117,6 +117,23 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define USE_ADC
#define CURRENT_METER_ADC_GPIO GPIOB
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_4
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define EXTERNAL1_ADC_GPIO GPIOA
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)

19
src/main/target/OLIMEXINO/target.h
View file

@ -85,6 +85,25 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define USE_ADC
#define CURRENT_METER_ADC_GPIO GPIOB
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_4
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define EXTERNAL1_ADC_GPIO GPIOA
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS

18
src/main/target/PORT103R/target.h
View file

@ -95,6 +95,24 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define USE_ADC
#define CURRENT_METER_ADC_GPIO GPIOB
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_9
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_4
#define RSSI_ADC_GPIO GPIOA
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define EXTERNAL1_ADC_GPIO GPIOA
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define LED0

2
src/main/target/SPARKY/target.h
View file

@ -26,6 +26,8 @@
#define LED1_PIN Pin_5 // Green LEDs - PB5
#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB
#define USABLE_TIMER_CHANNEL_COUNT 11
// MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component.
#define GYRO
#define USE_GYRO_MPU6050

4
src/main/target/MASSIVEF3/system_stm32f30x.c → src/main/target/SPRACINGF3/system_stm32f30x.c
View file

@ -55,9 +55,9 @@
*-----------------------------------------------------------------------------
* APB1 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 12000000
* HSE Frequency(Hz) | 8000000
*----------------------------------------------------------------------------
* PLLMUL | 6
* PLLMUL | 9
*-----------------------------------------------------------------------------
* PREDIV | 1
*-----------------------------------------------------------------------------

0
src/main/target/MASSIVEF3/system_stm32f30x.h → src/main/target/SPRACINGF3/system_stm32f30x.h
View file

86
src/main/target/SPRACINGF3/target.h Normal file
View file

@ -0,0 +1,86 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "SRF3"
#define LED0_GPIO GPIOB
#define LED0_PIN Pin_3
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define USABLE_TIMER_CHANNEL_COUNT 17
#define GYRO
#define USE_GYRO_MPU6050
#define ACC
#define USE_ACC_MPU6050
#define BARO
#define USE_BARO_MS5611
#define MAG
#define USE_MAG_HMC5883
#define BEEPER
#define LED0
#define USE_USART1
#define USE_USART2
#define USE_USART3
#define SERIAL_PORT_COUNT 3
#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK
#define UART2_RX_PIN GPIO_Pin_15 // PA15
#define UART2_GPIO GPIOA
#define UART2_GPIO_AF GPIO_AF_7
#define UART2_TX_PINSOURCE GPIO_PinSource14
#define UART2_RX_PINSOURCE GPIO_PinSource15
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
//#define USE_SPI
//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_GPIO GPIOA
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
#define WS2811_GPIO_AF GPIO_AF_6
#define WS2811_PIN GPIO_Pin_8
#define WS2811_PIN_SOURCE GPIO_PinSource8
#define WS2811_TIMER TIM1
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE

9
src/main/target/STM32F3DISCOVERY/target.h
View file

@ -39,9 +39,14 @@
#define GYRO
#define USE_GYRO_L3GD20
#define GYRO_L3GD20_ALIGN CW90_DEG
#define ACC
#define USE_ACC_LSM303DLHC
#define MAG
#define USE_MAG_HMC5883
#define BEEPER
#define LED0
#define LED1
@ -54,7 +59,9 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define SENSORS_SET (SENSOR_ACC)
#define USE_ADC
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define BLACKBOX
#define GPS

2
src/main/target/stm32_flash_f303_128k.ld
View file

@ -22,7 +22,7 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
/* Specify the memory areas */
MEMORY
{
FLASH (rx) : ORIGIN = 0x04000000, LENGTH = 126K /* last 2kb used for config storage */
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}

2
src/main/version.h
View file

@ -16,7 +16,7 @@
*/
#define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define MW_VERSION 231

1
src/test/Makefile
View file

@ -149,6 +149,7 @@ flight_imu_unittest : \
$(OBJECT_DIR)/flight/imu.o \
$(OBJECT_DIR)/flight/altitudehold.o \
$(OBJECT_DIR)/flight_imu_unittest.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@

10
src/test/unit/flight_imu_unittest.cc
View file

@ -85,18 +85,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
UNUSED(rollAndPitchTrims);
}
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }
int constrain(int amt, int low, int high)
{
UNUSED(amt);
UNUSED(low);
UNUSED(high);
return 0;
}
}