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 "stm32f10x_conf.h"
|
||||||
#include "core_cm3.h"
|
#include "core_cm3.h"
|
||||||
|
|
||||||
|
#ifndef __CC_ARM
|
||||||
|
// only need this garbage on gcc
|
||||||
|
#define USE_LAME_PRINTF
|
||||||
#include "printf.h"
|
#include "printf.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef M_PI
|
#ifndef M_PI
|
||||||
#define M_PI 3.14159265358979323846f
|
#define M_PI 3.14159265358979323846f
|
||||||
|
|
13
src/main.c
13
src/main.c
|
@ -8,11 +8,20 @@ extern rcReadRawDataPtr rcReadRawFunc;
|
||||||
extern uint16_t pwmReadRawRC(uint8_t chan);
|
extern uint16_t pwmReadRawRC(uint8_t chan);
|
||||||
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
||||||
|
|
||||||
|
#ifdef USE_LAME_PRINTF
|
||||||
|
// gcc/GNU version
|
||||||
static void _putc(void *p, char c)
|
static void _putc(void *p, char c)
|
||||||
{
|
{
|
||||||
uartWrite(c);
|
uartWrite(c);
|
||||||
}
|
}
|
||||||
|
#else
|
||||||
|
// keil/armcc version
|
||||||
|
int fputc(int c, FILE *f)
|
||||||
|
{
|
||||||
|
uartWrite(c);
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
|
@ -42,7 +51,9 @@ int main(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
systemInit();
|
systemInit();
|
||||||
|
#ifdef USE_LAME_PRINTF
|
||||||
init_printf(NULL, _putc);
|
init_printf(NULL, _putc);
|
||||||
|
#endif
|
||||||
|
|
||||||
checkFirstTime(false);
|
checkFirstTime(false);
|
||||||
readEEPROM();
|
readEEPROM();
|
||||||
|
|
24
src/mw.c
24
src/mw.c
|
@ -284,12 +284,12 @@ static int32_t errorAngleI[2] = { 0, 0 };
|
||||||
static void pidMultiWii(void)
|
static void pidMultiWii(void)
|
||||||
{
|
{
|
||||||
int axis, prop;
|
int axis, prop;
|
||||||
int16_t error, errorAngle;
|
int32_t error, errorAngle;
|
||||||
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
|
int32_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
|
||||||
static int16_t lastGyro[3] = { 0, 0, 0 };
|
static int16_t lastGyro[3] = { 0, 0, 0 };
|
||||||
static int16_t delta1[3], delta2[3];
|
static int32_t delta1[3], delta2[3];
|
||||||
int16_t deltaSum;
|
int32_t deltaSum;
|
||||||
int16_t delta;
|
int32_t delta;
|
||||||
|
|
||||||
// **** PITCH & ROLL & YAW PID ****
|
// **** PITCH & ROLL & YAW PID ****
|
||||||
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
|
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
|
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
|
||||||
// 50 degrees max inclination
|
// 50 degrees max inclination
|
||||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
|
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);
|
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||||
|
|
||||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
|
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
|
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];
|
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;
|
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
||||||
}
|
}
|
||||||
if (f.HORIZON_MODE && axis < 2) {
|
if (f.HORIZON_MODE && axis < 2) {
|
||||||
PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500;
|
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
|
||||||
ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500;
|
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
|
||||||
} else {
|
} else {
|
||||||
if (f.ANGLE_MODE && axis < 2) {
|
if (f.ANGLE_MODE && axis < 2) {
|
||||||
PTerm = PTermACC;
|
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
|
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];
|
lastGyro[axis] = gyroData[axis];
|
||||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||||
delta2[axis] = delta1[axis];
|
delta2[axis] = delta1[axis];
|
||||||
delta1[axis] = delta;
|
delta1[axis] = delta;
|
||||||
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation
|
DTerm = (deltaSum * dynD8[axis]) / 32;
|
||||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,8 +30,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "board.h"
|
#include "board.h"
|
||||||
#include "printf.h"
|
#ifdef USE_LAME_PRINTF
|
||||||
|
|
||||||
#define PRINTF_LONG_SUPPORT
|
#define PRINTF_LONG_SUPPORT
|
||||||
|
|
||||||
typedef void (*putcf) (void *, char);
|
typedef void (*putcf) (void *, char);
|
||||||
|
@ -246,3 +245,4 @@ void tfp_sprintf(char *s, char *fmt, ...)
|
||||||
putcp(&s, 0);
|
putcp(&s, 0);
|
||||||
va_end(va);
|
va_end(va);
|
||||||
}
|
}
|
||||||
|
#endif /* USE_LAME_PRINTF */
|
||||||
|
|
Loading…
Reference in New Issue