rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'multiwii/master' 

Conflicts:
	src/cli.c
	src/config.c
	src/drv_system.c
	src/main.c
This commit is contained in:
Dominic Clifton 2013-12-15 16:13:45 +00:00
parent ed3a3e683c 72c879560e
commit 874bf96447
32 changed files with 4505 additions and 3941 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -1,2 +1,7 @@
*.o *.o
*~ *~
*.uvopt
*.dep
*.bak
*.uvgui.*
obj/

8
Makefile
View File

@ -60,6 +60,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
drv_i2c_soft.c \ drv_i2c_soft.c \
drv_system.c \ drv_system.c \
drv_serial.c \ drv_serial.c \
drv_softserial.c \
drv_uart.c \ drv_uart.c \
printf.c \ printf.c \
utils.c \ utils.c \
@ -98,9 +99,12 @@ OLIMEXINO_SRC = drv_spi.c \
drv_l3g4200d.c \ drv_l3g4200d.c \
drv_pwm.c \ drv_pwm.c \
drv_timer.c \ drv_timer.c \
drv_softserial.c \
$(COMMON_SRC) $(COMMON_SRC)
# In some cases, %.s regarded as intermediate file, which is actually not.
# This will prevent accidental deletion of startup code.
.PRECIOUS: %.s
# Search path for baseflight sources # Search path for baseflight sources
VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups VPATH := $(SRC_DIR):$(SRC_DIR)/baseflight_startups
@ -182,7 +186,7 @@ TARGET_MAP = $(OBJECT_DIR)/baseflight_$(TARGET).map
# It would be nice to compute these lists, but that seems to be just beyond make. # It would be nice to compute these lists, but that seems to be just beyond make.
$(TARGET_HEX): $(TARGET_ELF) $(TARGET_HEX): $(TARGET_ELF)
$(OBJCOPY) -O ihex $< $@ $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@
$(TARGET_ELF): $(TARGET_OBJS) $(TARGET_ELF): $(TARGET_OBJS)
$(CC) -o $@ $^ $(LDFLAGS) $(CC) -o $@ $^ $(LDFLAGS)

50
baseflight.uvproj
View File

@ -30,6 +30,7 @@
<SLE66AMisc></SLE66AMisc> <SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc> <SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile> <SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv> <UseEnv>0</UseEnv>
<BinPath></BinPath> <BinPath></BinPath>
<IncludePath></IncludePath> <IncludePath></IncludePath>
@ -97,6 +98,7 @@
<StopOnExitCode>3</StopOnExitCode> <StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<DllOption> <DllOption>
<SimDllName>SARMCM3.DLL</SimDllName> <SimDllName>SARMCM3.DLL</SimDllName>
@ -169,6 +171,10 @@
<Flash2>BIN\UL2CM3.DLL</Flash2> <Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3> <Flash3>"" ()</Flash3>
<Flash4></Flash4> <Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities> </Utilities>
<TargetArmAds> <TargetArmAds>
<ArmAdsMisc> <ArmAdsMisc>
@ -551,6 +557,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -596,6 +603,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -696,6 +704,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>0</ComprImg>
</CommonProperty> </CommonProperty>
<GroupArmAds> <GroupArmAds>
<Cads> <Cads>
@ -788,11 +797,6 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath> <FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath>
</File> </File>
<File>
<FileName>stm32f10x_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_gpio.c</FilePath>
</File>
<File> <File>
<FileName>stm32f10x_flash.c</FileName> <FileName>stm32f10x_flash.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
@ -822,6 +826,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Aads> <Aads>
@ -881,6 +886,7 @@
<SLE66AMisc></SLE66AMisc> <SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc> <SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile> <SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv> <UseEnv>0</UseEnv>
<BinPath></BinPath> <BinPath></BinPath>
<IncludePath></IncludePath> <IncludePath></IncludePath>
@ -948,6 +954,7 @@
<StopOnExitCode>3</StopOnExitCode> <StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<DllOption> <DllOption>
<SimDllName>SARMCM3.DLL</SimDllName> <SimDllName>SARMCM3.DLL</SimDllName>
@ -1020,6 +1027,10 @@
<Flash2>BIN\UL2CM3.DLL</Flash2> <Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3> <Flash3>"" ()</Flash3>
<Flash4></Flash4> <Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities> </Utilities>
<TargetArmAds> <TargetArmAds>
<ArmAdsMisc> <ArmAdsMisc>
@ -1402,6 +1413,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -1447,6 +1459,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -1583,11 +1596,6 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath> <FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath>
</File> </File>
<File>
<FileName>stm32f10x_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_gpio.c</FilePath>
</File>
<File> <File>
<FileName>stm32f10x_flash.c</FileName> <FileName>stm32f10x_flash.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
@ -1617,6 +1625,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Aads> <Aads>
@ -1676,6 +1685,7 @@
<SLE66AMisc></SLE66AMisc> <SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc> <SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\ST\STM32F10xx\STM32F10xxB.sfr</SFDFile> <SFDFile>SFD\ST\STM32F10xx\STM32F10xxB.sfr</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv> <UseEnv>0</UseEnv>
<BinPath></BinPath> <BinPath></BinPath>
<IncludePath></IncludePath> <IncludePath></IncludePath>
@ -1743,6 +1753,7 @@
<StopOnExitCode>3</StopOnExitCode> <StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<DllOption> <DllOption>
<SimDllName>SARMCM3.DLL</SimDllName> <SimDllName>SARMCM3.DLL</SimDllName>
@ -1815,6 +1826,10 @@
<Flash2>BIN\UL2CM3.DLL</Flash2> <Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3> <Flash3>"" ()</Flash3>
<Flash4></Flash4> <Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities> </Utilities>
<TargetArmAds> <TargetArmAds>
<ArmAdsMisc> <ArmAdsMisc>
@ -2142,6 +2157,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2187,6 +2203,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2232,6 +2249,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2277,6 +2295,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2327,6 +2346,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2372,6 +2392,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2427,6 +2448,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2472,6 +2494,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Cads> <Cads>
@ -2582,6 +2605,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>0</ComprImg>
</CommonProperty> </CommonProperty>
<GroupArmAds> <GroupArmAds>
<Cads> <Cads>
@ -2674,11 +2698,6 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath> <FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</FilePath>
</File> </File>
<File>
<FileName>stm32f10x_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_gpio.c</FilePath>
</File>
<File> <File>
<FileName>stm32f10x_flash.c</FileName> <FileName>stm32f10x_flash.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
@ -2703,6 +2722,7 @@
<StopOnExitCode>11</StopOnExitCode> <StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument> <CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules> <IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty> </CommonProperty>
<FileArmAds> <FileArmAds>
<Aads> <Aads>

6963
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

10
src/board.h
View File

@ -46,6 +46,7 @@ typedef enum {
SENSOR_MAG = 1 << 3, SENSOR_MAG = 1 << 3,
SENSOR_SONAR = 1 << 4, SENSOR_SONAR = 1 << 4,
SENSOR_GPS = 1 << 5, SENSOR_GPS = 1 << 5,
SENSOR_GPSMAG = 1 << 6,
} AvailableSensors; } AvailableSensors;
// Type of accelerometer used/detected // Type of accelerometer used/detected
@ -86,9 +87,16 @@ typedef enum {
typedef enum { typedef enum {
GPS_NMEA = 0, GPS_NMEA = 0,
GPS_UBLOX, GPS_UBLOX,
GPS_MTK, GPS_MTK_NMEA,
GPS_MTK_BINARY,
GPS_MAG_BINARY,
} GPSHardware; } GPSHardware;
typedef enum {
TELEMETRY_UART = 0,
TELEMETRY_SOFTSERIAL,
} TelemetrySerial;
typedef enum { typedef enum {
X = 0, X = 0,
Y, Y,

155
src/cli.c
View File

@ -9,9 +9,11 @@ static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine); static void cliDump(char *cmdLine);
static void cliExit(char *cmdline); static void cliExit(char *cmdline);
static void cliFeature(char *cmdline); static void cliFeature(char *cmdline);
static void cliGpsPassthrough(char *cmdline);
static void cliHelp(char *cmdline); static void cliHelp(char *cmdline);
static void cliMap(char *cmdline); static void cliMap(char *cmdline);
static void cliMixer(char *cmdline); static void cliMixer(char *cmdline);
static void cliMotor(char *cmdline);
static void cliProfile(char *cmdline); static void cliProfile(char *cmdline);
static void cliSave(char *cmdline); static void cliSave(char *cmdline);
static void cliSet(char *cmdline); static void cliSet(char *cmdline);
@ -25,6 +27,9 @@ extern uint8_t accHardware;
// from config.c RC Channel mapping // from config.c RC Channel mapping
extern const char rcChannelLetters[]; extern const char rcChannelLetters[];
// from mixer.c
extern int16_t motor_disarmed[MAX_MOTORS];
// buffer // buffer
static char cliBuffer[48]; static char cliBuffer[48];
static uint32_t bufferIndex = 0; static uint32_t bufferIndex = 0;
@ -37,7 +42,9 @@ static const char * const mixerNames[] = {
"TRI", "QUADP", "QUADX", "BI", "TRI", "QUADP", "QUADX", "BI",
"GIMBAL", "Y6", "HEX6", "GIMBAL", "Y6", "HEX6",
"FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX",
"AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", "CUSTOM", NULL "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4",
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
"CUSTOM", NULL
}; };
// sync this with AvailableFeatures enum from board.h // sync this with AvailableFeatures enum from board.h
@ -50,7 +57,7 @@ static const char * const featureNames[] = {
// sync this with AvailableSensors enum from board.h // sync this with AvailableSensors enum from board.h
static const char * const sensorNames[] = { static const char * const sensorNames[] = {
"ACC", "BARO", "MAG", "SONAR", "GPS", NULL "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
}; };
static const char * const accNames[] = { static const char * const accNames[] = {
@ -71,9 +78,11 @@ const clicmd_t cmdTable[] = {
{ "dump", "print configurable settings in a pastable form", cliDump }, { "dump", "print configurable settings in a pastable form", cliDump },
{ "exit", "", cliExit }, { "exit", "", cliExit },
{ "feature", "list or -val or val", cliFeature }, { "feature", "list or -val or val", cliFeature },
{ "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
{ "help", "", cliHelp }, { "help", "", cliHelp },
{ "map", "mapping of rc channel order", cliMap }, { "map", "mapping of rc channel order", cliMap },
{ "mixer", "mixer name or list", cliMixer }, { "mixer", "mixer name or list", cliMixer },
{ "motor", "get/set motor output value", cliMotor },
{ "profile", "index (0 to 2)", cliProfile }, { "profile", "index (0 to 2)", cliProfile },
{ "save", "save and reboot", cliSave }, { "save", "save and reboot", cliSave },
{ "set", "name=value or blank or * for list", cliSet }, { "set", "name=value or blank or * for list", cliSet },
@ -114,11 +123,14 @@ const clivalue_t valueTable[] = {
{ "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 }, { "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 },
{ "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 },
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
{ "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 },
{ "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 },
{ "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 }, { "softserial_baudrate", VAR_UINT32, &mcfg.softserial_baudrate, 9600, 19200 },
{ "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 }, { "softserial_inverted", VAR_UINT8, &mcfg.softserial_inverted, 0, 1 },
{ "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 }, { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
{ "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, -1, 4 },
{ "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 }, { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 2 },
{ "telemetry_softserial", VAR_UINT8, &mcfg.telemetry_softserial, 0, 1 },
{ "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 }, { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 }, { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
@ -126,13 +138,15 @@ const clivalue_t valueTable[] = {
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 }, { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 }, { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
{ "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 }, { "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
{ "align_board_roll", VAR_INT16, &mcfg.board_align_roll, -180, 360 },
{ "align_board_pitch", VAR_INT16, &mcfg.board_align_pitch, -180, 360 },
{ "align_board_yaw", VAR_INT16, &mcfg.board_align_yaw, -180, 360 },
{ "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 }, { "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 }, { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 }, { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 }, { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_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 }, { "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 },
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
@ -142,7 +156,7 @@ const clivalue_t valueTable[] = {
{ "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 }, { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 }, { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 },
{ "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 }, { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 },
{ "thr_expo", VAR_UINT8, &cfg.thrExpo8, 0, 250 }, { "thr_expo", VAR_UINT8, &cfg.thrExpo8, 0, 100 },
{ "roll_pitch_rate", VAR_UINT8, &cfg.rollPitchRate, 0, 100 }, { "roll_pitch_rate", VAR_UINT8, &cfg.rollPitchRate, 0, 100 },
{ "yawrate", VAR_UINT8, &cfg.yawRate, 0, 100 }, { "yawrate", VAR_UINT8, &cfg.yawRate, 0, 100 },
{ "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 }, { "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
@ -152,28 +166,7 @@ const clivalue_t valueTable[] = {
{ "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 }, { "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 }, { "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
{ "wing_left_min", VAR_UINT16, &cfg.wing_left_min, 0, 2000 },
{ "wing_left_mid", VAR_UINT16, &cfg.wing_left_mid, 0, 2000 },
{ "wing_left_max", VAR_UINT16, &cfg.wing_left_max, 0, 2000 },
{ "wing_right_min", VAR_UINT16, &cfg.wing_right_min, 0, 2000 },
{ "wing_right_mid", VAR_UINT16, &cfg.wing_right_mid, 0, 2000 },
{ "wing_right_max", VAR_UINT16, &cfg.wing_right_max, 0, 2000 },
{ "pitch_direction_l", VAR_INT8, &cfg.pitch_direction_l, -1, 1 },
{ "pitch_direction_r", VAR_INT8, &cfg.pitch_direction_r, -1, 1 },
{ "roll_direction_l", VAR_INT8, &cfg.roll_direction_l, -1, 1 },
{ "roll_direction_r", VAR_INT8, &cfg.roll_direction_r, -1, 1 },
{ "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255}, { "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255},
{ "gimbal_pitch_gain", VAR_INT8, &cfg.gimbal_pitch_gain, -100, 100 },
{ "gimbal_roll_gain", VAR_INT8, &cfg.gimbal_roll_gain, -100, 100 },
{ "gimbal_pitch_min", VAR_UINT16, &cfg.gimbal_pitch_min, 100, 3000 },
{ "gimbal_pitch_max", VAR_UINT16, &cfg.gimbal_pitch_max, 100, 3000 },
{ "gimbal_pitch_mid", VAR_UINT16, &cfg.gimbal_pitch_mid, 100, 3000 },
{ "gimbal_roll_min", VAR_UINT16, &cfg.gimbal_roll_min, 100, 3000 },
{ "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_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
{ "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 }, { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 },
{ "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 }, { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 },
@ -218,7 +211,13 @@ const clivalue_t valueTable[] = {
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
static void cliSetVar(const clivalue_t *var, const int32_t value);
typedef union {
int32_t int_value;
float float_value;
} int_float_value_t;
static void cliSetVar(const clivalue_t *var, const int_float_value_t value);
static void cliPrintVar(const clivalue_t *var, uint32_t full); static void cliPrintVar(const clivalue_t *var, uint32_t full);
static void cliPrint(const char *str); static void cliPrint(const char *str);
static void cliWrite(uint8_t ch); static void cliWrite(uint8_t ch);
@ -279,7 +278,7 @@ char *itoa(int i, char *a, int r)
static float _atof(const char *p) static float _atof(const char *p)
{ {
int frac = 0; int frac = 0;
double sign, value, scale; float sign, value, scale;
// Skip leading white space, if any. // Skip leading white space, if any.
while (white_space(*p) ) { while (white_space(*p) ) {
@ -287,9 +286,9 @@ static float _atof(const char *p)
} }
// Get sign, if any. // Get sign, if any.
sign = 1.0; sign = 1.0f;
if (*p == '-') { if (*p == '-') {
sign = -1.0; sign = -1.0f;
p += 1; p += 1;
} else if (*p == '+') { } else if (*p == '+') {
@ -297,26 +296,26 @@ static float _atof(const char *p)
} }
// Get digits before decimal point or exponent, if any. // Get digits before decimal point or exponent, if any.
value = 0.0; value = 0.0f;
while (valid_digit(*p)) { while (valid_digit(*p)) {
value = value * 10.0 + (*p - '0'); value = value * 10.0f + (*p - '0');
p += 1; p += 1;
} }
// Get digits after decimal point, if any. // Get digits after decimal point, if any.
if (*p == '.') { if (*p == '.') {
double pow10 = 10.0; float pow10 = 10.0f;
p += 1; p += 1;
while (valid_digit(*p)) { while (valid_digit(*p)) {
value += (*p - '0') / pow10; value += (*p - '0') / pow10;
pow10 *= 10.0; pow10 *= 10.0f;
p += 1; p += 1;
} }
} }
// Handle exponent, if any. // Handle exponent, if any.
scale = 1.0; scale = 1.0f;
if ((*p == 'e') || (*p == 'E')) { if ((*p == 'e') || (*p == 'E')) {
unsigned int expon; unsigned int expon;
p += 1; p += 1;
@ -337,12 +336,13 @@ static float _atof(const char *p)
expon = expon * 10 + (*p - '0'); expon = expon * 10 + (*p - '0');
p += 1; p += 1;
} }
if (expon > 308) expon = 308; if (expon > 308)
expon = 308;
// Calculate scaling factor. // Calculate scaling factor.
while (expon >= 50) { scale *= 1E50; expon -= 50; } // while (expon >= 50) { scale *= 1E50f; expon -= 50; }
while (expon >= 8) { scale *= 1E8; expon -= 8; } while (expon >= 8) { scale *= 1E8f; expon -= 8; }
while (expon > 0) { scale *= 10.0; expon -= 1; } while (expon > 0) { scale *= 10.0f; expon -= 1; }
} }
// Return signed and scaled floating point result. // Return signed and scaled floating point result.
@ -466,7 +466,7 @@ static void cliCMix(char *cmdline)
} }
cliPrint("Sanity check:\t"); cliPrint("Sanity check:\t");
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++)
cliPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t"); cliPrint(fabsf(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
cliPrint("\r\n"); cliPrint("\r\n");
return; return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) { } else if (strncasecmp(cmdline, "reset", 5) == 0) {
@ -611,6 +611,8 @@ static void cliExit(char *cmdline)
*cliBuffer = '\0'; *cliBuffer = '\0';
bufferIndex = 0; bufferIndex = 0;
cliMode = 0; cliMode = 0;
// incase some idiot leaves a motor running during motortest, clear it here
mixerResetMotors();
// save and reboot... I think this makes the most sense // save and reboot... I think this makes the most sense
cliSave(cmdline); cliSave(cmdline);
} }
@ -671,6 +673,14 @@ static void cliFeature(char *cmdline)
} }
} }
static void cliGpsPassthrough(char *cmdline)
{
if (gpsSetPassthrough() == -1)
cliPrint("Error: Enable and plug in GPS first\r\n");
else
cliPrint("Enabling GPS passthrough...\r\n");
}
static void cliHelp(char *cmdline) static void cliHelp(char *cmdline)
{ {
uint32_t i = 0; uint32_t i = 0;
@ -741,6 +751,52 @@ static void cliMixer(char *cmdline)
} }
} }
static void cliMotor(char *cmdline)
{
int motor_index = 0;
int motor_value = 0;
int len, index = 0;
char *pch = NULL;
len = strlen(cmdline);
if (len == 0) {
printf("Usage:\r\nmotor index [value] - show [or set] motor value\r\n");
return;
}
pch = strtok(cmdline, " ");
while (pch != NULL) {
switch (index) {
case 0:
motor_index = atoi(pch);
break;
case 1:
motor_value = atoi(pch);
break;
}
index++;
pch = strtok(NULL, " ");
}
if (motor_index < 0 || motor_index >= MAX_MOTORS) {
printf("No such motor, use a number [0, %d]\r\n", MAX_MOTORS);
return;
}
if (index < 2) {
printf("Motor %d is set at %d\r\n", motor_index, motor_disarmed[motor_index]);
return;
}
if (motor_value < 1000 || motor_value > 2000) {
printf("Invalid motor value, 1000..2000\r\n");
return;
}
printf("Setting motor %d to %d\r\n", motor_index, motor_value);
motor_disarmed[motor_index] = motor_value;
}
static void cliProfile(char *cmdline) static void cliProfile(char *cmdline)
{ {
uint8_t len; uint8_t len;
@ -819,25 +875,25 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
printf(" %d %d", var->min, var->max); printf(" %d %d", var->min, var->max);
} }
static void cliSetVar(const clivalue_t *var, const int32_t value) static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
{ {
switch (var->type) { switch (var->type) {
case VAR_UINT8: case VAR_UINT8:
case VAR_INT8: case VAR_INT8:
*(char *)var->ptr = (char)value; *(char *)var->ptr = (char)value.int_value;
break; break;
case VAR_UINT16: case VAR_UINT16:
case VAR_INT16: case VAR_INT16:
*(short *)var->ptr = (short)value; *(short *)var->ptr = (short)value.int_value;
break; break;
case VAR_UINT32: case VAR_UINT32:
*(int *)var->ptr = (int)value; *(int *)var->ptr = (int)value.int_value;
break; break;
case VAR_FLOAT: case VAR_FLOAT:
*(float *)var->ptr = *(float *)&value; *(float *)var->ptr = (float)value.float_value;
break; break;
} }
} }
@ -871,7 +927,12 @@ static void cliSet(char *cmdline)
val = &valueTable[i]; val = &valueTable[i];
if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0) { if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0) {
if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT? if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT?
cliSetVar(val, valueTable[i].type == VAR_FLOAT ? *(uint32_t *)&valuef : value); // this is a silly dirty hack. please fix me later. int_float_value_t tmp;
if (valueTable[i].type == VAR_FLOAT)
tmp.float_value = valuef;
else
tmp.int_value = value;
cliSetVar(val, tmp);
printf("%s set to ", valueTable[i].name); printf("%s set to ", valueTable[i].name);
cliPrintVar(val, 0); cliPrintVar(val, 0);
} else { } else {
@ -913,7 +974,7 @@ static void cliStatus(char *cmdline)
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]); printf("ACCHW: %s", accNames[accHardware]);
if (accHardware == ACC_MPU6050) if (accHardware == ACC_MPU6050)
printf(".%c", mcfg.mpu6050_scale ? 'o' : 'n'); printf(".%c", core.mpu6050_scale ? 'o' : 'n');
} }
cliPrint("\r\n"); cliPrint("\r\n");

48
src/config.c
View File

@ -7,13 +7,13 @@
#endif #endif
#define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
master_t mcfg; // master config struct with data independent from profiles master_t mcfg; // master config struct with data independent from profiles
config_t cfg; // profile config struct config_t cfg; // profile config struct
const char rcChannelLetters[] = "AERT1234"; const char rcChannelLetters[] = "AERT1234";
static const uint8_t EEPROM_CONF_VERSION = 52; static const uint8_t EEPROM_CONF_VERSION = 55;
static uint32_t enabledSensors = 0; static uint32_t enabledSensors = 0;
static void resetConf(void); static void resetConf(void);
@ -69,23 +69,22 @@ void readEEPROM(void)
mcfg.current_profile = 0; mcfg.current_profile = 0;
memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t));
for (i = 0; i < 6; i++) for (i = 0; i < PITCH_LOOKUP_LENGTH; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
for (i = 0; i < 11; i++) { for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
int16_t tmp = 10 * i - cfg.thrMid8; int16_t tmp = 10 * i - cfg.thrMid8;
uint8_t y = 1; uint8_t y = 1;
if (tmp > 0) if (tmp > 0)
y = 100 - cfg.thrMid8; y = 100 - cfg.thrMid8;
if (tmp < 0) if (tmp < 0)
y = cfg.thrMid8; y = cfg.thrMid8;
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000] lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
} }
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
setPIDController(cfg.pidController); setPIDController(cfg.pidController);
GPS_set_pids(); gpsSetPIDs();
} }
void writeEEPROM(uint8_t b, uint8_t updateProfile) void writeEEPROM(uint8_t b, uint8_t updateProfile)
@ -181,6 +180,9 @@ static void resetConf(void)
mcfg.gyro_align = ALIGN_DEFAULT; mcfg.gyro_align = ALIGN_DEFAULT;
mcfg.acc_align = ALIGN_DEFAULT; mcfg.acc_align = ALIGN_DEFAULT;
mcfg.mag_align = ALIGN_DEFAULT; mcfg.mag_align = ALIGN_DEFAULT;
mcfg.board_align_roll = 0;
mcfg.board_align_pitch = 0;
mcfg.board_align_yaw = 0;
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.yaw_control_direction = 1; mcfg.yaw_control_direction = 1;
mcfg.moron_threshold = 32; mcfg.moron_threshold = 32;
@ -190,6 +192,7 @@ static void resetConf(void)
mcfg.vbatmincellvoltage = 33; mcfg.vbatmincellvoltage = 33;
mcfg.power_adc_channel = 0; mcfg.power_adc_channel = 0;
mcfg.serialrx_type = 0; mcfg.serialrx_type = 0;
mcfg.telemetry_softserial = 0;
mcfg.midrc = 1500; mcfg.midrc = 1500;
mcfg.mincheck = 1100; mcfg.mincheck = 1100;
mcfg.maxcheck = 1900; mcfg.maxcheck = 1900;
@ -206,7 +209,7 @@ static void resetConf(void)
mcfg.servo_pwm_rate = 50; mcfg.servo_pwm_rate = 50;
// gps/nav stuff // gps/nav stuff
mcfg.gps_type = GPS_NMEA; mcfg.gps_type = GPS_NMEA;
mcfg.gps_baudrate = 115200; mcfg.gps_baudrate = 0;
// serial (USART1) baudrate // serial (USART1) baudrate
mcfg.serial_baudrate = 115200; mcfg.serial_baudrate = 115200;
mcfg.softserial_baudrate = 19200; mcfg.softserial_baudrate = 19200;
@ -224,9 +227,9 @@ static void resetConf(void)
cfg.P8[YAW] = 85; cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45; cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0; cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 50; cfg.P8[PIDALT] = 40;
cfg.I8[PIDALT] = 25; cfg.I8[PIDALT] = 25;
cfg.D8[PIDALT] = 80; cfg.D8[PIDALT] = 60;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0; cfg.D8[PIDPOS] = 0;
@ -288,32 +291,9 @@ static void resetConf(void)
cfg.yaw_direction = 1; cfg.yaw_direction = 1;
cfg.tri_unarmed_servo = 1; cfg.tri_unarmed_servo = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
// flying wing
cfg.wing_left_min = 1020;
cfg.wing_left_mid = 1500;
cfg.wing_left_max = 2000;
cfg.wing_right_min = 1020;
cfg.wing_right_mid = 1500;
cfg.wing_right_max = 2000;
cfg.pitch_direction_l = 1;
cfg.pitch_direction_r = -1;
cfg.roll_direction_l = 1;
cfg.roll_direction_r = 1;
// gimbal // gimbal
cfg.gimbal_pitch_gain = 10;
cfg.gimbal_roll_gain = 10;
cfg.gimbal_flags = GIMBAL_NORMAL; cfg.gimbal_flags = GIMBAL_NORMAL;
cfg.gimbal_pitch_min = 1020;
cfg.gimbal_pitch_max = 2000;
cfg.gimbal_pitch_mid = 1500;
cfg.gimbal_roll_min = 1020;
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff // gps/nav stuff
cfg.gps_wp_radius = 200; cfg.gps_wp_radius = 200;

2
src/drv_adxl345.c
View File

@ -35,7 +35,7 @@ static void adxl345Init(sensor_align_e align);
static void adxl345Read(int16_t *accelData); static void adxl345Read(int16_t *accelData);
static bool useFifo = false; static bool useFifo = false;
static sensor_align_e accAlign = CW0_DEG; static sensor_align_e accAlign = CW270_DEG;
bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc) bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
{ {

2
src/drv_bmp085.c
View File

@ -115,7 +115,7 @@ bool bmp085Detect(baro_t *baro)
#endif #endif
// EXTI interrupt for barometer EOC // EXTI interrupt for barometer EOC
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14); gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
EXTI_InitStructure.EXTI_Line = EXTI_Line14; EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;

9
src/drv_gpio.c
View File

@ -31,3 +31,12 @@ void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
} }
} }
} }
void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc)
{
uint32_t tmp = 0x00;
tmp = ((uint32_t)0x0F) << (0x04 * (pinsrc & (uint8_t)0x03));
AFIO->EXTICR[pinsrc >> 0x02] &= ~tmp;
AFIO->EXTICR[pinsrc >> 0x02] |= (((uint32_t)portsrc) << (0x04 * (pinsrc & (uint8_t)0x03)));
}

1
src/drv_gpio.h
View File

@ -53,3 +53,4 @@ typedef struct
#define digitalIn(p, i) (p->IDR & i) #define digitalIn(p, i) (p->IDR & i)
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config); void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc);

2
src/drv_hcsr04.c
View File

@ -90,7 +90,7 @@ void hcsr04_init(sonar_config_t config)
gpioInit(GPIOB, &gpio); gpioInit(GPIOB, &gpio);
// setup external interrupt on echo pin // setup external interrupt on echo pin
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, exti_pin_source); gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
EXTI_ClearITPendingBit(exti_line); EXTI_ClearITPendingBit(exti_line);

6
src/drv_hmc5883l.c
View File

@ -157,9 +157,9 @@ void hmc5883lInit(void)
LED1_TOGGLE; LED1_TOGGLE;
} }
magGain[X] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]); magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]); magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode // leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode

2
src/drv_i2c_soft.c
View File

@ -131,7 +131,7 @@ static uint8_t I2C_ReceiveByte(void)
void i2cInit(I2C_TypeDef * I2C) void i2cInit(I2C_TypeDef * I2C)
{ {
GPIO_Config gpio; gpio_config_t gpio;
gpio.pin = Pin_10 | Pin_11; gpio.pin = Pin_10 | Pin_11;
gpio.speed = Speed_2MHz; gpio.speed = Speed_2MHz;

4
src/drv_pwm.c
View File

@ -254,7 +254,7 @@ static void ppmCallback(uint8_t port, uint16_t capture)
if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
chan = 0; chan = 0;
} else { } else {
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range if (diff > 750 && diff < 2250 && chan < MAX_INPUTS) { // 750 to 2250 ms is our 'valid' channel range
captures[chan] = diff; captures[chan] = diff;
if (chan < 4 && diff > failsafeThreshold) if (chan < 4 && diff > failsafeThreshold)
GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK
@ -354,7 +354,7 @@ bool pwmInit(drv_pwm_config_t *init)
pwmInConfig(port, pwmCallback, numInputs); pwmInConfig(port, pwmCallback, numInputs);
numInputs++; numInputs++;
} else if (mask & TYPE_M) { } else if (mask & TYPE_M) {
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, PULSE_1MS); motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, init->idlePulse > 0 ? init->idlePulse : PULSE_1MS);
} else if (mask & TYPE_S) { } else if (mask & TYPE_S) {
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS); servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS);
} }

2
src/drv_pwm.h
View File

@ -15,6 +15,8 @@ typedef struct drv_pwm_config_t {
uint8_t adcChannel; // steal one RC input for current sensor uint8_t adcChannel; // steal one RC input for current sensor
uint16_t motorPwmRate; uint16_t motorPwmRate;
uint16_t servoPwmRate; uint16_t servoPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver;
// default of zero means PULSE_1MS, otherwise set to given value. Used by 3D mode.
uint16_t failsafeThreshold; uint16_t failsafeThreshold;
} drv_pwm_config_t; } drv_pwm_config_t;

48
src/drv_system.c
View File

@ -103,7 +103,6 @@ void systemInit(void)
systemBeepPtr = beepRev4; systemBeepPtr = beepRev4;
BEEP_OFF; BEEP_OFF;
#endif #endif
LED0_OFF; LED0_OFF;
LED1_OFF; LED1_OFF;
@ -224,50 +223,3 @@ void systemBeep(bool onoff)
#endif #endif
} }
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
{
switch (rotation) {
case CW0_DEG:
dest[X] = src[X];
dest[Y] = src[Y];
dest[Z] = src[Z];
break;
case CW90_DEG:
dest[X] = src[Y];
dest[Y] = -src[X];
dest[Z] = src[Z];
break;
case CW180_DEG:
dest[X] = -src[X];
dest[Y] = -src[Y];
dest[Z] = src[Z];
break;
case CW270_DEG:
dest[X] = -src[Y];
dest[Y] = src[X];
dest[Z] = src[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -src[X];
dest[Y] = src[Y];
dest[Z] = -src[Z];
break;
case CW90_DEG_FLIP:
dest[X] = src[Y];
dest[Y] = src[X];
dest[Z] = -src[Z];
break;
case CW180_DEG_FLIP:
dest[X] = src[X];
dest[Y] = -src[Y];
dest[Z] = -src[Z];
break;
case CW270_DEG_FLIP:
dest[X] = -src[Y];
dest[Y] = -src[X];
dest[Z] = -src[Z];
break;
default:
break;
}
}

3
src/drv_system.h
View File

@ -15,6 +15,3 @@ void systemReset(bool toBootloader);
// current crystal frequency - 8 or 12MHz // current crystal frequency - 8 or 12MHz
extern uint32_t hse_value; extern uint32_t hse_value;
// sensor orientation
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);

331
src/gps.c
View File

@ -5,105 +5,213 @@
#define sq(x) ((x)*(x)) #define sq(x) ((x)*(x))
#endif #endif
const uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 }; // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2.5second)
#define GPS_TIMEOUT (2500)
// How many entries in gpsInitData array below
#define GPS_INIT_ENTRIES (5)
static void GPS_NewData(uint16_t c); typedef struct gpsInitData_t {
static void gpsPrint(const char *str); uint32_t baudrate;
const char *ubx;
const char *mtk;
} gpsInitData_t;
#define UBX_INIT_STRING_INDEX 0 static const gpsInitData_t gpsInitData[] = {
#define MTK_INIT_STRING_INDEX 4 { 115200, "$PUBX,41,1,0003,0001,115200,0*1E\r\n", "$PMTK251,115200*1F\r\n" },
{ 57600, "$PUBX,41,1,0003,0001,57600,0*2D\r\n", "$PMTK251,57600*2C\r\n" },
static const char * const gpsInitStrings[] = { { 38400, "$PUBX,41,1,0003,0001,38400,0*26\r\n", "$PMTK251,38400*27\r\n" },
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3 { 19200, "$PUBX,41,1,0003,0001,19200,0*23\r\n", "$PMTK251,19200*22\r\n" },
"$PUBX,41,1,0003,0001,38400,0*26\r\n", // 9600 is not enough for 5Hz updates - leave for compatibility to dumb NMEA that only runs at this speed
"$PUBX,41,1,0003,0001,57600,0*2D\r\n", { 9600, "", "" }
"$PUBX,41,1,0003,0001,115200,0*1E\r\n",
"$PMTK251,19200*22\r\n", // MTK4..7
"$PMTK251,38400*27\r\n",
"$PMTK251,57600*2C\r\n",
"$PMTK251,115200*1F\r\n",
}; };
static const uint8_t ubloxInit[] = { static const uint8_t ubloxInit[] = {
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // disable all default NMEA messages 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // VGS: Course over ground and Ground speed
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15, // GSV: GNSS Satellites in View
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11, // GLL: Latitude and longitude, with time of position fix and status
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F, // GGA: Global positioning system fix data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13, // GSA: GNSS DOP and Active Satellites
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17, 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17, // RMC: Recommended Minimum data
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47, // set POSLLH MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49, // set STATUS MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F, // set SOL MSG rate
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67, // set VELNED MSG rate
0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41, // set WAAS to EGNOS 0xB5, 0x62, 0x06, 0x16, 0x08, 0x00, 0x03, 0x07, 0x03, 0x00, 0x51, 0x08, 0x00, 0x00, 0x8A, 0x41, // set WAAS to EGNOS
0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A // set rate to 5Hz 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A, // set rate to 5Hz
}; };
void gpsInit(uint32_t baudrate) static const char *mtkNMEAInit[] = {
"$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n", // only GGA and RMC sentence
"$PMTK220,200*2C\r\n" // 5 Hz update rate
};
static const char *mtkBinaryInit[] = {
"$PMTK319,1*24\r\n", // SBAS Integrity Mode
"$PMTK220,200*2C\r\n", // 5 Hz update rate
"$PMTK397,0*23\r\n", // NAVTHRES_OFF
"$PMTK313,1*2E\r\n", // SBAS_ON
"$PMTK301,2*2E\r\n", // WAAS_ON
"$PGCMD,16,0,0,0,0,0*6A\r\n" // Binary ON
};
enum {
GPS_UNKNOWN,
GPS_INITIALIZING,
GPS_INITDONE,
GPS_RECEIVINGDATA,
GPS_LOSTCOMMS,
};
typedef struct gpsData_t {
uint8_t state; // GPS thread state. Used for detecting cable disconnects and configuring attached devices
uint8_t baudrateIndex; // index into auto-detecting or current baudrate
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
uint32_t lastMessage; // last time valid GPS data was received (millis)
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
uint32_t state_position; // incremental variable for loops
} gpsData_t;
gpsData_t gpsData;
static void gpsNewData(uint16_t c);
static bool gpsNewFrameNMEA(char c);
static bool gpsNewFrameUBLOX(uint8_t data);
static void gpsSetState(uint8_t state)
{ {
int i; gpsData.state = state;
int offset = 0; gpsData.state_position = 0;
GPS_set_pids();
core.gpsport = uartOpen(USART2, GPS_NewData, baudrate, MODE_RXTX);
if (mcfg.gps_type == GPS_UBLOX)
offset = UBX_INIT_STRING_INDEX;
else if (mcfg.gps_type == GPS_MTK)
offset = MTK_INIT_STRING_INDEX;
if (mcfg.gps_type != GPS_NMEA) {
for (i = 0; i < 5; i++) {
serialSetBaudRate(core.gpsport, init_speed[i]);
// verify the requested change took effect.
baudrate = serialGetBaudRate(core.gpsport);
switch (baudrate) {
case 19200:
gpsPrint(gpsInitStrings[offset]);
break;
case 38400:
gpsPrint(gpsInitStrings[offset + 1]);
break;
case 57600:
gpsPrint(gpsInitStrings[offset + 2]);
break;
case 115200:
gpsPrint(gpsInitStrings[offset + 3]);
break;
}
delay(10);
}
} }
serialSetBaudRate(core.gpsport, baudrate); void gpsInit(uint8_t baudrate)
if (mcfg.gps_type == GPS_UBLOX) {
for (i = 0; i < sizeof(ubloxInit); i++) {
serialWrite(core.gpsport, ubloxInit[i]); // send ubx init binary
delay(4);
}
} else if (mcfg.gps_type == GPS_MTK) {
gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence
gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate
}
// catch some GPS frames. TODO check this
delay(1000);
if (GPS_Present)
sensorsSet(SENSOR_GPS);
}
static void gpsPrint(const char *str)
{ {
while (*str) { portMode_t mode = MODE_RXTX;
serialWrite(core.gpsport, *str);
if (mcfg.gps_type == GPS_UBLOX) // init gpsData structure. if we're not actually enabled, don't bother doing anything else
delay(4); gpsSetState(GPS_UNKNOWN);
str++; if (!feature(FEATURE_GPS))
return;
gpsData.baudrateIndex = baudrate;
gpsData.lastMessage = millis();
gpsData.errors = 0;
// only RX is needed for NMEA-style GPS
if (mcfg.gps_type == GPS_NMEA)
mode = MODE_RX;
gpsSetPIDs();
core.gpsport = uartOpen(USART2, gpsNewData, gpsInitData[baudrate].baudrate, mode);
// signal GPS "thread" to initialize when it gets to it
gpsSetState(GPS_INITIALIZING);
} }
// wait to send all
while (!isSerialTransmitBufferEmpty(core.gpsport)); void gpsInitHardware(void)
delay(30); {
switch (mcfg.gps_type) {
case GPS_NMEA:
// nothing to do, just set baud rate and try receiving some stuff and see if it parses
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
gpsSetState(GPS_RECEIVINGDATA);
return;
case GPS_UBLOX:
// UBX will run at mcfg.baudrate, it shouldn't be "autodetected". So here we force it to that rate
// Wait until GPS transmit buffer is empty
if (!isSerialTransmitBufferEmpty(core.gpsport))
break;
if (gpsData.state == GPS_INITIALIZING) {
if (gpsData.state_position < GPS_INIT_ENTRIES) {
// try different speed to INIT
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.state_position].baudrate);
// but print our FIXED init string for the baudrate we want to be at
serialPrint(core.gpsport, gpsInitData[mcfg.gps_baudrate].ubx);
gpsData.state_position++;
} else {
// we're now (hopefully) at the correct rate, next state will switch to it
gpsData.baudrateIndex = mcfg.gps_baudrate;
gpsSetState(GPS_INITDONE);
} }
} else {
// GPS_INITDONE, set our real baud rate and push some ublox config strings
if (gpsData.state_position == 0)
serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(core.gpsport, ubloxInit[gpsData.state_position]); // send ubx init binary
gpsData.state_position++;
} else {
// ublox should be init'd, time to try receiving some junk
gpsSetState(GPS_RECEIVINGDATA);
}
}
break;
case GPS_MTK_NMEA:
case GPS_MTK_BINARY:
// TODO. need to find my old piece of shit MTK GPS.
break;
}
// clear error counter
gpsData.errors = 0;
}
void gpsThread(void)
{
switch (gpsData.state) {
case GPS_UNKNOWN:
break;
case GPS_INITIALIZING:
case GPS_INITDONE:
gpsInitHardware();
break;
case GPS_LOSTCOMMS:
gpsData.errors++;
// try another rate
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
f.GPS_FIX = 0;
gpsSetState(GPS_INITIALIZING);
break;
case GPS_RECEIVINGDATA:
// check for no data/gps timeout/cable disconnection etc
if (millis() - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOSTCOMMS);
}
break;
}
}
static bool gpsNewFrame(uint8_t c)
{
switch (mcfg.gps_type) {
case GPS_NMEA: // NMEA
case GPS_MTK_NMEA: // MTK in NMEA mode
return gpsNewFrameNMEA(c);
case GPS_UBLOX: // UBX binary
return gpsNewFrameUBLOX(c);
case GPS_MTK_BINARY: // MTK in BINARY mode (TODO)
return false;
}
return false;
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* *
@ -130,9 +238,6 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
static void GPS_calc_poshold(void); static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(int max_speed); static void GPS_calc_nav_rate(int max_speed);
static void GPS_update_crosstrack(void); static void GPS_update_crosstrack(void);
static bool GPS_newFrame(char c);
static bool GPS_NMEA_newFrame(char c);
static bool GPS_UBLOX_newFrame(uint8_t data);
static bool UBLOX_parse_gps(void); static bool UBLOX_parse_gps(void);
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow); static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);
@ -285,7 +390,7 @@ static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing; static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c) static void gpsNewData(uint16_t c)
{ {
int axis; int axis;
static uint32_t nav_loopTimer; static uint32_t nav_loopTimer;
@ -293,7 +398,11 @@ void GPS_NewData(uint16_t c)
int32_t dir; int32_t dir;
int16_t speed; int16_t speed;
if (GPS_newFrame(c)) { if (gpsNewFrame(c)) {
// new data received and parsed, we're in business
gpsData.lastLastMessage = gpsData.lastMessage;
gpsData.lastMessage = millis();
sensorsSet(SENSOR_GPS);
if (GPS_update == 1) if (GPS_update == 1)
GPS_update = 0; GPS_update = 0;
else else
@ -411,7 +520,7 @@ void GPS_reset_nav(void)
} }
// Get the relevant P I D values and set the PID controllers // Get the relevant P I D values and set the PID controllers
void GPS_set_pids(void) void gpsSetPIDs(void)
{ {
posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f; posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f;
posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f; posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f;
@ -428,6 +537,31 @@ void GPS_set_pids(void)
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
} }
int8_t gpsSetPassthrough(void)
{
if (gpsData.state != GPS_RECEIVINGDATA)
return -1;
// get rid of callback
core.gpsport->callback = NULL;
LED0_OFF;
LED1_OFF;
while(1) {
if (serialTotalBytesWaiting(core.gpsport)) {
LED0_ON;
serialWrite(core.mainport, serialRead(core.gpsport));
LED0_OFF;
}
if (serialTotalBytesWaiting(core.mainport)) {
LED1_ON;
serialWrite(core.gpsport, serialRead(core.mainport));
LED1_OFF;
}
}
}
// OK here is the onboard GPS code // OK here is the onboard GPS code
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
@ -777,19 +911,6 @@ static uint8_t hex_c(uint8_t n)
return n; return n;
} }
static bool GPS_newFrame(char c)
{
switch (mcfg.gps_type) {
case GPS_NMEA: // NMEA
case GPS_MTK: // MTK outputs NMEA too
return GPS_NMEA_newFrame(c);
case GPS_UBLOX: // UBX
return GPS_UBLOX_newFrame(c);
}
return false;
}
/* This is a light implementation of a GPS frame decoding /* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output NMEA frames. This should work with most of modern GPS devices configured to output NMEA frames.
It assumes there are some NMEA GGA frames to decode on the serial bus It assumes there are some NMEA GGA frames to decode on the serial bus
@ -805,7 +926,7 @@ static bool GPS_newFrame(char c)
#define FRAME_GGA 1 #define FRAME_GGA 1
#define FRAME_RMC 2 #define FRAME_RMC 2
static bool GPS_NMEA_newFrame(char c) static bool gpsNewFrameNMEA(char c)
{ {
uint8_t frameOK = 0; uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0; static uint8_t param = 0, offset = 0, parity = 0;
@ -868,8 +989,6 @@ static bool GPS_NMEA_newFrame(char c)
if (!checksum_param) if (!checksum_param)
parity ^= c; parity ^= c;
} }
if (frame)
GPS_Present = 1;
return frameOK && (frame == FRAME_GGA); return frameOK && (frame == FRAME_GGA);
} }
@ -1026,7 +1145,7 @@ void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
} }
} }
static bool GPS_UBLOX_newFrame(uint8_t data) static bool gpsNewFrameUBLOX(uint8_t data)
{ {
bool parsed = false; bool parsed = false;
@ -1083,10 +1202,8 @@ static bool GPS_UBLOX_newFrame(uint8_t data)
_step = 0; _step = 0;
if (_ck_b != data) if (_ck_b != data)
break; // bad checksum break; // bad checksum
GPS_Present = 1; if (UBLOX_parse_gps())
if (UBLOX_parse_gps()) {
parsed = true; parsed = true;
}
} //end switch } //end switch
return parsed; return parsed;
} }

53
src/imu.c
View File

@ -26,7 +26,7 @@ float accVelScale;
int16_t gyroData[3] = { 0, 0, 0 }; int16_t gyroData[3] = { 0, 0, 0 };
int16_t gyroZero[3] = { 0, 0, 0 }; int16_t gyroZero[3] = { 0, 0, 0 };
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0, 0 }; // absolute angle inclination in radians float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
static void getEstimatedAttitude(void); static void getEstimatedAttitude(void);
@ -67,12 +67,14 @@ void computeIMU(void)
Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff; Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
} }
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1) / Smoothing[axis]); gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroADC[axis] + 1) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis]; gyroSmooth[axis] = gyroData[axis];
} }
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) { } else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3; gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3;
gyroYawSmooth = gyroData[YAW]; gyroYawSmooth = gyroData[YAW];
gyroData[ROLL] = gyroADC[ROLL];
gyroData[PITCH] = gyroADC[PITCH];
} else { } else {
for (axis = 0; axis < 3; axis++) for (axis = 0; axis < 3; axis++)
gyroData[axis] = gyroADC[axis]; gyroData[axis] = gyroADC[axis];
@ -172,17 +174,21 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value; return value;
} }
#define F_CUT_ACCZ 20.0f
static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
// rotate acc into Earth frame and calculate acceleration in it // rotate acc into Earth frame and calculate acceleration in it
void acc_calc(uint32_t deltaT) void acc_calc(uint32_t deltaT)
{ {
static int32_t accZoffset = 0; static int32_t accZoffset = 0;
static float accz_smooth;
float rpy[3]; float rpy[3];
t_fp_vector accel_ned; t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame // the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL]; rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH]; rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RADX10 * 10.0f; rpy[2] = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = accSmooth[1];
@ -199,18 +205,20 @@ void acc_calc(uint32_t deltaT)
} else } else
accel_ned.V.Z -= acc_1G; accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence // apply Deadband to reduce integration drift and vibration influence
accel_ned.V.Z = applyDeadband(accel_ned.V.Z, cfg.accz_deadband); accel_ned.V.Z = applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
accel_ned.V.X = applyDeadband(accel_ned.V.X, cfg.accxy_deadband); accel_ned.V.X = applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accel_ned.V.Y = applyDeadband(accel_ned.V.Y, cfg.accxy_deadband); accel_ned.V.Y = applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
// sum up Values for later integration to get velocity and distance // sum up Values for later integration to get velocity and distance
accTimeSum += deltaT; accTimeSum += deltaT;
accSumCount++; accSumCount++;
accSum[0] += accel_ned.V.X; accSum[X] += lrintf(accel_ned.V.X);
accSum[1] += accel_ned.V.Y; accSum[Y] += lrintf(accel_ned.V.Y);
accSum[2] += accel_ned.V.Z; accSum[Z] += lrintf(accel_ned.V.Z);
} }
void accSum_reset(void) void accSum_reset(void)
@ -288,7 +296,7 @@ static void getEstimatedAttitude(void)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR; EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
} }
if (abs(EstG.A[Z]) > accZ_25deg) if (EstG.A[Z] > accZ_25deg)
f.SMALL_ANGLES_25 = 1; f.SMALL_ANGLES_25 = 1;
else else
f.SMALL_ANGLES_25 = 0; f.SMALL_ANGLES_25 = 0;
@ -307,7 +315,7 @@ static void getEstimatedAttitude(void)
acc_calc(deltaT); // rotate acc vector into earth frame acc_calc(deltaT); // rotate acc vector into earth frame
if (cfg.throttle_angle_correction) { if (cfg.throttle_angle_correction) {
int cosZ = EstG.V.Z / acc_1G * 100.0f; int cosZ = EstG.V.Z / (acc_1G * 100.0f);
throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8; throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8;
} }
} }
@ -317,7 +325,6 @@ static void getEstimatedAttitude(void)
int getEstimatedAltitude(void) int getEstimatedAltitude(void)
{ {
static int32_t baroGroundPressure;
static uint32_t previousT; static uint32_t previousT;
uint32_t currentT = micros(); uint32_t currentT = micros();
uint32_t dTime; uint32_t dTime;
@ -326,11 +333,12 @@ int getEstimatedAltitude(void)
int32_t vel_tmp; int32_t vel_tmp;
int32_t BaroAlt_tmp; int32_t BaroAlt_tmp;
float dt; float dt;
float PressureScaling;
float vel_acc; float vel_acc;
static float vel = 0.0f; static float vel = 0.0f;
static float accAlt = 0.0f; static float accAlt = 0.0f;
static int32_t lastBaroAlt; static int32_t lastBaroAlt;
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
dTime = currentT - previousT; dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL) if (dTime < UPDATE_INTERVAL)
@ -338,19 +346,22 @@ int getEstimatedAltitude(void)
previousT = currentT; previousT = currentT;
if (calibratingB > 0) { if (calibratingB > 0) {
baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1); baroGroundPressure -= baroGroundPressure / 8;
calibratingB--; baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
vel = 0; vel = 0;
accAlt = 0; accAlt = 0;
calibratingB--;
} }
// calculates height from ground via baro readings // calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
PressureScaling = (float)baroPressureSum / ((float)baroGroundPressure * (float)(cfg.baro_tab_size - 1)); BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp = 153.8462f * (baroTemperature + 27315) * (1.0f - expf(0.190259f * logf(PressureScaling))); // in cm BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = (float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise
dt = accTimeSum * 1e-6; // delta acc reading time in seconds dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec // Integrator - velocity, cm/sec
vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount; vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;
@ -391,7 +402,7 @@ int getEstimatedAltitude(void)
vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h) vel = constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
// D // D
vel_tmp = vel; vel_tmp = lrintf(vel);
vel_tmp = applyDeadband(vel_tmp, 5); vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp; vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150); BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);

20
src/main.c
View File

@ -30,7 +30,9 @@ int main(void)
uint8_t i; uint8_t i;
drv_pwm_config_t pwm_params; drv_pwm_config_t pwm_params;
drv_adc_config_t adc_params; drv_adc_config_t adc_params;
#ifdef SOFTSERIAL_LOOPBACK
serialPort_t* loopbackPort = NULL; serialPort_t* loopbackPort = NULL;
#endif
systemInit(); systemInit();
#ifdef USE_LAME_PRINTF #ifdef USE_LAME_PRINTF
@ -49,6 +51,7 @@ int main(void)
} }
adcInit(&adc_params); adcInit(&adc_params);
initBoardAlignment();
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET); sensorsSet(SENSORS_SET);
@ -67,6 +70,7 @@ int main(void)
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = feature(FEATURE_3D) ? mcfg.neutral3d : 0;
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) { switch (mcfg.power_adc_channel) {
case 1: case 1:
@ -82,8 +86,11 @@ int main(void)
pwmInit(&pwm_params); pwmInit(&pwm_params);
// configure PWM/CPPM read function. spektrum or sbus below will override that // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
for (i = 0; i < RC_CHANS; i++)
rcData[i] = 1502;
rcReadRawFunc = pwmReadRawRC; rcReadRawFunc = pwmReadRawRC;
core.numRCChannels = MAX_INPUTS;
if (feature(FEATURE_SERIALRX)) { if (feature(FEATURE_SERIALRX)) {
switch (mcfg.serialrx_type) { switch (mcfg.serialrx_type) {
@ -96,10 +103,12 @@ int main(void)
sbusInit(&rcReadRawFunc); sbusInit(&rcReadRawFunc);
break; break;
} }
} else { } else { // spektrum and GPS are mutually exclusive
// spektrum and GPS are mutually exclusive
// Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
if (feature(FEATURE_GPS)) // gpsInit will return if FEATURE_GPS is not enabled.
// Sanity check below - protocols other than NMEA do not support baud rate autodetection
if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
mcfg.gps_baudrate = 0;
gpsInit(mcfg.gps_baudrate); gpsInit(mcfg.gps_baudrate);
} }
#ifdef SONAR #ifdef SONAR
@ -141,6 +150,9 @@ int main(void)
#endif #endif
} }
if (feature(FEATURE_TELEMETRY))
initTelemetry();
previousTime = micros(); previousTime = micros();
if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = CALIBRATING_ACC_CYCLES; calibratingA = CALIBRATING_ACC_CYCLES;

216
src/mixer.c
View File

@ -3,7 +3,8 @@
static uint8_t numberMotor = 0; static uint8_t numberMotor = 0;
int16_t motor[MAX_MOTORS]; int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; int16_t motor_disarmed[MAX_MOTORS];
int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
static motorMixer_t currentMixer[MAX_MOTORS]; static motorMixer_t currentMixer[MAX_MOTORS];
@ -106,6 +107,20 @@ static const motorMixer_t mixerVtail4[] = {
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
}; };
static const motorMixer_t mixerHex6H[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT
{ 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT
};
static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
};
// Keep this synced with MultiType struct in mw.h! // Keep this synced with MultiType struct in mw.h!
const mixer_t mixers[] = { const mixer_t mixers[] = {
// Mo Se Mixtable // Mo Se Mixtable
@ -127,9 +142,37 @@ const mixer_t mixers[] = {
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
{ 6, 0, mixerHex6H }, // MULTITYPE_HEX6H
{ 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO
{ 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
{ 0, 0, NULL }, // MULTITYPE_CUSTOM { 0, 0, NULL }, // MULTITYPE_CUSTOM
}; };
int16_t servoMiddle(int nr)
{
// Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than
// the number of RC channels, it means the center value is taken FROM that RC channel (by its index)
if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS)
return rcData[cfg.servoConf[nr].middle];
else
return cfg.servoConf[nr].middle;
}
int servoDirection(int nr, int lr)
{
// servo.rate is overloaded for servos that don't have a rate, but only need direction
// bit set = negative, clear = positive
// rate[2] = ???_direction
// rate[1] = roll_direction
// rate[0] = pitch_direction
// servo.rate is also used as gimbal gain multiplier (yeah)
if (cfg.servoConf[nr].rate & lr)
return -1;
else
return 1;
}
void mixerInit(void) void mixerInit(void)
{ {
int i; int i;
@ -168,6 +211,15 @@ void mixerInit(void)
} }
} }
} }
mixerResetMotors();
}
void mixerResetMotors(void)
{
int i;
// set disarmed motor values
for (i = 0; i < MAX_MOTORS; i++)
motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand;
} }
void mixerLoadMix(int index) void mixerLoadMix(int index)
@ -211,16 +263,29 @@ void writeServos(void)
} }
break; break;
case MULTITYPE_AIRPLANE: case MULTITYPE_FLYING_WING:
pwmWriteServo(0, servo[3]);
pwmWriteServo(1, servo[4]);
break; break;
case MULTITYPE_FLYING_WING:
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
pwmWriteServo(0, servo[0]); pwmWriteServo(0, servo[0]);
pwmWriteServo(1, servo[1]); pwmWriteServo(1, servo[1]);
break; break;
case MULTITYPE_DUALCOPTER:
pwmWriteServo(0, servo[4]);
pwmWriteServo(1, servo[5]);
break;
case MULTITYPE_AIRPLANE:
case MULTITYPE_SINGLECOPTER:
pwmWriteServo(0, servo[3]);
pwmWriteServo(1, servo[4]);
pwmWriteServo(2, servo[5]);
pwmWriteServo(3, servo[6]);
break;
default: default:
// Two servos for SERVO_TILT, if enabled // Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
@ -253,52 +318,55 @@ void writeAllMotors(int16_t mc)
static void airplaneMixer(void) static void airplaneMixer(void)
{ {
#if 0 int16_t flapperons[2] = { 0, 0 };
uint16_t servomid[8]; int i;
int16_t flaperons[2] = { 0, 0 };
for (i = 0; i < 8; i++) {
servomid[i] = 1500 + cfg.servotrim[i]; // servo center is 1500?
}
if (!f.ARMED) if (!f.ARMED)
motor[0] = cfg.mincommand; // Kill throttle when disarmed servo[7] = mcfg.mincommand; // Kill throttle when disarmed
else else
motor[0] = rcData[THROTTLE]; servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
motor[0] = servo[7];
#if 0
if (cfg.flaperons) { if (cfg.flaperons) {
} }
#endif
if (cfg.flaps) { if (mcfg.flaps_speed) {
int16_t flap = 1500 - constrain(rcData[cfg.flaps], cfg.servoendpoint_low[2], cfg.servoendpoint_high[2]); // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
static int16_t slowFlaps = flap; // use servo min, servo max and servo rate for proper endpoints adjust
static int16_t slow_LFlaps;
int16_t lFlap = servoMiddle(2);
if (cfg.flapspeed) { lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max);
if (slowFlaps < flap) { lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle?
slowFlaps += cfg.flapspeed; if (slow_LFlaps < lFlap)
} else if (slowFlaps > flap) { slow_LFlaps += mcfg.flaps_speed;
slowFlaps -= cfg.flapspeed; else if (slow_LFlaps > lFlap)
} slow_LFlaps -= mcfg.flaps_speed;
} else {
slowFlaps = flap; servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L;
} servo[2] += mcfg.midrc;
servo[2] = servomid[2] + (slowFlaps * cfg.servoreverse[2]);
} }
if (f.PASSTHRU_MODE) { // Direct passthru from RX if (f.PASSTHRU_MODE) { // Direct passthru from RX
servo[3] = servomid[3] + ((rcCommand[ROLL] + flapperons[0]) * cfg.servoreverse[3]); // Wing 1 servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1
servo[4] = servomid[4] + ((rcCommand[ROLL] + flapperons[1]) * cfg.servoreverse[4]); // Wing 2 servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2
servo[5] = servomid[5] + (rcCommand[YAW] * cfg.servoreverse[5]); // Rudder servo[5] = rcCommand[YAW]; // Rudder
servo[6] = servomid[6] + (rcCommand[PITCH] * cfg.servoreverse[6]); // Elevator servo[6] = rcCommand[PITCH]; // Elevator
} else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui } else {
servo[3] = (servomid[3] + ((axisPID[ROLL] + flapperons[0]) * cfg.servoreverse[3])); // Wing 1 // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
servo[4] = (servomid[4] + ((axisPID[ROLL] + flapperons[1]) * cfg.servoreverse[4])); // Wing 2 servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1
servo[5] = (servomid[5] + (axisPID[YAW] * cfg.servoreverse[5])); // Rudder servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2
servo[6] = (servomid[6] + (axisPID[PITCH] * cfg.servoreverse[6])); // Elevator servo[5] = axisPID[YAW]; // Rudder
servo[6] = axisPID[PITCH]; // Elevator
}
for (i = 3; i < 7; i++) {
servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates
servo[i] += servoMiddle(i);
} }
#endif
} }
void mixTable(void) void mixTable(void)
@ -319,17 +387,17 @@ void mixTable(void)
// airplane / servo mixes // airplane / servo mixes
switch (mcfg.mixerConfiguration) { switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI: case MULTITYPE_BI:
servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT
servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT
break; break;
case MULTITYPE_TRI: case MULTITYPE_TRI:
servo[5] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR
break; break;
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max); servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
break; break;
case MULTITYPE_AIRPLANE: case MULTITYPE_AIRPLANE:
@ -337,47 +405,62 @@ void mixTable(void)
break; break;
case MULTITYPE_FLYING_WING: case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE]; if (!f.ARMED)
servo[7] = mcfg.mincommand;
else
servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
motor[0] = servo[7];
if (f.PASSTHRU_MODE) { if (f.PASSTHRU_MODE) {
// do not use sensors for correction, simple 2 channel mixing // do not use sensors for correction, simple 2 channel mixing
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc); servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc); servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]);
} else { } else {
// use sensors to correct (gyro only or gyro + acc) // use sensors to correct (gyro only or gyro + acc)
servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL]; servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]);
servo[1] = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL]; servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]);
} }
servo[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max); servo[3] += servoMiddle(3);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max); servo[4] += servoMiddle(4);
break;
case MULTITYPE_DUALCOPTER:
for (i = 4; i < 6; i++ ) {
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
servo[i] += servoMiddle(i);
}
break;
case MULTITYPE_SINGLECOPTER:
for (i = 3; i < 7; i++) {
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
servo[i] += servoMiddle(i);
}
motor[0] = rcCommand[THROTTLE];
break; break;
} }
// do camstab // do camstab
if (feature(FEATURE_SERVO_TILT)) { if (feature(FEATURE_SERVO_TILT)) {
uint16_t aux[2] = { 0, 0 }; // center at fixed position, or vary either pitch or roll by RC channel
servo[0] = servoMiddle(0);
if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY)) servo[1] = servoMiddle(1);
aux[0] = rcData[AUX3] - mcfg.midrc;
if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
aux[1] = rcData[AUX4] - mcfg.midrc;
servo[0] = cfg.gimbal_pitch_mid + aux[0];
servo[1] = cfg.gimbal_roll_mid + aux[1];
if (rcOptions[BOXCAMSTAB]) { if (rcOptions[BOXCAMSTAB]) {
if (cfg.gimbal_flags & GIMBAL_MIXTILT) { if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16; servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
servo[1] += (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 + cfg.gimbal_roll_gain * angle[ROLL] / 16; servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
} else { } else {
servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16; servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16; servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50;
}
} }
} }
servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); // constrain servos
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max); for (i = 0; i < MAX_SERVOS; i++)
} servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values
// forward AUX1-4 to servo outputs (not constrained)
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = 0; int offset = 0;
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
@ -408,7 +491,8 @@ void mixTable(void)
motor[i] = mcfg.mincommand; motor[i] = mcfg.mincommand;
} }
} }
if (!f.ARMED) if (!f.ARMED) {
motor[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; motor[i] = motor_disarmed[i];
}
} }
} }

29
src/mw.c
View File

@ -16,10 +16,10 @@ int16_t telemTemperature1; // gyro sensor temperature
int16_t failsafeCnt = 0; int16_t failsafeCnt = 0;
int16_t failsafeEvents = 0; int16_t failsafeEvents = 0;
int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000] int16_t rcData[RC_CHANS]; // interval [1000;2000]
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
uint16_t rssi; // range: [0;1023] uint16_t rssi; // range: [0;1023]
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
@ -45,8 +45,6 @@ uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees * 10 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[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother 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 int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
@ -179,9 +177,9 @@ void annexCode(void)
LED0_OFF; LED0_OFF;
if (f.ARMED) if (f.ARMED)
LED0_ON; LED0_ON;
// This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState()
if (feature(FEATURE_TELEMETRY)) if (feature(FEATURE_TELEMETRY))
initTelemetry(f.ARMED); updateTelemetryState();
} }
#ifdef LEDRING #ifdef LEDRING
@ -758,8 +756,8 @@ void loop(void)
} }
} else { // not in rc loop } else { // not in rc loop
static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
if (taskOrder > 3) if (taskOrder > 4)
taskOrder -= 4; taskOrder -= 5;
switch (taskOrder) { switch (taskOrder) {
case 0: case 0:
taskOrder++; taskOrder++;
@ -780,6 +778,15 @@ void loop(void)
break; break;
#endif #endif
case 3: case 3:
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
// change this based on available hardware
taskOrder++;
if (feature(FEATURE_GPS)) {
gpsThread();
break;
}
case 4:
taskOrder++; taskOrder++;
#ifdef SONAR #ifdef SONAR
if (sensors(SENSOR_SONAR)) { if (sensors(SENSOR_SONAR)) {
@ -802,9 +809,6 @@ void loop(void)
currentTime = micros(); currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime); cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime; previousTime = currentTime;
#ifdef MPU6050_DMP
mpu6050DmpLoop();
#endif
#ifdef MAG #ifdef MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
@ -814,6 +818,7 @@ void loop(void)
dif += 360; dif += 360;
if (dif >= +180) if (dif >= +180)
dif -= 360; dif -= 360;
dif *= -mcfg.yaw_control_direction;
if (f.SMALL_ANGLES_25) if (f.SMALL_ANGLES_25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else } else

81
src/mw.h
View File

@ -4,11 +4,13 @@
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define BARO_TAB_SIZE_MAX 48 #define BARO_TAB_SIZE_MAX 48
#define VERSION 220 #define VERSION 230
#define LAT 0 #define LAT 0
#define LON 1 #define LON 1
#define RC_CHANS (18)
// Serial GPS only variables // Serial GPS only variables
// navigation mode // navigation mode
typedef enum NavigationMode typedef enum NavigationMode
@ -38,16 +40,18 @@ typedef enum MultiType
MULTITYPE_HELI_120_CCPM = 15, MULTITYPE_HELI_120_CCPM = 15,
MULTITYPE_HELI_90_DEG = 16, MULTITYPE_HELI_90_DEG = 16,
MULTITYPE_VTAIL4 = 17, MULTITYPE_VTAIL4 = 17,
MULTITYPE_CUSTOM = 18, // no current GUI displays this MULTITYPE_HEX6H = 18,
MULTITYPE_LAST = 19 MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay
MULTITYPE_DUALCOPTER = 20,
MULTITYPE_SINGLECOPTER = 21,
MULTITYPE_CUSTOM = 22, // no current GUI displays this
MULTITYPE_LAST = 23
} MultiType; } MultiType;
typedef enum GimbalFlags { typedef enum GimbalFlags {
GIMBAL_NORMAL = 1 << 0, GIMBAL_NORMAL = 1 << 0,
GIMBAL_TILTONLY = 1 << 1, GIMBAL_MIXTILT = 1 << 1,
GIMBAL_DISABLEAUX34 = 1 << 2, GIMBAL_FORWARDAUX = 1 << 2,
GIMBAL_FORWARDAUX = 1 << 3,
GIMBAL_MIXTILT = 1 << 4,
} GimbalFlags; } GimbalFlags;
/*********** RC alias *****************/ /*********** RC alias *****************/
@ -194,33 +198,9 @@ typedef struct config_t {
// mixer-related configuration // mixer-related configuration
int8_t yaw_direction; int8_t yaw_direction;
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
uint16_t tri_yaw_min; // tail servo min
uint16_t tri_yaw_max; // tail servo max
// flying wing related configuration
uint16_t wing_left_min; // min/mid/max servo travel
uint16_t wing_left_mid;
uint16_t wing_left_max;
uint16_t wing_right_min;
uint16_t wing_right_mid;
uint16_t wing_right_max;
int8_t pitch_direction_l; // left servo - pitch orientation
int8_t pitch_direction_r; // right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored)
int8_t roll_direction_l; // left servo - roll orientation
int8_t roll_direction_r; // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
// gimbal-related configuration // gimbal-related configuration
int8_t gimbal_pitch_gain; // gimbal pitch servo gain (tied to angle) can be negative to invert movement
int8_t gimbal_roll_gain; // gimbal roll servo gain (tied to angle) can be negative to invert movement
uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff
uint16_t gimbal_pitch_min; // gimbal pitch servo min travel
uint16_t gimbal_pitch_max; // gimbal pitch servo max travel
uint16_t gimbal_pitch_mid; // gimbal pitch servo neutral value
uint16_t gimbal_roll_min; // gimbal roll servo min travel
uint16_t gimbal_roll_max; // gimbal roll servo max travel
uint16_t gimbal_roll_mid; // gimbal roll servo neutral value
// gps-related stuff // gps-related stuff
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
@ -258,6 +238,9 @@ typedef struct master_t {
sensor_align_e gyro_align; // gyro alignment sensor_align_e gyro_align; // gyro alignment
sensor_align_e acc_align; // acc alignment sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment sensor_align_e mag_align; // mag alignment
int16_t board_align_roll; // board alignment correction in roll (deg)
int16_t board_align_pitch; // board alignment correction in pitch (deg)
int16_t board_align_yaw; // board alignment correction in yaw (deg)
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device 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_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
@ -265,7 +248,6 @@ typedef struct master_t {
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 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 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 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.
int16_t accZero[3]; int16_t accZero[3];
int16_t magZero[3]; int16_t magZero[3];
@ -282,18 +264,21 @@ typedef struct master_t {
uint16_t mincheck; // minimum rc end uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end uint16_t maxcheck; // maximum rc end
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
uint8_t flaps_speed; // airplane mode flaps, 0 = no flaps, > 0 = flap speed, larger = faster
uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable. uint8_t rssi_aux_channel; // Read rssi from channel. 1+ = AUX1+, 0 to disable.
// gps-related stuff // gps-related stuff
uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ?? uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2: MTK NMEA 3: MTK Binary
uint32_t gps_baudrate; // GPS baudrate int8_t gps_baudrate; // GPS baudrate, -1: autodetect (NMEA only), 0: 115200, 1: 57600, 2: 38400, 3: 19200, 4: 9600
uint32_t serial_baudrate; uint32_t serial_baudrate;
uint32_t softserial_baudrate; uint32_t softserial_baudrate;
uint8_t softserial_inverted; // use inverted softserial input and output signals uint8_t softserial_inverted; // use inverted softserial input and output signals
uint8_t telemetry_softserial; // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
config_t profile[3]; // 3 separate profiles config_t profile[3]; // 3 separate profiles
uint8_t current_profile; // currently loaded profile uint8_t current_profile; // currently loaded profile
@ -307,8 +292,9 @@ typedef struct core_t {
serialPort_t *gpsport; serialPort_t *gpsport;
serialPort_t *telemport; serialPort_t *telemport;
serialPort_t *rcvrport; serialPort_t *rcvrport;
bool useServo; 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.
uint8_t numRCChannels; // number of rc channels as reported by current input driver
bool useServo; // feature SERVO_TILT or wing/airplane mixers will enable this
} core_t; } core_t;
typedef struct flags_t { typedef struct flags_t {
@ -365,15 +351,18 @@ extern int16_t throttleAngleCorrection;
extern int16_t headFreeModeHold; extern int16_t headFreeModeHold;
extern int16_t heading, magHold; extern int16_t heading, magHold;
extern int16_t motor[MAX_MOTORS]; extern int16_t motor[MAX_MOTORS];
extern int16_t servo[8]; extern int16_t servo[MAX_SERVOS];
extern int16_t rcData[8]; extern int16_t rcData[RC_CHANS];
extern uint16_t rssi; // range: [0;1023] extern uint16_t rssi; // range: [0;1023]
extern uint8_t vbat; extern uint8_t vbat;
extern int16_t telemTemperature1; // gyro sensor temperature extern int16_t telemTemperature1; // gyro sensor temperature
extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE
extern uint8_t toggleBeep; extern uint8_t toggleBeep;
#define PITCH_LOOKUP_LENGTH 7
#define THROTTLE_LOOKUP_LENGTH 12
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
// GPS stuff // GPS stuff
extern int32_t GPS_coord[2]; extern int32_t GPS_coord[2];
extern int32_t GPS_home[2]; extern int32_t GPS_home[2];
@ -385,8 +374,6 @@ extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
extern uint16_t GPS_ground_course; // degrees*10 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 int16_t nav[2];
extern int8_t nav_mode; // Navigation mode 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 int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
@ -429,6 +416,7 @@ void Sonar_update(void);
// Output // Output
void mixerInit(void); void mixerInit(void);
void mixerResetMotors(void);
void mixerLoadMix(int index); void mixerLoadMix(int index);
void writeServos(void); void writeServos(void);
void writeMotors(void); void writeMotors(void);
@ -470,13 +458,16 @@ void systemBeep(bool onoff);
void cliProcess(void); void cliProcess(void);
// gps // gps
void gpsInit(uint32_t baudrate); void gpsInit(uint8_t baudrate);
void gpsThread(void);
void gpsSetPIDs(void);
int8_t gpsSetPassthrough(void);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_reset_nav(void); void GPS_reset_nav(void);
void GPS_set_pids(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon); void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);
// telemetry // telemetry
void initTelemetry(bool State); void initTelemetry(void);
void updateTelemetryState(void);
void sendTelemetry(void); void sendTelemetry(void);

1
src/sbus.c
View File

@ -26,6 +26,7 @@ void sbusInit(rcReadRawDataPtr *callback)
core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, MODE_RX); core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, MODE_RX);
if (callback) if (callback)
*callback = sbusReadRawRC; *callback = sbusReadRawRC;
core.numRCChannels = SBUS_MAX_CHANNEL;
} }
struct sbus_dat struct sbus_dat

4
src/sensors.c
View File

@ -36,7 +36,7 @@ void sensorsAutodetect(void)
bool haveMpu6k = false; bool haveMpu6k = false;
// Autodetect gyro hardware. We have MPU3050 or MPU6050. // Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale)) { if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
// this filled up acc.* struct with init values // this filled up acc.* struct with init values
haveMpu6k = true; haveMpu6k = true;
} else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
@ -64,7 +64,7 @@ retry:
; // fallthrough ; // fallthrough
case ACC_MPU6050: // MPU6050 case ACC_MPU6050: // MPU6050
if (haveMpu6k) { if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050) if (mcfg.acc_hardware == ACC_MPU6050)
break; break;

211
src/serial.c
View File

@ -3,7 +3,9 @@
// Multiwii Serial Protocol 0 // Multiwii Serial Protocol 0
#define MSP_VERSION 0 #define MSP_VERSION 0
#define PLATFORM_32BIT ((uint32_t)1 << 31) #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
#define CAP_DYNBALANCE ((uint32_t)1 << 2)
#define CAP_FLAPS ((uint32_t)1 << 3)
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable #define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
@ -89,51 +91,8 @@ struct box_t {
static uint8_t availableBoxes[CHECKBOXITEMS]; static uint8_t availableBoxes[CHECKBOXITEMS];
// this is the number of filled indexes in above array // this is the number of filled indexes in above array
static uint8_t numberBoxItems = 0; static uint8_t numberBoxItems = 0;
// from mixer.c
static const char boxnames[] = extern int16_t motor_disarmed[MAX_MOTORS];
"ARM;"
"ANGLE;"
"HORIZON;"
"BARO;"
"VARIO;"
"MAG;"
"HEADFREE;"
"HEADADJ;"
"CAMSTAB;"
"CAMTRIG;"
"GPS HOME;"
"GPS HOLD;"
"PASSTHRU;"
"BEEPER;"
"LEDMAX;"
"LEDLOW;"
"LLIGHTS;"
"CALIB;"
"GOVERNOR;"
"OSD SW;";
const uint8_t boxids[] = { // permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function.
0, // "ARM;"
1, // "ANGLE;"
2, // "HORIZON;"
3, // "BARO;"
4, // "VARIO;"
5, // "MAG;"
6, // "HEADFREE;"
7, // "HEADADJ;"
8, // "CAMSTAB;"
9, // "CAMTRIG;"
10, // "GPS HOME;"
11, // "GPS HOLD;"
12, // "PASSTHRU;"
13, // "BEEPER;"
14, // "LEDMAX;"
15, // "LEDLOW;"
16, // "LLIGHTS;"
17, // "CALIB;"
18, // "GOVERNOR;"
19, // "OSD_SWITCH;"
};
static const char pidnames[] = static const char pidnames[] =
"ROLL;" "ROLL;"
@ -149,7 +108,6 @@ static const char pidnames[] =
static uint8_t checksum, indRX, inBuf[INBUF_SIZE]; static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
static uint8_t cmdMSP; static uint8_t cmdMSP;
static bool guiConnected = false;
// signal that we're in cli mode // signal that we're in cli mode
uint8_t cliMode = 0; uint8_t cliMode = 0;
@ -247,31 +205,35 @@ void serializeNames(const char *s)
void serializeBoxNamesReply(void) void serializeBoxNamesReply(void)
{ {
char buf[256]; // no fucking idea int i, idx, j, flag = 1, count = 0, len;
char *c;
int i, j;
memset(buf, 0, sizeof(buf)); reset:
for (i = 0; i < CHECKBOXITEMS; i++) { // in first run of the loop, we grab total size of junk to be sent
for (j = 0; j < numberBoxItems; j++) { // then come back and actually send it
if (boxes[i].boxIndex == availableBoxes[j]) for (i = 0; i < numberBoxItems; i++) {
strcat(buf, boxes[i].boxName); idx = availableBoxes[i];
len = strlen(boxes[idx].boxName);
if (flag) {
count += len;
} else {
for (j = 0; j < len; j++)
serialize8(boxes[idx].boxName[j]);
} }
} }
headSerialReply(strlen(buf)); if (flag) {
for (c = buf; *c; c++) headSerialReply(count);
serialize8(*c); flag = 0;
goto reset;
}
} }
void serialInit(uint32_t baudrate) void serialInit(uint32_t baudrate)
{ {
int idx; int idx;
bool hfadded = false;
core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX); core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
// TODO fix/hax
core.telemport = core.mainport;
// calculate used boxes based on features and fill availableBoxes[] array // calculate used boxes based on features and fill availableBoxes[] array
memset(availableBoxes, 0xFF, sizeof(availableBoxes)); memset(availableBoxes, 0xFF, sizeof(availableBoxes));
@ -280,27 +242,20 @@ void serialInit(uint32_t baudrate)
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
availableBoxes[idx++] = BOXANGLE; availableBoxes[idx++] = BOXANGLE;
availableBoxes[idx++] = BOXHORIZON; availableBoxes[idx++] = BOXHORIZON;
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
hfadded = true;
} }
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
availableBoxes[idx++] = BOXBARO; availableBoxes[idx++] = BOXBARO;
if (feature(FEATURE_VARIO)) if (feature(FEATURE_VARIO))
availableBoxes[idx++] = BOXVARIO; availableBoxes[idx++] = BOXVARIO;
} }
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
// this really shouldn't even needed to be tested as it wouldn't be possible without acc anyway
if (!hfadded) {
availableBoxes[idx++] = BOXMAG; availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE; availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ; availableBoxes[idx++] = BOXHEADADJ;
} }
}
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB; availableBoxes[idx++] = BOXCAMSTAB;
if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) { if (feature(FEATURE_GPS)) {
availableBoxes[idx++] = BOXGPSHOME; availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD; availableBoxes[idx++] = BOXGPSHOLD;
} }
@ -309,12 +264,13 @@ void serialInit(uint32_t baudrate)
availableBoxes[idx++] = BOXBEEPERON; availableBoxes[idx++] = BOXBEEPERON;
if (feature(FEATURE_INFLIGHT_ACC_CAL)) if (feature(FEATURE_INFLIGHT_ACC_CAL))
availableBoxes[idx++] = BOXCALIB; availableBoxes[idx++] = BOXCALIB;
availableBoxes[idx++] = BOXOSD;
numberBoxItems = idx; numberBoxItems = idx;
} }
static void evaluateCommand(void) static void evaluateCommand(void)
{ {
uint32_t i, tmp; uint32_t i, tmp, junk;
uint8_t wp_no; uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0; int32_t lat = 0, lon = 0, alt = 0;
@ -363,8 +319,23 @@ static void evaluateCommand(void)
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_MISC: case MSP_SET_MISC:
read16(); // powerfailmeter
mcfg.minthrottle = read16();
read32(); // mcfg.maxthrottle, mcfg.mincommand
cfg.failsafe_throttle = read16();
read16();
read32();
cfg.mag_declination = read16() * 10;
mcfg.vbatscale = read8(); // actual vbatscale as intended
mcfg.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI
mcfg.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI
read8(); // vbatlevel_crit (unused)
headSerialReply(0); headSerialReply(0);
break; break;
case MSP_SET_MOTOR:
for (i = 0; i < 8; i++)
motor_disarmed[i] = read16();
break;
case MSP_SELECT_SETTING: case MSP_SELECT_SETTING:
if (!f.ARMED) { if (!f.ARMED) {
mcfg.current_profile = read8(); mcfg.current_profile = read8();
@ -384,17 +355,18 @@ static void evaluateCommand(void)
serialize8(VERSION); // multiwii version serialize8(VERSION); // multiwii version
serialize8(mcfg.mixerConfiguration); // type of multicopter serialize8(mcfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability" serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0)); // "capability"
break; break;
case MSP_STATUS: case MSP_STATUS:
headSerialReply(11); headSerialReply(11);
serialize16(cycleTime); serialize16(cycleTime);
serialize16(i2cGetErrorCounter()); serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); 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 // 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. // 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 | // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
junk = 0;
tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
@ -406,50 +378,13 @@ static void evaluateCommand(void)
rcOptions[BOXCALIB] << BOXCALIB | rcOptions[BOXCALIB] << BOXCALIB |
rcOptions[BOXGOV] << BOXGOV | rcOptions[BOXGOV] << BOXGOV |
rcOptions[BOXOSD] << BOXOSD | rcOptions[BOXOSD] << BOXOSD |
f.ARMED << BOXARM); f.ARMED << BOXARM;
#else
// Serialize the boxes in the order we delivered them
tmp = 0;
for (i = 0; i < numberBoxItems; i++) { for (i = 0; i < numberBoxItems; i++) {
uint8_t val, box = availableBoxes[i]; int flag = (tmp & (1 << availableBoxes[i]));
switch (box) { if (flag)
// Handle the special cases junk |= 1 << i;
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(junk);
}
serialize32(tmp);
#endif
serialize8(mcfg.current_profile); serialize8(mcfg.current_profile);
break; break;
case MSP_RAW_IMU: case MSP_RAW_IMU:
@ -468,17 +403,28 @@ static void evaluateCommand(void)
serialize16(magADC[i]); serialize16(magADC[i]);
break; break;
case MSP_SERVO: case MSP_SERVO:
headSerialReply(16); s_struct((uint8_t *)&servo, 16);
for (i = 0; i < 8; i++)
serialize16(servo[i]);
break; break;
case MSP_SERVO_CONF: case MSP_SERVO_CONF:
s_struct((uint8_t *)&cfg.servoConf, 56); // struct servoConf is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56 headSerialReply(56);
for (i = 0; i < MAX_SERVOS; i++) {
serialize16(cfg.servoConf[i].min);
serialize16(cfg.servoConf[i].max);
serialize16(cfg.servoConf[i].middle);
serialize8(cfg.servoConf[i].rate);
}
break;
case MSP_SET_SERVO_CONF:
headSerialReply(0);
for (i = 0; i < MAX_SERVOS; i++) {
cfg.servoConf[i].min = read16();
cfg.servoConf[i].max = read16();
cfg.servoConf[i].middle = read16();
cfg.servoConf[i].rate = read8();
}
break; break;
case MSP_MOTOR: case MSP_MOTOR:
headSerialReply(16); s_struct((uint8_t *)motor, 16);
for (i = 0; i < 8; i++)
serialize16(motor[i]);
break; break;
case MSP_RC: case MSP_RC:
headSerialReply(16); headSerialReply(16);
@ -556,8 +502,19 @@ static void evaluateCommand(void)
serialize8(availableBoxes[i]); serialize8(availableBoxes[i]);
break; break;
case MSP_MISC: case MSP_MISC:
headSerialReply(2); headSerialReply(2 * 6 + 4 + 2 + 4);
serialize16(0); // intPowerTrigger1 serialize16(0); // intPowerTrigger1 (aka useless trash)
serialize16(mcfg.minthrottle);
serialize16(mcfg.maxthrottle);
serialize16(mcfg.mincommand);
serialize16(cfg.failsafe_throttle);
serialize16(0); // plog useless shit
serialize32(0); // plog useless shit
serialize16(cfg.mag_declination / 10); // TODO check this shit
serialize8(mcfg.vbatscale);
serialize8(mcfg.vbatmincellvoltage);
serialize8(mcfg.vbatmaxcellvoltage);
serialize8(0);
break; break;
case MSP_MOTOR_PINS: case MSP_MOTOR_PINS:
headSerialReply(8); headSerialReply(8);
@ -718,7 +675,6 @@ void serialCom(void)
indRX = 0; indRX = 0;
checksum ^= c; checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow c_state = HEADER_SIZE; // the command is to follow
guiConnected = true;
} else if (c_state == HEADER_SIZE) { } else if (c_state == HEADER_SIZE) {
cmdMSP = c; cmdMSP = c;
checksum ^= c; checksum ^= c;
@ -733,8 +689,7 @@ void serialCom(void)
c_state = IDLE; c_state = IDLE;
} }
} }
if (!cliMode && !serialTotalBytesWaiting(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream if (!cliMode && feature(FEATURE_TELEMETRY)) { // The first condition should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry(); sendTelemetry();
return;
} }
} }

1
src/spektrum.c
View File

@ -37,6 +37,7 @@ void spektrumInit(rcReadRawDataPtr *callback)
core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX); core.rcvrport = uartOpen(USART2, spektrumDataReceive, 115200, MODE_RX);
if (callback) if (callback)
*callback = spektrumReadRawRC; *callback = spektrumReadRawRC;
core.numRCChannels = SPEK_MAX_CHANNEL;
} }
// Receive ISR callback // Receive ISR callback

39
src/telemetry.c
View File

@ -45,6 +45,8 @@
#define ID_GYRO_Y 0x41 #define ID_GYRO_Y 0x41
#define ID_GYRO_Z 0x42 #define ID_GYRO_Z 0x42
#define ID_VERT_SPEED 0x30 //opentx vario
// from sensors.c // from sensors.c
extern uint8_t batteryCellCount; extern uint8_t batteryCellCount;
@ -96,7 +98,7 @@ static void sendBaro(void)
sendDataHead(ID_ALTITUDE_BP); sendDataHead(ID_ALTITUDE_BP);
serialize16(BaroAlt / 100); serialize16(BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP); sendDataHead(ID_ALTITUDE_AP);
serialize16(BaroAlt % 100); serialize16(abs(BaroAlt % 100));
} }
static void sendTemperature1(void) static void sendTemperature1(void)
@ -135,6 +137,16 @@ static void sendGPS(void)
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E'); serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
} }
/*
* Send vertical speed for opentx. ID_VERT_SPEED
* Unit is cm/s
*/
static void sendVario(void)
{
sendDataHead(ID_VERT_SPEED);
serialize16(vario);
}
/* /*
* Send voltage via ID_VOLT * Send voltage via ID_VOLT
* *
@ -202,13 +214,29 @@ static void sendHeading(void)
static bool telemetryEnabled = false; static bool telemetryEnabled = false;
void initTelemetry(bool State) void initTelemetry(void)
{ {
// Sanity check for softserial vs. telemetry port
if (!feature(FEATURE_SOFTSERIAL))
mcfg.telemetry_softserial = TELEMETRY_UART;
if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL)
core.telemport = &(softSerialPorts[0].port);
else
core.telemport = core.mainport;
}
void updateTelemetryState(void)
{
bool State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
if (State != telemetryEnabled) { if (State != telemetryEnabled) {
if (mcfg.telemetry_softserial == TELEMETRY_UART) {
if (State) if (State)
serialInit(9600); serialInit(9600);
else else
serialInit(mcfg.serial_baudrate); serialInit(mcfg.serial_baudrate);
}
telemetryEnabled = State; telemetryEnabled = State;
} }
} }
@ -218,12 +246,19 @@ static uint8_t cycleNum = 0;
void sendTelemetry(void) void sendTelemetry(void)
{ {
if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED)
return;
if (serialTotalBytesWaiting(core.telemport) != 0)
return;
if (millis() - lastCycleTime >= CYCLETIME) { if (millis() - lastCycleTime >= CYCLETIME) {
lastCycleTime = millis(); lastCycleTime = millis();
cycleNum++; cycleNum++;
// Sent every 125ms // Sent every 125ms
sendAccel(); sendAccel();
sendVario();
sendTelemetryTail(); sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms if ((cycleNum % 4) == 0) { // Sent every 500ms

109
src/utils.c
View File

@ -1,6 +1,9 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
static bool standardBoardAlignment = true; // board orientation correction
static float boardRotation[3][3]; // matrix
int constrain(int amt, int low, int high) int constrain(int amt, int low, int high)
{ {
if (amt < low) if (amt < low)
@ -10,3 +13,109 @@ int constrain(int amt, int low, int high)
else else
return amt; return amt;
} }
void initBoardAlignment(void)
{
float roll, pitch, yaw;
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
// standard alignment, nothing to calculate
if (!mcfg.board_align_roll && !mcfg.board_align_pitch && !mcfg.board_align_yaw)
return;
standardBoardAlignment = false;
// deg2rad
roll = mcfg.board_align_roll * M_PI / 180.0f;
pitch = mcfg.board_align_pitch * M_PI / 180.0f;
yaw = mcfg.board_align_yaw * M_PI / 180.0f;
cosx = cosf(roll);
sinx = sinf(roll);
cosy = cosf(pitch);
siny = sinf(pitch);
cosz = cosf(yaw);
sinz = sinf(yaw);
coszcosx = cosz * cosx;
coszcosy = cosz * cosy;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
// define rotation matrix
boardRotation[0][0] = coszcosy;
boardRotation[0][1] = -cosy * sinz;
boardRotation[0][2] = siny;
boardRotation[1][0] = sinzcosx + (coszsinx * siny);
boardRotation[1][1] = coszcosx - (sinzsinx * siny);
boardRotation[1][2] = -sinx * cosy;
boardRotation[2][0] = (sinzsinx) - (coszcosx * siny);
boardRotation[2][1] = (coszsinx) + (sinzcosx * siny);
boardRotation[2][2] = cosy * cosx;
}
void alignBoard(int16_t *vec)
{
int16_t x = vec[X];
int16_t y = vec[Y];
int16_t z = vec[Z];
vec[X] = lrintf(boardRotation[0][0] * x + boardRotation[1][0] * y + boardRotation[2][0] * z);
vec[Y] = lrintf(boardRotation[0][1] * x + boardRotation[1][1] * y + boardRotation[2][1] * z);
vec[Z] = lrintf(boardRotation[0][2] * x + boardRotation[1][2] * y + boardRotation[2][2] * z);
}
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
{
switch (rotation) {
case CW0_DEG:
dest[X] = src[X];
dest[Y] = src[Y];
dest[Z] = src[Z];
break;
case CW90_DEG:
dest[X] = src[Y];
dest[Y] = -src[X];
dest[Z] = src[Z];
break;
case CW180_DEG:
dest[X] = -src[X];
dest[Y] = -src[Y];
dest[Z] = src[Z];
break;
case CW270_DEG:
dest[X] = -src[Y];
dest[Y] = src[X];
dest[Z] = src[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -src[X];
dest[Y] = src[Y];
dest[Z] = -src[Z];
break;
case CW90_DEG_FLIP:
dest[X] = src[Y];
dest[Y] = src[X];
dest[Z] = -src[Z];
break;
case CW180_DEG_FLIP:
dest[X] = src[X];
dest[Y] = -src[Y];
dest[Z] = -src[Z];
break;
case CW270_DEG_FLIP:
dest[X] = -src[Y];
dest[Y] = -src[X];
dest[Z] = -src[Z];
break;
default:
break;
}
if (!standardBoardAlignment)
alignBoard(dest);
}

3
src/utils.h
View File

@ -1,3 +1,6 @@
#pragma once #pragma once
int constrain(int amt, int low, int high); int constrain(int amt, int low, int high);
// sensor orientation
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
void initBoardAlignment(void);

2
support/stmloader/stmbootloader.c
View File

@ -322,7 +322,7 @@ void stmLoader(serialStruct_t *s, FILE *fp, unsigned char overrideParity, unsign
printf("Flashing device...\n"); printf("Flashing device...\n");
jumpAddress = stmHexLoader(s, fp); jumpAddress = stmHexLoader(s, fp);
if (jumpAddress) { if (jumpAddress) {
printf("\nFlash complete, cycle power\n"); printf("\nFlash complete, executing.\n");
go: go:
// send GO command // send GO command