mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 04:15:44 +03:00
Merge remote-tracking branch 'upstream/master' into blackbox-flash
This commit is contained in:
Nicholas Sherlock
commit
ebff1bdcd7
18 changed files with 290 additions and 84 deletions
|
|
@ -31,13 +31,13 @@ strength of the correction to be backed off in order to avoid overshooting the t
|
|||
|
||||
## PID controllers
|
||||
|
||||
Cleanflight has three built-in PID controllers which each have different flight behavior. Each controller requires
|
||||
Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires
|
||||
different PID settings for best performance, so if you tune your craft using one PID controller, those settings will
|
||||
likely not work well on any of the other controllers.
|
||||
|
||||
You can change between PID controllers by running `set pid_controller = X` on the CLI tab of the Cleanflight
|
||||
Configurator, where X is the number of the controller you want to use. Please read these notes first before trying one
|
||||
out!
|
||||
You can change between PID controllers by running `set pid_controller=n` on the CLI tab of the Cleanflight
|
||||
Configurator, where `n` is the number of the controller you want to use. Please read these notes first before trying one
|
||||
out.
|
||||
|
||||
### PID controller 0, "MultiWii" (default)
|
||||
|
||||
|
|
@ -71,7 +71,7 @@ need to be increased in order to perform like PID controller 0.
|
|||
|
||||
The LEVEL "D" setting is not used by this controller.
|
||||
|
||||
### PID controller 2, "Baseflight"
|
||||
### PID controller 2, "LuxFloat"
|
||||
|
||||
PID Controller 2 is Lux's new floating point PID controller. Both controller 0 and 1 use integer arithmetic, which was
|
||||
faster in the days of the slower 8-bit MultiWii controllers, but is less precise.
|
||||
|
|
@ -82,10 +82,7 @@ don't have to be retuned when the looptime setting changes.
|
|||
There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by
|
||||
nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it.
|
||||
|
||||
Even though PC2 is called "pidBaseflight" in the code, it was never in Baseflight or MultiWii. A better name might have
|
||||
been pidFloatingPoint, or pidCleanflight. It is the first PID Controller designed for 32-bit processors and not derived
|
||||
from MultiWii. I believe it was named pidBaseflight because it was to be the first true 32-bit processor native PID
|
||||
controller, and thus the native Baseflight PC, but Timecop never accepted the code into Baseflight.
|
||||
It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
|
||||
|
||||
The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
|
||||
is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
|
||||
|
|
@ -110,7 +107,7 @@ stick, and no self-leveling will be applied at 75% stick and onwards.
|
|||
|
||||
PID Controller 3 is an direct port of the PID controller from MultiWii 2.3 and later.
|
||||
|
||||
The algorithm is handling roll and pitch differently to yaw. users with problems on yaw authority should try this one.
|
||||
The algorithm is handling roll and pitch differently to yaw. Users with problems on yaw authority should try this one.
|
||||
|
||||
For the ALIENWII32 targets the gyroscale is removed for even more yaw authority. This will provide best performance on very small multicopters with brushed motors.
|
||||
|
||||
|
|
@ -118,6 +115,22 @@ For the ALIENWII32 targets the gyroscale is removed for even more yaw authority.
|
|||
|
||||
PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and pitch is using the MultiWii 2.2 algorithm and yaw is using the 2.3 algorithm.
|
||||
|
||||
This PID controller was initialy implementd for testing purposes but is also performing quite well.
|
||||
This PID controller was initialy implemented for testing purposes but is also performing quite well.
|
||||
|
||||
For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors.
|
||||
|
||||
### PID controller 5, "Harakiri"
|
||||
|
||||
PID Controller 5 is an port of the PID controller from the Harakiri firmware.
|
||||
|
||||
The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don<6F>t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri:
|
||||
|
||||
OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
|
||||
MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
|
||||
|
||||
The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.
|
||||
|
||||
Yaw authority is also quite good.
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -720,6 +720,12 @@ void validateAndFixConfig(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
|
||||
if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
|
||||
featureClear(FEATURE_DISPLAY);
|
||||
}
|
||||
#endif
|
||||
|
||||
useRxConfig(&masterConfig.rxConfig);
|
||||
|
||||
serialConfig_t *serialConfig = &masterConfig.serialConfig;
|
||||
|
|
|
|||
|
|
@ -234,9 +234,7 @@ static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca
|
|||
pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
|
||||
|
||||
if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
|
||||
if (pwmInputPort->state == 0) {
|
||||
captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
|
||||
}
|
||||
captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
|
||||
pwmInputPort->missedEvents = 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -47,13 +47,20 @@ static uartPort_t uartPort2;
|
|||
static uartPort_t uartPort3;
|
||||
#endif
|
||||
|
||||
// Using RX DMA disables the use of receive callbacks
|
||||
#define USE_USART1_RX_DMA
|
||||
|
||||
#if defined(CC3D) // FIXME move board specific code to target.h files.
|
||||
#undef USE_USART1_RX_DMA
|
||||
#endif
|
||||
|
||||
void uartStartTxDMA(uartPort_t *s);
|
||||
|
||||
void usartIrqCallback(uartPort_t *s)
|
||||
{
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_RXNE) {
|
||||
if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) {
|
||||
// If we registered a callback, pass crap there
|
||||
if (s->port.callback) {
|
||||
s->port.callback(s->USARTx->DR);
|
||||
|
|
@ -98,11 +105,13 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
|
|||
|
||||
s->USARTx = USART1;
|
||||
|
||||
s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
|
||||
#ifdef USE_USART1_RX_DMA
|
||||
s->rxDMAChannel = DMA1_Channel5;
|
||||
s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
#endif
|
||||
s->txDMAChannel = DMA1_Channel4;
|
||||
s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||
|
|
@ -129,6 +138,15 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
|
|||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
#ifndef USE_USART1_RX_DMA
|
||||
// RX/TX Interrupt
|
||||
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
#endif
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
|
|
@ -146,20 +164,11 @@ void DMA1_Channel4_IRQHandler(void)
|
|||
s->txDMAEmpty = true;
|
||||
}
|
||||
|
||||
// USART1 Tx IRQ Handler
|
||||
// USART1 Rx/Tx IRQ Handler
|
||||
void USART1_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort1;
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_TXE) {
|
||||
if (s->port.txBufferTail != s->port.txBufferHead) {
|
||||
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
|
||||
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
|
||||
} else {
|
||||
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
|
||||
}
|
||||
}
|
||||
usartIrqCallback(s);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -53,7 +53,6 @@ uint8_t velocityControl = 0;
|
|||
int32_t errorVelocityI = 0;
|
||||
int32_t altHoldThrottleAdjustment = 0;
|
||||
int32_t AltHold;
|
||||
int32_t EstAlt; // in cm
|
||||
int32_t vario = 0; // variometer in cm/s
|
||||
|
||||
|
||||
|
|
@ -78,7 +77,7 @@ void configureAltitudeHold(
|
|||
#if defined(BARO) || defined(SONAR)
|
||||
|
||||
static int16_t initialThrottleHold;
|
||||
|
||||
static int32_t EstAlt; // in cm
|
||||
|
||||
// 40hz update rate (20hz LPF on acc)
|
||||
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
|
||||
|
|
@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
|
|||
float vel_acc;
|
||||
int32_t vel_tmp;
|
||||
float accZ_tmp;
|
||||
int32_t sonarAlt = -1;
|
||||
static float accZ_old = 0.0f;
|
||||
static float vel = 0.0f;
|
||||
static float accAlt = 0.0f;
|
||||
|
|
@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
|
|||
int16_t tiltAngle;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
dTime = currentTime - previousTime;
|
||||
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
|
||||
return;
|
||||
|
|
@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
|
|||
|
||||
accZ_old = accZ_tmp;
|
||||
}
|
||||
|
||||
int32_t altitudeHoldGetEstimatedAltitude(void)
|
||||
{
|
||||
return EstAlt;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -15,14 +15,16 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
//extern int32_t errorVelocityI;
|
||||
//extern int32_t altHoldThrottleAdjustment;
|
||||
//extern uint8_t velocityControl;
|
||||
//extern int32_t setVelocity;
|
||||
#include "flight/flight.h"
|
||||
|
||||
#include "io/escservo.h"
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "sensors/barometer.h"
|
||||
|
||||
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
|
||||
void applyAltHold(airplaneConfig_t *airplaneConfig);
|
||||
void updateAltHoldState(void);
|
||||
void updateSonarAltHoldState(void);
|
||||
|
||||
int32_t altitudeHoldGetEstimatedAltitude(void);
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ bool shouldAutotune(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
float RateError, errorAngle, AngleRate, gyroRate;
|
||||
|
|
@ -506,6 +506,136 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
|
|||
#endif
|
||||
}
|
||||
|
||||
#define RCconstPI 0.159154943092f // 0.5f / M_PI;
|
||||
#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
|
||||
#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
|
||||
|
||||
static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
|
||||
float delta, RCfactor, rcCommandAxis, MainDptCut;
|
||||
float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
|
||||
static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
|
||||
float tmp0flt;
|
||||
int32_t tmp0;
|
||||
uint8_t axis;
|
||||
float ACCDeltaTimeINS = 0;
|
||||
float FLOATcycleTime = 0;
|
||||
|
||||
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
|
||||
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
|
||||
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
|
||||
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
|
||||
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f;
|
||||
}
|
||||
|
||||
for (axis = 0; axis < 2; axis++) {
|
||||
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
|
||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
|
||||
#ifdef GPS
|
||||
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
|
||||
#else
|
||||
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
|
||||
#endif
|
||||
|
||||
#ifdef AUTOTUNE
|
||||
if (shouldAutotune()) {
|
||||
error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error)));
|
||||
}
|
||||
#endif
|
||||
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
|
||||
tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
|
||||
PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
|
||||
ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
|
||||
}
|
||||
|
||||
if (!FLIGHT_MODE(ANGLE_MODE)) {
|
||||
if (ABS((int16_t)gyroData[axis]) > 2560) {
|
||||
errorGyroI[axis] = 0.0f;
|
||||
} else {
|
||||
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
|
||||
}
|
||||
|
||||
ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
|
||||
ITerm = ITermACC + prop * (ITermGYRO - ITermACC);
|
||||
} else {
|
||||
PTerm = rcCommandAxis;
|
||||
ITerm = ITermGYRO;
|
||||
}
|
||||
} else {
|
||||
PTerm = PTermACC;
|
||||
ITerm = ITermACC;
|
||||
}
|
||||
|
||||
PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
|
||||
delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
|
||||
|
||||
lastGyro[axis] = gyroData[axis];
|
||||
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
|
||||
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
|
||||
|
||||
axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result.
|
||||
|
||||
#ifdef BLACKBOX
|
||||
axisPID_P[axis] = PTerm;
|
||||
axisPID_I[axis] = ITerm;
|
||||
axisPID_D[axis] = -DTerm;
|
||||
#endif
|
||||
}
|
||||
|
||||
tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
|
||||
tmp0flt /= 3000.0f;
|
||||
|
||||
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
|
||||
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
|
||||
tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
|
||||
PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
|
||||
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
|
||||
errorGyroI[FD_YAW] = 0;
|
||||
} else {
|
||||
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
|
||||
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
|
||||
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
|
||||
}
|
||||
} else {
|
||||
tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
|
||||
error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
|
||||
|
||||
if (ABS(tmp0) > 50) {
|
||||
errorGyroI[FD_YAW] = 0;
|
||||
} else {
|
||||
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
|
||||
}
|
||||
|
||||
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
|
||||
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
|
||||
|
||||
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
|
||||
tmp0 = 300;
|
||||
if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
|
||||
PTerm = constrain(PTerm, -tmp0, tmp0);
|
||||
}
|
||||
}
|
||||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
|
||||
|
||||
#ifdef BLACKBOX
|
||||
axisPID_P[FD_YAW] = PTerm;
|
||||
axisPID_I[FD_YAW] = ITerm;
|
||||
axisPID_D[FD_YAW] = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||
rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
||||
{
|
||||
|
|
@ -606,13 +736,16 @@ void setPIDController(int type)
|
|||
pid_controller = pidRewrite;
|
||||
break;
|
||||
case 2:
|
||||
pid_controller = pidBaseflight;
|
||||
pid_controller = pidLuxFloat;
|
||||
break;
|
||||
case 3:
|
||||
pid_controller = pidMultiWii23;
|
||||
break;
|
||||
case 4:
|
||||
pid_controller = pidMultiWiiHybrid;
|
||||
break;
|
||||
case 5:
|
||||
pid_controller = pidHarakiri;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
|||
extern int16_t heading, magHold;
|
||||
|
||||
extern int32_t AltHold;
|
||||
extern int32_t EstAlt;
|
||||
extern int32_t vario;
|
||||
|
||||
void setPIDController(int type);
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
typedef enum {
|
||||
BOXARM = 0,
|
||||
BOXANGLE,
|
||||
|
|
|
|||
|
|
@ -44,7 +44,6 @@
|
|||
|
||||
void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
|
||||
|
||||
void mspInit(serialConfig_t *serialConfig);
|
||||
void cliInit(serialConfig_t *serialConfig);
|
||||
|
||||
// this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.
|
||||
|
|
@ -92,13 +91,13 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
|
|||
{SERIAL_PORT_USB_VCP, 9600, 115200, SPF_NONE },
|
||||
#endif
|
||||
#ifdef USE_USART1
|
||||
{SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
#endif
|
||||
#ifdef USE_USART2
|
||||
{SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
#endif
|
||||
#ifdef USE_USART3
|
||||
{SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART3, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
#endif
|
||||
};
|
||||
|
||||
|
|
@ -118,8 +117,8 @@ 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_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART3, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
|
||||
};
|
||||
#else
|
||||
|
|
@ -134,7 +133,7 @@ static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
|
|||
};
|
||||
|
||||
const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
|
||||
{SERIAL_PORT_USART1, 9600, 250000, SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART1, 9600, 250000, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
{SERIAL_PORT_USART2, 9600, 250000, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
|
||||
#if (SERIAL_PORT_COUNT > 2)
|
||||
{SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
|
||||
|
|
|
|||
|
|
@ -384,7 +384,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE },
|
||||
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
|
||||
|
||||
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 },
|
||||
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 },
|
||||
|
||||
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
|
||||
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
|
||||
|
|
|
|||
|
|
@ -45,6 +45,8 @@
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "flight/altitudehold.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
#include "io/escservo.h"
|
||||
|
|
@ -55,9 +57,11 @@
|
|||
#include "io/ledstrip.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
|
|
@ -204,6 +208,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
|||
#define MSP_VOLTAGE_METER_CONFIG 56
|
||||
#define MSP_SET_VOLTAGE_METER_CONFIG 57
|
||||
|
||||
#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
|
||||
|
||||
#define MSP_PID_CONTROLLER 59
|
||||
#define MSP_SET_PID_CONTROLLER 60
|
||||
//
|
||||
// Baseflight MSP commands (if enabled they exist in Cleanflight)
|
||||
//
|
||||
|
|
@ -373,7 +381,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
|
|||
|
||||
static mspPort_t *currentPort;
|
||||
|
||||
void serialize32(uint32_t a)
|
||||
static void serialize32(uint32_t a)
|
||||
{
|
||||
static uint8_t t;
|
||||
t = a;
|
||||
|
|
@ -390,7 +398,7 @@ void serialize32(uint32_t a)
|
|||
currentPort->checksum ^= t;
|
||||
}
|
||||
|
||||
void serialize16(int16_t a)
|
||||
static void serialize16(int16_t a)
|
||||
{
|
||||
static uint8_t t;
|
||||
t = a;
|
||||
|
|
@ -401,32 +409,32 @@ void serialize16(int16_t a)
|
|||
currentPort->checksum ^= t;
|
||||
}
|
||||
|
||||
void serialize8(uint8_t a)
|
||||
static void serialize8(uint8_t a)
|
||||
{
|
||||
serialWrite(mspSerialPort, a);
|
||||
currentPort->checksum ^= a;
|
||||
}
|
||||
|
||||
uint8_t read8(void)
|
||||
static uint8_t read8(void)
|
||||
{
|
||||
return currentPort->inBuf[currentPort->indRX++] & 0xff;
|
||||
}
|
||||
|
||||
uint16_t read16(void)
|
||||
static uint16_t read16(void)
|
||||
{
|
||||
uint16_t t = read8();
|
||||
t += (uint16_t)read8() << 8;
|
||||
return t;
|
||||
}
|
||||
|
||||
uint32_t read32(void)
|
||||
static uint32_t read32(void)
|
||||
{
|
||||
uint32_t t = read16();
|
||||
t += (uint32_t)read16() << 16;
|
||||
return t;
|
||||
}
|
||||
|
||||
void headSerialResponse(uint8_t err, uint8_t responseBodySize)
|
||||
static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
|
||||
{
|
||||
serialize8('$');
|
||||
serialize8('M');
|
||||
|
|
@ -436,36 +444,36 @@ void headSerialResponse(uint8_t err, uint8_t responseBodySize)
|
|||
serialize8(currentPort->cmdMSP);
|
||||
}
|
||||
|
||||
void headSerialReply(uint8_t responseBodySize)
|
||||
static void headSerialReply(uint8_t responseBodySize)
|
||||
{
|
||||
headSerialResponse(0, responseBodySize);
|
||||
}
|
||||
|
||||
void headSerialError(uint8_t responseBodySize)
|
||||
static void headSerialError(uint8_t responseBodySize)
|
||||
{
|
||||
headSerialResponse(1, responseBodySize);
|
||||
}
|
||||
|
||||
void tailSerialReply(void)
|
||||
static void tailSerialReply(void)
|
||||
{
|
||||
serialize8(currentPort->checksum);
|
||||
}
|
||||
|
||||
void s_struct(uint8_t *cb, uint8_t siz)
|
||||
static void s_struct(uint8_t *cb, uint8_t siz)
|
||||
{
|
||||
headSerialReply(siz);
|
||||
while (siz--)
|
||||
serialize8(*cb++);
|
||||
}
|
||||
|
||||
void serializeNames(const char *s)
|
||||
static void serializeNames(const char *s)
|
||||
{
|
||||
const char *c;
|
||||
for (c = s; *c; c++)
|
||||
serialize8(*c);
|
||||
}
|
||||
|
||||
const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
|
||||
static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
|
||||
{
|
||||
uint8_t boxIndex;
|
||||
const box_t *candidate;
|
||||
|
|
@ -478,7 +486,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
|
|||
return NULL;
|
||||
}
|
||||
|
||||
const box_t *findBoxByPermenantId(uint8_t permenantId)
|
||||
static const box_t *findBoxByPermenantId(uint8_t permenantId)
|
||||
{
|
||||
uint8_t boxIndex;
|
||||
const box_t *candidate;
|
||||
|
|
@ -491,7 +499,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId)
|
|||
return NULL;
|
||||
}
|
||||
|
||||
void serializeBoxNamesReply(void)
|
||||
static void serializeBoxNamesReply(void)
|
||||
{
|
||||
int i, activeBoxId, j, flag = 1, count = 0, len;
|
||||
const box_t *box;
|
||||
|
|
@ -523,7 +531,7 @@ reset:
|
|||
}
|
||||
}
|
||||
|
||||
void serializeDataflashSummaryReply(void)
|
||||
static void serializeDataflashSummaryReply(void)
|
||||
{
|
||||
#ifdef FLASHFS
|
||||
const flashGeometry_t *geometry = flashfsGetGeometry();
|
||||
|
|
@ -538,7 +546,7 @@ void serializeDataflashSummaryReply(void)
|
|||
}
|
||||
|
||||
#ifdef FLASHFS
|
||||
void serializeDataflashReadReply(uint32_t address, uint8_t size)
|
||||
static void serializeDataflashReadReply(uint32_t address, uint8_t size)
|
||||
{
|
||||
enum { DATAFLASH_READ_REPLY_CHUNK_SIZE = 128 };
|
||||
|
||||
|
|
@ -695,7 +703,6 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
{
|
||||
uint32_t i, tmp, junk;
|
||||
|
||||
|
||||
#ifdef GPS
|
||||
uint8_t wp_no;
|
||||
int32_t lat = 0, lon = 0;
|
||||
|
|
@ -864,18 +871,30 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
break;
|
||||
case MSP_ALTITUDE:
|
||||
headSerialReply(6);
|
||||
serialize32(EstAlt);
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
serialize32(altitudeHoldGetEstimatedAltitude());
|
||||
#else
|
||||
serialize32(0);
|
||||
#endif
|
||||
serialize16(vario);
|
||||
break;
|
||||
case MSP_SONAR_ALTITUDE:
|
||||
headSerialReply(4);
|
||||
#if defined(SONAR)
|
||||
serialize32(sonarGetLatestAltitude());
|
||||
#else
|
||||
serialize32(0);
|
||||
#endif
|
||||
break;
|
||||
case MSP_ANALOG:
|
||||
headSerialReply(7);
|
||||
serialize8((uint8_t)constrain(vbat, 0, 255));
|
||||
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery
|
||||
serialize16(rssi);
|
||||
if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
|
||||
serialize16((uint16_t)constrain((ABS(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
|
||||
serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
|
||||
} else
|
||||
serialize16((uint16_t)constrain(ABS(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
|
||||
serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
|
||||
break;
|
||||
case MSP_RC_TUNING:
|
||||
headSerialReply(7);
|
||||
|
|
@ -918,6 +937,10 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
headSerialReply(sizeof(pidnames) - 1);
|
||||
serializeNames(pidnames);
|
||||
break;
|
||||
case MSP_PID_CONTROLLER:
|
||||
headSerialReply(1);
|
||||
serialize8(currentProfile->pidController);
|
||||
break;
|
||||
case MSP_MODE_RANGES:
|
||||
headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
|
||||
for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
|
||||
|
|
@ -1240,6 +1263,10 @@ static bool processInCommand(void)
|
|||
currentProfile->accelerometerTrims.values.pitch = read16();
|
||||
currentProfile->accelerometerTrims.values.roll = read16();
|
||||
break;
|
||||
case MSP_SET_PID_CONTROLLER:
|
||||
currentProfile->pidController = read8();
|
||||
setPIDController(currentProfile->pidController);
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
if (currentProfile->pidController == 2) {
|
||||
for (i = 0; i < 3; i++) {
|
||||
|
|
|
|||
|
|
@ -17,9 +17,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "drivers/serial.h"
|
||||
|
||||
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
|
||||
#define MAX_MSP_PORT_COUNT 2
|
||||
|
||||
void mspInit(serialConfig_t *serialConfig);
|
||||
|
||||
void mspProcess(void);
|
||||
void sendMspTelemetry(void);
|
||||
void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);
|
||||
|
|
|
|||
|
|
@ -333,7 +333,7 @@ void init(void)
|
|||
|
||||
#ifdef SONAR
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
Sonar_init();
|
||||
sonarInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -469,7 +469,7 @@ void executePeriodicTasks(void)
|
|||
#ifdef SONAR
|
||||
case UPDATE_SONAR_TASK:
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
Sonar_update();
|
||||
sonarUpdate();
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,11 +33,13 @@
|
|||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
|
||||
// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
|
||||
|
||||
#ifdef SONAR
|
||||
|
||||
void Sonar_init(void)
|
||||
static int32_t calculatedAltitude;
|
||||
|
||||
void sonarInit(void)
|
||||
{
|
||||
#if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
|
||||
static const sonarHardware_t const sonarPWM56 = {
|
||||
|
|
@ -74,25 +76,33 @@ void Sonar_init(void)
|
|||
#endif
|
||||
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
sonarAlt = 0;
|
||||
calculatedAltitude = -1;
|
||||
}
|
||||
|
||||
void Sonar_update(void)
|
||||
void sonarUpdate(void)
|
||||
{
|
||||
hcsr04_start_reading();
|
||||
}
|
||||
|
||||
int32_t sonarRead(void)
|
||||
{
|
||||
return hcsr04_get_distance();
|
||||
}
|
||||
|
||||
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
|
||||
{
|
||||
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
|
||||
if (tiltAngle > 250)
|
||||
return -1;
|
||||
calculatedAltitude = -1;
|
||||
else
|
||||
calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
|
||||
|
||||
return sonarAlt * (900.0f - tiltAngle) / 900.0f;
|
||||
return calculatedAltitude;
|
||||
}
|
||||
|
||||
int32_t sonarRead(void) {
|
||||
return hcsr04_get_distance();
|
||||
int32_t sonarGetLatestAltitude(void)
|
||||
{
|
||||
return calculatedAltitude;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,10 +17,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
extern int32_t sonarAlt;
|
||||
|
||||
void Sonar_init(void);
|
||||
void Sonar_update(void);
|
||||
void sonarInit(void);
|
||||
void sonarUpdate(void);
|
||||
|
||||
int32_t sonarRead(void);
|
||||
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
|
||||
int32_t sonarGetLatestAltitude(void);
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
*/
|
||||
|
||||
#define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc)
|
||||
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc)
|
||||
#define FC_VERSION_MINOR 7 // 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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue