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

Merge branch 'upstream' of github.com:multiwii/baseflight into upstream

Browse source 
Conflicts:
	Makefile
	src/main.c
This commit is contained in:
Dominic Clifton 2013-06-24 20:34:33 +01:00
commit cae6bc86f1
25 changed files with 3757 additions and 3500 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitignore vendored Normal file
View file

@ -0,0 +1,2 @@
*.o
*~

17
Makefile
View file

@ -23,6 +23,9 @@ OPTIONS ?=
# Debugger optons, must be empty or GDB
DEBUG ?=
# Serial port/Device for flashing
SERIAL_DEVICE ?= /dev/ttyUSB0
###############################################################################
# Things that need to be maintained as the source changes
#
@ -192,6 +195,20 @@ $(OBJECT_DIR)/$(TARGET)/%.o): %.S
clean:
rm -f $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS)
flash_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
echo -n 'R' >$(SERIAL_DEVICE)
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
flash: flash_$(TARGET)
unbrick_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
unbrick: unbrick_$(TARGET)
help:
@echo ""
@echo "Makefile for the baseflight firmware"

11
baseflight.uvproj
View file

@ -165,6 +165,7 @@
<Capability>1</Capability>
<DriverSelection>4096</DriverSelection>
</Flash1>
<bUseTDR>0</bUseTDR>
<Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>
@ -196,7 +197,7 @@
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<RvctClst>1</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M3"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
@ -970,8 +971,9 @@
<Capability>1</Capability>
<DriverSelection>4096</DriverSelection>
</Flash1>
<bUseTDR>0</bUseTDR>
<Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3></Flash3>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>
</Utilities>
<TargetArmAds>
@ -988,7 +990,7 @@
<ldmm>1</ldmm>
<ldXref>1</ldXref>
<BigEnd>0</BigEnd>
<AdsALst>0</AdsALst>
<AdsALst>1</AdsALst>
<AdsACrf>1</AdsACrf>
<AdsANop>0</AdsANop>
<AdsANot>0</AdsANot>
@ -1001,7 +1003,7 @@
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<RvctClst>1</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M3"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
@ -1719,6 +1721,7 @@
<Capability>1</Capability>
<DriverSelection>4096</DriverSelection>
</Flash1>
<bUseTDR>0</bUseTDR>
<Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>

6401
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

7
src/board.h
View file

@ -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
@ -75,6 +80,7 @@ typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and a
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype
typedef struct sensor_t
{
@ -195,6 +201,7 @@ typedef struct baro_t
#include "drv_pwm.h"
#include "drv_uart.h"
#else
// AfroFlight32
#include "drv_system.h" // timers, delays, etc
#include "drv_adc.h"

108
src/cli.c
View file

@ -44,7 +44,7 @@ const char * const mixerNames[] = {
const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "VARIO",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO",
NULL
};
@ -131,8 +131,9 @@ const clivalue_t valueTable[] = {
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 },
{ "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
{ "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
@ -145,6 +146,7 @@ const clivalue_t valueTable[] = {
{ "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
{ "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, 2000 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
@ -169,7 +171,6 @@ const clivalue_t valueTable[] = {
{ "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
{ "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
{ "acc_lpf_for_velocity", VAR_UINT8, &cfg.acc_lpf_for_velocity, 1, 250 },
{ "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 },
{ "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 },
{ "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
@ -598,7 +599,7 @@ static void cliDump(char *cmdline)
static void cliExit(char *cmdline)
{
uartPrint("\r\nLeaving CLI mode...\r\n");
memset(cliBuffer, 0, sizeof(cliBuffer));
*cliBuffer = '\0';
bufferIndex = 0;
cliMode = 0;
// save and reboot... I think this makes the most sense
@ -860,6 +861,16 @@ static void cliSet(char *cmdline)
}
}
uartPrint("ERR: Unknown variable name\r\n");
} else {
// no equals, check for matching variables.
for (i = 0; i < VALUE_COUNT; i++) {
if (strstr(valueTable[i].name, cmdline)) {
val = &valueTable[i];
printf("%s = ", valueTable[i].name);
cliPrintVar(val, 0);
printf("\r\n");
}
}
}
}
@ -904,6 +915,35 @@ void cliProcess(void)
while (uartAvailable()) {
uint8_t c = uartRead();
/* first step: translate "ESC[" -> "CSI" */
if (c == '\033') {
c = uartReadPoll();
if (c == '[')
c = 0x9b;
else
/* ignore unknown sequences */
c = 0;
}
/* second step: translate known CSI sequence into singlebyte control sequences */
if (c == 0x9b) {
c = uartReadPoll();
if (c == 'A') //up
c = 0x0b;
else if (c == 'B') //down
c = 0x0a;
else if (c == 'C') //right
c = 0x0c;
else if (c == 'D') //left
c = 0x08;
else if (c == 0x33 && uartReadPoll() == 0x7e) //delete
c = 0xff; // nonstandard, borrowing 0xff for the delete key
else
c = 0;
}
/* from here on everything is a single byte */
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
@ -919,9 +959,10 @@ void cliProcess(void)
for (; ; bufferIndex++) {
if (pstart->name[bufferIndex] != pend->name[bufferIndex])
break;
if (!pstart->name[bufferIndex]) {
if (!pstart->name[bufferIndex] && bufferIndex < sizeof(cliBuffer) - 2) {
/* Unambiguous -- append a space */
cliBuffer[bufferIndex++] = ' ';
cliBuffer[bufferIndex] = '\0';
break;
}
cliBuffer[bufferIndex] = pstart->name[bufferIndex];
@ -942,20 +983,41 @@ void cliProcess(void)
} else if (!bufferIndex && c == 4) {
cliExit(cliBuffer);
return;
} else if (c == 0x15) {
// ctrl+u == delete line
uartPrint("\033[G\033[K# ");
bufferIndex = 0;
*cliBuffer = '\0';
} else if (c == 0x0b) {
//uartPrint("up unimplemented");
} else if (c == 0x0a) {
//uartPrint("down unimplemend");
} else if (c == 0x08) {
if (bufferIndex > 0) {
bufferIndex--;
uartPrint("\033[D");
}
} else if (c == 12) {
// clear screen
uartPrint("\033[2J\033[1;1H");
cliPrompt();
} else if (bufferIndex && (c == '\n' || c == '\r')) {
if (cliBuffer[bufferIndex]) {
bufferIndex++;
uartPrint("\033[C");
}
} else if (c == 0xff) {
// delete key
if (cliBuffer[bufferIndex]) {
int len = strlen(cliBuffer + bufferIndex);
memmove(cliBuffer + bufferIndex, cliBuffer + bufferIndex + 1, len + 1);
printf("%s \033[%dD", cliBuffer + bufferIndex, len);
}
} else if (*cliBuffer && (c == '\n' || c == '\r')) {
// enter pressed
clicmd_t *cmd = NULL;
clicmd_t target;
uartPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name) + 1);
@ -971,15 +1033,25 @@ void cliProcess(void)
cliPrompt();
} else if (c == 127) {
// backspace
if (bufferIndex) {
cliBuffer[--bufferIndex] = 0;
uartPrint("\010 \010");
if (bufferIndex && *cliBuffer) {
int len = strlen(cliBuffer + bufferIndex);
--bufferIndex;
memmove(cliBuffer + bufferIndex, cliBuffer + bufferIndex + 1, len + 1);
printf("\033[D%s \033[%dD", cliBuffer + bufferIndex, len + 1);
}
} else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
} else if (strlen(cliBuffer) + 1 < sizeof(cliBuffer) && c >= 32 && c <= 126) {
int len;
if (!bufferIndex && c == 32)
continue;
cliBuffer[bufferIndex++] = c;
uartWrite(c);
len = strlen(cliBuffer + bufferIndex);
memmove(cliBuffer + bufferIndex + 1, cliBuffer + bufferIndex, len + 1);
cliBuffer[bufferIndex] = c;
printf("%s \033[%dD", cliBuffer + bufferIndex, len + 1);
++bufferIndex;
}
}
}

15
src/config.c
View file

@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234";
static uint8_t EEPROM_CONF_VERSION = 47;
static uint8_t EEPROM_CONF_VERSION = 48;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -84,6 +84,7 @@ void readEEPROM(void)
}
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
setPIDController(cfg.pidController);
}
void writeEEPROM(uint8_t b, uint8_t updateProfile)
@ -170,7 +171,8 @@ static void resetConf(void)
// global settings
mcfg.current_profile = 0; // default profile
mcfg.gyro_cmpf_factor = 400; // default MWC
mcfg.gyro_cmpf_factor = 600; // default MWC
mcfg.gyro_cmpfm_factor = 250; // default MWC
mcfg.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
mcfg.accZero[0] = 0;
mcfg.accZero[1] = 0;
@ -201,6 +203,7 @@ static void resetConf(void)
mcfg.serial_baudrate = 115200;
mcfg.looptime = 3500;
cfg.pidController = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
@ -242,7 +245,6 @@ static void resetConf(void)
cfg.angleTrim[1] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
@ -256,9 +258,10 @@ static void resetConf(void)
cfg.alt_hold_fast_change = 1;
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
cfg.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
// servos
cfg.yaw_direction = 1;

21
src/drv_hmc5883l.c
View file

@ -17,7 +17,9 @@
#define HMC_POS_BIAS 1
#define HMC_NEG_BIAS 2
bool hmc5883lDetect(void)
static int8_t sensor_align[3];
bool hmc5883lDetect(int8_t *align)
{
bool ack = false;
uint8_t sig = 0;
@ -26,6 +28,8 @@ bool hmc5883lDetect(void)
if (!ack || sig != 'H')
return false;
memcpy(sensor_align, align, 3);
return true;
}
@ -117,10 +121,19 @@ void hmc5883lInit(float *calibrationGain)
void hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
int16_t mag[3];
int i;
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
mag[0] = buf[0] << 8 | buf[1];
mag[1] = buf[2] << 8 | buf[3];
mag[2] = buf[4] << 8 | buf[5];
magData[0] = buf[0] << 8 | buf[1];
magData[1] = buf[2] << 8 | buf[3];
magData[2] = buf[4] << 8 | buf[5];
for (i = 0; i < 3; i++) {
int8_t axis = sensor_align[i];
if (axis > 0)
magData[axis - 1] = mag[i];
else
magData[-axis - 1] = -mag[i];
}
}

2
src/drv_hmc5883l.h
View file

@ -1,5 +1,5 @@
#pragma once
bool hmc5883lDetect(void);
bool hmc5883lDetect(int8_t *align);
void hmc5883lInit(float *calibrationGain);
void hmc5883lRead(int16_t *magData);

13
src/drv_i2c.c
View file

@ -31,7 +31,6 @@ void I2C2_EV_IRQHandler(void)
i2c_ev_handler();
}
#define I2C_DEFAULT_TIMEOUT 30000
static volatile uint16_t i2cErrorCount = 0;
@ -79,26 +78,18 @@ static void i2c_er_handler(void)
bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
{
uint8_t i;
uint8_t my_data[16];
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
addr = addr_ << 1;
reg = reg_;
writing = 1;
reading = 0;
write_p = my_data;
read_p = my_data;
write_p = data;
read_p = data;
bytes = len_;
busy = 1;
error = false;
// too long
if (len_ > 16)
return false;
for (i = 0; i < len_; i++)
my_data[i] = data[i];
if (!(I2Cx->CR2 & I2C_IT_EVT)) { //if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200) { ; } //wait for any stop to finish sending

14
src/drv_pwm.c
View file

@ -118,6 +118,7 @@ static pwmPortData_t *servos[MAX_SERVOS];
static uint8_t numMotors = 0;
static uint8_t numServos = 0;
static uint8_t numInputs = 0;
static uint16_t failsafeThreshold = 985;
// external vars (ugh)
extern int16_t failsafeCnt;
@ -394,6 +395,7 @@ static void ppmCallback(uint8_t port, uint16_t capture)
static uint16_t now;
static uint16_t last = 0;
static uint8_t chan = 0;
static uint8_t GoodPulses;
last = now;
now = capture;
@ -404,6 +406,15 @@ static void ppmCallback(uint8_t port, uint16_t capture)
} else {
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range
captures[chan] = diff;
if (chan < 4 && diff > failsafeThreshold)
GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK
if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter
GoodPulses = 0;
if (failsafeCnt > 20)
failsafeCnt -= 20;
else
failsafeCnt = 0;
}
}
chan++;
failsafeCnt = 0;
@ -434,6 +445,9 @@ bool pwmInit(drv_pwm_config_t *init)
int i = 0;
const uint8_t *setup;
// to avoid importing cfg/mcfg
failsafeThreshold = init->failsafeThreshold;
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
if (init->airplane)
i = 2; // switch to air hardware config

1
src/drv_pwm.h
View file

@ -14,6 +14,7 @@ typedef struct drv_pwm_config_t {
uint8_t adcChannel; // steal one RC input for current sensor
uint16_t motorPwmRate;
uint16_t servoPwmRate;
uint16_t failsafeThreshold;
} drv_pwm_config_t;
// This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data.

10
src/drv_pwm_fy90q.c
View file

@ -66,6 +66,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
static uint16_t now;
static uint16_t last = 0;
static uint8_t chan = 0;
static uint8_t GoodPulses;
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
last = now;
@ -86,6 +87,15 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
} else {
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range
Inputs[chan].capture = diff;
if (chan < 4 && diff > FAILSAFE_DETECT_TRESHOLD)
GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK
if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter
GoodPulses = 0;
if (failsafeCnt > 20)
failsafeCnt -= 20;
else
failsafeCnt = 0;
}
}
chan++;
failsafeCnt = 0;

2
src/drv_system.c
View file

@ -33,7 +33,7 @@ uint32_t micros(void)
ms = sysTickUptime;
cycle_cnt = SysTick->VAL;
} while (ms != sysTickUptime);
return (ms * 1000) + (72000 - cycle_cnt) / 72;
return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks;
}
// Return system uptime in milliseconds (rollover in 49 days)

16
src/drv_uart.c
View file

@ -8,10 +8,11 @@
// Receive buffer, circular DMA
volatile uint8_t rxBuffer[UART_BUFFER_SIZE];
uint32_t rxDMAPos = 0;
volatile uint32_t rxDMAPos = 0;
volatile uint8_t txBuffer[UART_BUFFER_SIZE];
uint32_t txBufferTail = 0;
uint32_t txBufferHead = 0;
volatile uint32_t txBufferTail = 0;
volatile uint32_t txBufferHead = 0;
volatile bool txDMAEmpty = false;
static void uartTxDMA(void)
{
@ -23,7 +24,7 @@ static void uartTxDMA(void)
DMA1_Channel4->CNDTR = UART_BUFFER_SIZE - txBufferTail;
txBufferTail = 0;
}
txDMAEmpty = false;
DMA_Cmd(DMA1_Channel4, ENABLE);
}
@ -34,6 +35,8 @@ void DMA1_Channel4_IRQHandler(void)
if (txBufferHead != txBufferTail)
uartTxDMA();
else
txDMAEmpty = true;
}
void uartInit(uint32_t speed)
@ -109,6 +112,11 @@ uint16_t uartAvailable(void)
return (DMA_GetCurrDataCounter(DMA1_Channel5) != rxDMAPos) ? true : false;
}
bool uartTransmitDMAEmpty(void)
{
return txDMAEmpty;
}
bool uartTransmitEmpty(void)
{
return (txBufferTail == txBufferHead);

1
src/drv_uart.h
View file

@ -4,6 +4,7 @@
void uartInit(uint32_t speed);
uint16_t uartAvailable(void);
bool uartTransmitEmpty(void);
bool uartTransmitDMAEmpty(void);
uint8_t uartRead(void);
uint8_t uartReadPoll(void);
void uartWrite(uint8_t ch);

85
src/gps.c
View file

@ -928,24 +928,46 @@ typedef struct {
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} ubx_nav_velned;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
uint32_t heading_accuracy;
} ubx_nav_velned;
typedef struct
{
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
uint8_t svid; // Satellite ID
uint8_t flags; // Bitmask
uint8_t quality; // Bitfield
uint8_t cno; // Carrier to Noise Ratio (Signal Strength)
uint8_t elev; // Elevation in integer degrees
int16_t azim; // Azimuth in integer degrees
int32_t prRes; // Pseudo range residual in centimetres
} ubx_nav_svinfo_channel;
typedef struct
{
uint32_t time; // GPS Millisecond time of week
uint8_t numCh; // Number of channels
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
uint16_t reserved2; // Reserved
ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
} ubx_nav_svinfo;
enum {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_SVINFO = 0x30,
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24
} ubs_protocol_bytes;
@ -986,13 +1008,14 @@ static uint8_t _disable_counter;
// Receive buffer
static union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
uint8_t bytes[64];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_nav_svinfo svinfo;
uint8_t bytes[200];
} _buffer;
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
{
while (len--) {
*ck_a += *data;
@ -1068,6 +1091,7 @@ static bool GPS_UBLOX_newFrame(uint8_t data)
static bool UBLOX_parse_gps(void)
{
int i;
switch (_msg_id) {
case MSG_POSLLH:
//i2c_dataset.time = _buffer.posllh.time;
@ -1093,12 +1117,23 @@ static bool UBLOX_parse_gps(void)
case MSG_VELNED:
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
default:
return false;
}
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:
GPS_numCh = _buffer.svinfo.numCh;
if (GPS_numCh > 16)
GPS_numCh = 16;
for (i = 0; i < GPS_numCh; i++){
GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn;
GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid;
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
}
break;
default:
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data

147
src/imu.c
View file

@ -2,7 +2,6 @@
#include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
@ -48,8 +47,6 @@ void computeIMU(void)
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
#define GYRO_INTERLEAVE
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
@ -57,18 +54,11 @@ void computeIMU(void)
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
#ifdef GYRO_INTERLEAVE
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
#else
gyroData[axis] = gyroADC[axis];
#endif
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
}
timeInterleave = micros();
annexCode();
#ifdef GYRO_INTERLEAVE
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
@ -84,7 +74,6 @@ void computeIMU(void)
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
}
#endif
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
@ -128,12 +117,6 @@ void computeIMU(void)
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
@ -142,7 +125,7 @@ void computeIMU(void)
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
@ -164,11 +147,6 @@ void rotateV(struct fp_vector *v, float *delta)
{
struct fp_vector v_tmp = *v;
#if INACCURATE
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
#else
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
@ -200,7 +178,6 @@ void rotateV(struct fp_vector *v, float *delta)
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];
#endif
}
static int16_t _atan2f(float y, float x)
@ -209,18 +186,26 @@ static int16_t _atan2f(float y, float x)
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
}
// Use original baseflight angle calculation
// #define BASEFLIGHT_CALC
static float invG;
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
#ifndef BASEFLIGHT_CALC
float sqGZ;
float sqGX;
float sqGY;
float sqGX_sqGZ;
float invmagXZ;
#endif
scale = (currentT - previousT) * gyro.scale;
previousT = currentT;
@ -234,17 +219,7 @@ static void getEstimatedAttitude(void)
} else {
accSmooth[axis] = accADC[axis];
}
accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f / cfg.acc_lpf_for_velocity)) + accADC[axis] * (1.0f / cfg.acc_lpf_for_velocity);
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
if (sensors(SENSOR_MAG)) {
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
}
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
@ -258,35 +233,39 @@ static void getEstimatedAttitude(void)
f.SMALL_ANGLES_25 = 0;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
if (72 < accMag && accMag < 133) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
}
// Attitude of the estimated vector
#if INACCURATE
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
#else
#ifdef BASEFLIGHT_CALC
// This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
#else
// MW2.2 version
sqGZ = EstG.V.Z * EstG.V.Z;
sqGX = EstG.V.X * EstG.V.X;
sqGY = EstG.V.Y * EstG.V.Y;
sqGX_sqGZ = sqGX + sqGZ;
invmagXZ = 1.0f / sqrtf(sqGX_sqGZ);
invG = 1.0f / sqrtf(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = _atan2f(EstG.V.Y, invmagXZ * sqGX_sqGZ);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
#if INACCURATE
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
heading = heading + magneticDeclination;
heading = heading / 10;
#else
#ifdef BASEFLIGHT_CALC
// baseflight calculation
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = -(float)angle[PITCH] * RADX10;
float magX = EstM.A[1]; // Swap X/Y
@ -300,6 +279,11 @@ static void getEstimatedAttitude(void)
float Yh = magY * cr - magZ * sr;
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = hd;
#else
// MW 2.2 calculation
heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * sqGX_sqGZ - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
heading = heading + magneticDeclination;
heading = heading / 10;
#endif
if (heading > 180)
heading = heading - 360;
@ -313,7 +297,7 @@ static void getEstimatedAttitude(void)
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
int16_t applyDeadband16(int16_t value, int16_t deadband)
int16_t applyDeadband(int16_t value, int16_t deadband)
{
if (abs(value) < deadband) {
value = 0;
@ -325,54 +309,16 @@ int16_t applyDeadband16(int16_t value, int16_t deadband)
return value;
}
float applyDeadbandFloat(float value, int16_t deadband)
{
if (abs(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
float InvSqrt(float x)
{
union {
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f3759df - (conv.i >> 1);
return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
int32_t isq(int32_t x)
{
return x * x;
}
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
int getEstimatedAltitude(void)
{
static int32_t baroGroundPressure;
static uint16_t previousT;
uint16_t currentT = micros();
uint16_t dTime;
float invG;
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t dTime;
int16_t error16;
int16_t baroVel;
int16_t accZ;
static int16_t accZoffset = 0; // = acc_1G*6; //58 bytes saved and convergence is fast enough to omit init
static int16_t accZoffset = 0;
static float vel = 0.0f;
static int32_t lastBaroAlt;
int16_t vel_tmp;
@ -390,12 +336,12 @@ int getEstimatedAltitude(void)
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = log(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise
//P
error16 = constrain(AltHold - EstAlt, -300, 300);
applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
error16 = applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
//I
@ -404,8 +350,7 @@ int getEstimatedAltitude(void)
BaroPID += errorAltitudeI >> 9; // I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
// Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude)
accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
if (!f.ARMED) {
@ -413,7 +358,7 @@ int getEstimatedAltitude(void)
accZoffset += accZ;
}
accZ -= accZoffset >> 3;
applyDeadband(accZ, cfg.accz_deadband);
accZ = applyDeadband(accZ, cfg.accz_deadband);
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;
@ -422,7 +367,7 @@ int getEstimatedAltitude(void)
lastBaroAlt = EstAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
@ -430,7 +375,7 @@ int getEstimatedAltitude(void)
// D
vel_tmp = vel;
applyDeadband(vel_tmp, 5);
vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp >> 4, -150, 150);

26
src/main.c
View file

@ -7,12 +7,23 @@ 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)
{
// let DMA catch up a bit when using set or dump, we're too fast.
while (!uartTransmitDMAEmpty());
uartWrite(c);
return c;
}
#endif
int main(void)
{
@ -41,8 +52,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();
@ -72,7 +85,8 @@ int main(void)
pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) {
case 1:
pwm_params.adcChannel = PWM2;

229
src/mw.c
View file

@ -1,7 +1,7 @@
#include "board.h"
#include "mw.h"
// October 2012 V2.1-dev
// June 2013 V2.2-dev
flags_t f;
int16_t debug[4];
@ -24,6 +24,10 @@ int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
uint16_t rssi; // range: [0;1023]
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
static void pidMultiWii(void);
static void pidRewrite(void);
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t rcOptions[CHECKBOXITEMS];
@ -44,12 +48,17 @@ int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
// Automatic ACC Offset Calibration
uint16_t InflightcalibratingA = 0;
int16_t AccInflightCalibrationArmed;
uint16_t AccInflightCalibrationMeasurementDone = 0;
uint16_t AccInflightCalibrationSavetoEEProm = 0;
@ -127,6 +136,7 @@ void annexCode(void)
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t) cfg.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < mcfg.midrc)
rcCommand[axis] = -rcCommand[axis];
@ -268,24 +278,160 @@ static void mwVario(void)
}
static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(void)
{
int axis, prop;
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 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]
for (axis = 0; axis < 3; axis++) {
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 = 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 = (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];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
} else {
if (f.ANGLE_MODE && axis < 2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
}
}
#define GYRO_I_MAX 256
static void pidRewrite(void)
{
int32_t errorAngle;
int axis;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4;
}
}
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * cfg.P8[axis]) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * cfg.D8[axis]) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
}
}
void setPIDController(int type)
{
switch (type) {
case 0:
default:
pid_controller = pidMultiWii;
break;
case 1:
pid_controller = pidRewrite;
break;
}
}
void loop(void)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t stTmp = 0;
uint8_t axis, i;
int16_t error, errorAngle;
int16_t delta, deltaSum;
int16_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];
static int16_t errorGyroI[3] = { 0, 0, 0 };
static int16_t errorAngleI[2] = { 0, 0 };
int i;
static uint32_t rcTime = 0;
static int16_t initialThrottleHold;
static uint32_t loopTime;
uint16_t auxState = 0;
int16_t prop;
static uint8_t GPSNavReset = 1;
// this will return false if spektrum is disabled. shrug.
@ -423,7 +569,7 @@ void loop(void)
i = 1;
}
if (i) {
writeEEPROM(1, false);
writeEEPROM(1, true);
rcDelayCommand = 0; // allow autorepetition
}
}
@ -697,53 +843,8 @@ void loop(void)
}
}
// **** PITCH & ROLL & YAW PID ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
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 = 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;
}
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 -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
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;
} else {
if (f.ANGLE_MODE && axis < 2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
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
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;
}
// PID - note this is function pointer set by setPIDController()
pid_controller();
mixTable();
writeServos();

22
src/mw.h
View file

@ -145,6 +145,7 @@ enum {
};
typedef struct config_t {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671
uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS];
@ -163,7 +164,6 @@ typedef struct config_t {
// sensor-related stuff
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t acc_lpf_for_velocity; // ACC lowpass for AccZ height hold
uint8_t accz_deadband; // ??
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
@ -181,6 +181,7 @@ typedef struct config_t {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe.
// mixer-related configuration
int8_t yaw_direction;
@ -245,6 +246,7 @@ typedef struct master_t {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver.
@ -353,18 +355,24 @@ extern int16_t GPS_angle[2]; // it's the angles
extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern master_t mcfg;
extern config_t cfg;
extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern master_t mcfg;
extern config_t cfg;
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;
extern baro_t baro;
// main
void setPIDController(int type);
void loop(void);
// IMU

4
src/printf.c
View file

@ -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 */

12
src/sensors.c
View file

@ -107,7 +107,7 @@ retry:
gyro.init();
#ifdef MAG
if (!hmc5883lDetect())
if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
sensorsClear(SENSOR_MAG);
#endif
@ -402,19 +402,13 @@ void Gyro_getADC(void)
}
#ifdef MAG
static float magCal[3] = { 1.0, 1.0, 1.0 }; // gain for each axis, populated at sensor init
static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0;
static void Mag_getRawADC(void)
{
// MAG driver will align itself, so no need to alignSensors()
hmc5883lRead(magADC);
// Default mag orientation is -2, -3, 1 or
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
// magADC[ROLL] = rawADC[2]; // X
// magADC[PITCH] = -rawADC[0]; // Y
// magADC[YAW] = -rawADC[1]; // Z
alignSensors(ALIGN_MAG, magADC);
}
void Mag_init(void)

87
src/serial.c
View file

@ -25,6 +25,7 @@
#define MSP_PIDNAMES 117 //out message the PID names
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
#define MSP_SERVO_CONF 120 //out message Servo settings
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
@ -38,6 +39,8 @@
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
#define MSP_SET_SERVO_CONF 212 //in message Servo settings
#define MSP_SET_MOTOR 214 //in message PropBalance function
// #define MSP_BIND 240 //in message no param
@ -47,12 +50,13 @@
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define INBUF_SIZE 64
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
@ -293,7 +297,7 @@ void serialInit(uint32_t baudrate)
static void evaluateCommand(void)
{
uint32_t i;
uint32_t i, tmp;
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
@ -370,6 +374,9 @@ static void evaluateCommand(void)
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
#if FUCK_MULTIWII
// OK, so you waste all the fucking time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
// the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE FUCKING LOGIC IN THIS, FUCKWADS.
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
@ -383,6 +390,49 @@ static void evaluateCommand(void)
rcOptions[BOXGOV] << BOXGOV |
rcOptions[BOXOSD] << BOXOSD |
f.ARMED << BOXARM);
#else
// Serialize the boxes in the order we delivered them
tmp = 0;
for (i = 0; i < numberBoxItems; i++) {
uint8_t val, box = availableBoxes[i];
switch (box) {
// Handle the special cases
case BOXANGLE:
val = f.ANGLE_MODE;
break;
case BOXHORIZON:
val = f.HORIZON_MODE;
break;
case BOXMAG:
val = f.MAG_MODE;
break;
case BOXBARO:
val = f.BARO_MODE;
break;
case BOXHEADFREE:
val = f.HEADFREE_MODE;
break;
case BOXGPSHOME:
val = f.GPS_HOME_MODE;
break;
case BOXGPSHOLD:
val = f.GPS_HOLD_MODE;
break;
case BOXPASSTHRU:
val = f.PASSTHRU_MODE;
break;
case BOXARM:
val = f.ARMED;
break;
default:
// These just directly rely on their RC inputs
val = rcOptions[ box ];
break;
}
tmp |= (val << i);
}
serialize32(tmp);
#endif
serialize8(mcfg.current_profile);
break;
case MSP_RAW_IMU:
@ -565,13 +615,22 @@ static void evaluateCommand(void)
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}

4
src/telemetry.c
View file

@ -95,9 +95,9 @@ static void sendAccel(void)
static void sendBaro(void)
{
sendDataHead(ID_ALTITUDE_BP);
serialize16(EstAlt / 100);
serialize16(BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(EstAlt % 100);
serialize16(BaroAlt % 100);
}
static void sendTemperature1(void)