removed dependency on built-in printf() when using keil - they provided a much better internal one than GNU
got rid of int16 garbage in mwc pid controller - we aren't running on tarduino git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@345 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
4e94fd07e5
commit
76617bc7e4
|
@ -13,7 +13,12 @@
|
|||
|
||||
#include "stm32f10x_conf.h"
|
||||
#include "core_cm3.h"
|
||||
|
||||
#ifndef __CC_ARM
|
||||
// only need this garbage on gcc
|
||||
#define USE_LAME_PRINTF
|
||||
#include "printf.h"
|
||||
#endif
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846f
|
||||
|
|
21
src/main.c
21
src/main.c
|
@ -7,12 +7,21 @@ extern rcReadRawDataPtr rcReadRawFunc;
|
|||
// two receiver read functions
|
||||
extern uint16_t pwmReadRawRC(uint8_t chan);
|
||||
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
||||
|
||||
|
||||
#ifdef USE_LAME_PRINTF
|
||||
// gcc/GNU version
|
||||
static void _putc(void *p, char c)
|
||||
{
|
||||
uartWrite(c);
|
||||
}
|
||||
|
||||
}
|
||||
#else
|
||||
// keil/armcc version
|
||||
int fputc(int c, FILE *f)
|
||||
{
|
||||
uartWrite(c);
|
||||
return c;
|
||||
}
|
||||
#endif
|
||||
|
||||
int main(void)
|
||||
{
|
||||
|
@ -41,8 +50,10 @@ int main(void)
|
|||
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
|
||||
#endif
|
||||
|
||||
systemInit();
|
||||
init_printf(NULL, _putc);
|
||||
systemInit();
|
||||
#ifdef USE_LAME_PRINTF
|
||||
init_printf(NULL, _putc);
|
||||
#endif
|
||||
|
||||
checkFirstTime(false);
|
||||
readEEPROM();
|
||||
|
|
24
src/mw.c
24
src/mw.c
|
@ -284,12 +284,12 @@ static int32_t errorAngleI[2] = { 0, 0 };
|
|||
static void pidMultiWii(void)
|
||||
{
|
||||
int axis, prop;
|
||||
int16_t error, errorAngle;
|
||||
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
|
||||
int32_t error, errorAngle;
|
||||
int32_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
|
||||
static int16_t lastGyro[3] = { 0, 0, 0 };
|
||||
static int16_t delta1[3], delta2[3];
|
||||
int16_t deltaSum;
|
||||
int16_t delta;
|
||||
static int32_t delta1[3], delta2[3];
|
||||
int32_t deltaSum;
|
||||
int32_t delta;
|
||||
|
||||
// **** PITCH & ROLL & YAW PID ****
|
||||
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
|
||||
|
@ -297,11 +297,11 @@ static void pidMultiWii(void)
|
|||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
|
||||
PTermACC = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
|
||||
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
|
||||
ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
|
||||
}
|
||||
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
|
||||
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
|
||||
|
@ -315,8 +315,8 @@ static void pidMultiWii(void)
|
|||
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
||||
}
|
||||
if (f.HORIZON_MODE && axis < 2) {
|
||||
PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500;
|
||||
ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500;
|
||||
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
|
||||
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
|
||||
} else {
|
||||
if (f.ANGLE_MODE && axis < 2) {
|
||||
PTerm = PTermACC;
|
||||
|
@ -328,13 +328,13 @@ static void pidMultiWii(void)
|
|||
}
|
||||
|
||||
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
delta = gyroData[axis] - lastGyro[axis];
|
||||
lastGyro[axis] = gyroData[axis];
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
DTerm = (deltaSum * dynD8[axis]) / 32;
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -30,8 +30,7 @@
|
|||
*/
|
||||
|
||||
#include "board.h"
|
||||
#include "printf.h"
|
||||
|
||||
#ifdef USE_LAME_PRINTF
|
||||
#define PRINTF_LONG_SUPPORT
|
||||
|
||||
typedef void (*putcf) (void *, char);
|
||||
|
@ -246,3 +245,4 @@ void tfp_sprintf(char *s, char *fmt, ...)
|
|||
putcp(&s, 0);
|
||||
va_end(va);
|
||||
}
|
||||
#endif /* USE_LAME_PRINTF */
|
||||
|
|
Loading…
Reference in New Issue