mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
committed (untested) GPS support by sbaron;
fix for channel map cli stuff by simonk. reindented some code, so changes are large. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@127 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
0534444b2d
commit
007e033364
10 changed files with 3046 additions and 2475 deletions
|
|
@ -73,7 +73,7 @@
|
|||
<OPTFL>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<IsCurrentTarget>1</IsCurrentTarget>
|
||||
<IsCurrentTarget>0</IsCurrentTarget>
|
||||
</OPTFL>
|
||||
<CpuCode>255</CpuCode>
|
||||
<Books>
|
||||
|
|
@ -279,7 +279,7 @@
|
|||
<OPTFL>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<IsCurrentTarget>0</IsCurrentTarget>
|
||||
<IsCurrentTarget>1</IsCurrentTarget>
|
||||
</OPTFL>
|
||||
<CpuCode>255</CpuCode>
|
||||
<Books>
|
||||
|
|
@ -318,7 +318,7 @@
|
|||
<tRbox>1</tRbox>
|
||||
<sRunDeb>0</sRunDeb>
|
||||
<sLrtime>0</sLrtime>
|
||||
<nTsel>7</nTsel>
|
||||
<nTsel>1</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
|
|
@ -329,7 +329,7 @@
|
|||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>Segger\JL2CM3.dll</pMon>
|
||||
<pMon>BIN\UL2CM3.DLL</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
|
|
@ -355,7 +355,7 @@
|
|||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>UL2CM3</Key>
|
||||
<Name>-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</Name>
|
||||
<Name>-UV0168AVR -O206 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO11 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000</Name>
|
||||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint>
|
||||
|
|
@ -558,10 +558,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>12</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>39</TopLine>
|
||||
<CurrentLine>46</CurrentLine>
|
||||
<CurrentLine>50</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
||||
|
|
@ -600,10 +600,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>14</ColumnNumber>
|
||||
<ColumnNumber>18</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>18</CurrentLine>
|
||||
<CurrentLine>7</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\board.h</PathWithFileName>
|
||||
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
||||
|
|
@ -637,8 +637,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>22</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_adc.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
|
||||
|
|
@ -693,8 +693,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>137</TopLine>
|
||||
<CurrentLine>143</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
||||
|
|
@ -707,8 +707,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>27</TopLine>
|
||||
<CurrentLine>45</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu3050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
|
||||
|
|
@ -735,8 +735,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>42</TopLine>
|
||||
<CurrentLine>43</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_system.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
|
||||
|
|
@ -966,8 +966,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>116</TopLine>
|
||||
<CurrentLine>133</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\baseflight_startups\startup_stm32f10x_md.s</PathWithFileName>
|
||||
<FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>
|
||||
|
|
|
|||
|
|
@ -754,7 +754,7 @@
|
|||
<RestoreToolbox>1</RestoreToolbox>
|
||||
</Target>
|
||||
<RunDebugAfterBuild>0</RunDebugAfterBuild>
|
||||
<TargetSelection>7</TargetSelection>
|
||||
<TargetSelection>1</TargetSelection>
|
||||
<SimDlls>
|
||||
<CpuDll></CpuDll>
|
||||
<CpuDllArguments></CpuDllArguments>
|
||||
|
|
@ -768,7 +768,7 @@
|
|||
<PeripheralDll></PeripheralDll>
|
||||
<PeripheralDllArguments></PeripheralDllArguments>
|
||||
<InitializationFile></InitializationFile>
|
||||
<Driver>Segger\JL2CM3.dll</Driver>
|
||||
<Driver>BIN\UL2CM3.DLL</Driver>
|
||||
</TargetDlls>
|
||||
</DebugOption>
|
||||
<Utilities>
|
||||
|
|
@ -778,9 +778,9 @@
|
|||
<RunIndependent>0</RunIndependent>
|
||||
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
|
||||
<Capability>1</Capability>
|
||||
<DriverSelection>4099</DriverSelection>
|
||||
<DriverSelection>4096</DriverSelection>
|
||||
</Flash1>
|
||||
<Flash2>Segger\JL2CM3.dll</Flash2>
|
||||
<Flash2>BIN\UL2CM3.DLL</Flash2>
|
||||
<Flash3>"" ()</Flash3>
|
||||
<Flash4></Flash4>
|
||||
</Utilities>
|
||||
|
|
|
|||
4928
obj/baseflight.hex
4928
obj/baseflight.hex
File diff suppressed because it is too large
Load diff
|
|
@ -29,6 +29,7 @@ typedef enum {
|
|||
FEATURE_CAMTRIG = 1 << 6,
|
||||
FEATURE_GYRO_SMOOTHING = 1 << 7,
|
||||
FEATURE_LED_RING = 1 << 8,
|
||||
FEATURE_GPS = 1 << 9,
|
||||
} AvailableFeatures;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void);
|
||||
|
|
|
|||
44
src/cli.c
44
src/cli.c
|
|
@ -35,7 +35,7 @@ const char *mixerNames[] = {
|
|||
// sync this with AvailableFeatures enum from board.h
|
||||
const char *featureNames[] = {
|
||||
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "DIGITAL_SERVO", "MOTOR_STOP",
|
||||
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING",
|
||||
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
|
||||
NULL
|
||||
};
|
||||
|
||||
|
|
@ -51,7 +51,7 @@ const clicmd_t cmdTable[] = {
|
|||
{ "exit", "", cliExit },
|
||||
{ "feature", "list or -val or val", cliFeature },
|
||||
{ "help", "", cliHelp },
|
||||
{ "map", "mapping of first 4 channels", cliMap },
|
||||
{ "map", "mapping of rc channel order", cliMap },
|
||||
{ "mixer", "mixer name or list", cliMixer },
|
||||
{ "save", "save and reboot", cliSave },
|
||||
{ "set", "name=value or blank for list", cliSet },
|
||||
|
|
@ -113,6 +113,8 @@ const clivalue_t valueTable[] = {
|
|||
static void cliSetVar(const clivalue_t *var, const int32_t value);
|
||||
static void cliPrintVar(const clivalue_t *var);
|
||||
|
||||
#ifndef HAVE_ITOA_FUNCTION
|
||||
|
||||
/*
|
||||
** The following two functions together make up an itoa()
|
||||
** implementation. Function i2a() is a 'private' function
|
||||
|
|
@ -147,6 +149,8 @@ char *itoa(int i, char *a, int r)
|
|||
return a;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
static void cliPrompt(void)
|
||||
{
|
||||
uartPrint("\r\n# ");
|
||||
|
|
@ -255,35 +259,27 @@ static void cliMap(char *cmdline)
|
|||
uint8_t len;
|
||||
uint8_t i;
|
||||
char out[9];
|
||||
|
||||
|
||||
len = strlen(cmdline);
|
||||
|
||||
if (len == 0 || len != 8) {
|
||||
uartPrint("Current assignment: ");
|
||||
for (i = 0; i < 8; i++)
|
||||
out[cfg.rcmap[i]] = rcChannelLetters[i];
|
||||
out[i] = '\0';
|
||||
uartPrint(out);
|
||||
uartPrint("\r\n");
|
||||
return;
|
||||
} else {
|
||||
bool fail = false;
|
||||
if (len == 8) {
|
||||
// uppercase it
|
||||
for (i = 0; i < 8; i++) {
|
||||
for (i = 0; i < 8; i++)
|
||||
cmdline[i] = toupper(cmdline[i]);
|
||||
if (!strchr(rcChannelLetters, cmdline[i])) {
|
||||
fail = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (fail)
|
||||
for (i = 0; i < 8; i++) {
|
||||
if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i]))
|
||||
continue;
|
||||
uartPrint("Must be any order of AETR1234\r\n");
|
||||
else {
|
||||
parseRcChannels(cmdline);
|
||||
cliMap("");
|
||||
return;
|
||||
}
|
||||
parseRcChannels(cmdline);
|
||||
}
|
||||
uartPrint("Current assignment: ");
|
||||
for (i = 0; i < 8; i++)
|
||||
out[cfg.rcmap[i]] = rcChannelLetters[i];
|
||||
out[i] = '\0';
|
||||
uartPrint(out);
|
||||
uartPrint("\r\n");
|
||||
}
|
||||
|
||||
static void cliMixer(char *cmdline)
|
||||
|
|
|
|||
249
src/gps.c
Normal file
249
src/gps.c
Normal file
|
|
@ -0,0 +1,249 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
#ifndef sq
|
||||
#define sq(x) ((x)*(x))
|
||||
#endif
|
||||
|
||||
static void GPS_NewData(uint16_t c);
|
||||
static bool GPS_newFrame(char c);
|
||||
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing);
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
*
|
||||
* GPS low level routines
|
||||
*
|
||||
*-----------------------------------------------------------*/
|
||||
|
||||
void USART2_IRQHandler(void)
|
||||
{
|
||||
if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
|
||||
GPS_NewData(USART_ReceiveData(USART2));
|
||||
}
|
||||
}
|
||||
|
||||
static void uart2Init(void)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
USART_InitTypeDef USART_InitStructure;
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
|
||||
|
||||
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
USART_InitStructure.USART_BaudRate = 9600;
|
||||
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
|
||||
USART_InitStructure.USART_StopBits = USART_StopBits_1;
|
||||
USART_InitStructure.USART_Parity = USART_Parity_No;
|
||||
USART_InitStructure.USART_Mode = USART_Mode_Rx;
|
||||
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_Init(USART2, &USART_InitStructure);
|
||||
|
||||
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
|
||||
USART_Cmd(USART2, ENABLE);
|
||||
}
|
||||
|
||||
void gpsInit(void)
|
||||
{
|
||||
uart2Init();
|
||||
sensorsSet(SENSOR_GPS);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
*
|
||||
* Multiwii GPS code
|
||||
*
|
||||
*-----------------------------------------------------------*/
|
||||
|
||||
static void GPS_NewData(uint16_t c)
|
||||
{
|
||||
if (GPS_newFrame(c)) {
|
||||
if (GPS_update == 1)
|
||||
GPS_update = 0;
|
||||
else
|
||||
GPS_update = 1;
|
||||
if (GPS_fix == 1 && GPS_numSat > 3) {
|
||||
if (GPS_fix_home == 0) {
|
||||
GPS_fix_home = 1;
|
||||
GPS_latitude_home = GPS_latitude;
|
||||
GPS_longitude_home = GPS_longitude;
|
||||
}
|
||||
if (GPSModeHold == 1)
|
||||
GPS_distance(GPS_latitude_hold, GPS_longitude_hold, GPS_latitude, GPS_longitude, &GPS_distanceToHold, &GPS_directionToHold);
|
||||
else
|
||||
GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* this is an equirectangular approximation to calculate distance and bearing between 2 GPS points (lat/long)
|
||||
it's much more faster than an exact calculation
|
||||
the error is neglectible for few kilometers assuming a constant R for earth
|
||||
input: lat1/long1 <-> lat2/long2 unit: 1/100000 degree
|
||||
output: distance in meters, bearing in degrees
|
||||
*/
|
||||
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing)
|
||||
{
|
||||
float dLat = (lat2 - lat1); // difference of latitude in 1/100000 degrees
|
||||
float dLon = (lon2 - lon1) * cos(lat1 * (PI / 180 / 100000.0)); // difference of longitude in 1/100000 degrees
|
||||
*dist = 6372795 / 100000.0 * PI / 180 * (sqrt(sq(dLat) + sq(dLon)));
|
||||
*bearing = 180 / PI * (atan2(dLon, dLat));
|
||||
}
|
||||
|
||||
/* The latitude or longitude is coded this way in NMEA frames
|
||||
dm.m coded as degrees + minutes + minute decimal
|
||||
Where:
|
||||
- d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
|
||||
- m is always 2 char long
|
||||
- m can be 1 or more char long
|
||||
This function converts this format in a unique unsigned long where 1 degree = 100 000
|
||||
*/
|
||||
static uint32_t GPS_coord_to_degrees(char *s)
|
||||
{
|
||||
char *p, *d = s;
|
||||
uint32_t sec, m = 1000;
|
||||
uint16_t min, dec = 0;
|
||||
|
||||
if (!*s)
|
||||
return 0;
|
||||
for (p = s; *p != 0; p++) {
|
||||
if (d != s) {
|
||||
*p -= '0';
|
||||
dec += *p * m;
|
||||
m /= 10;
|
||||
}
|
||||
if (*p == '.')
|
||||
d = p;
|
||||
}
|
||||
m = 10000;
|
||||
min = *--d - '0';
|
||||
min += (*--d - '0') * 10;
|
||||
sec = (m * min + dec) / 6;
|
||||
while (d != s) {
|
||||
m *= 10;
|
||||
*--d -= '0';
|
||||
sec += *d * m;
|
||||
}
|
||||
return sec;
|
||||
}
|
||||
|
||||
// helper functions
|
||||
static uint16_t grab_fields(char *src, uint8_t mult)
|
||||
{ // convert string to uint16
|
||||
uint8_t i;
|
||||
uint16_t tmp = 0;
|
||||
for (i = 0; src[i] != 0; i++) {
|
||||
if (src[i] == '.') {
|
||||
i++;
|
||||
if (mult == 0)
|
||||
break;
|
||||
else
|
||||
src[i + mult] = 0;
|
||||
}
|
||||
tmp *= 10;
|
||||
if (src[i] >= '0' && src[i] <= '9')
|
||||
tmp += src[i] - '0';
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
static uint8_t hex_c(uint8_t n)
|
||||
{ // convert '0'..'9','A'..'F' to 0..15
|
||||
n -= '0';
|
||||
if (n > 9)
|
||||
n -= 7;
|
||||
n &= 0x0F;
|
||||
return n;
|
||||
}
|
||||
|
||||
/* This is a light implementation of a GPS frame decoding
|
||||
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
|
||||
Here we use only the following data :
|
||||
- latitude
|
||||
- longitude
|
||||
- GPS fix is/is not ok
|
||||
- GPS num sat (4 is enough to be +/- reliable)
|
||||
// added by Mis
|
||||
- GPS altitude (for OSD displaying)
|
||||
- GPS speed (for OSD displaying)
|
||||
*/
|
||||
#define FRAME_GGA 1
|
||||
#define FRAME_RMC 2
|
||||
|
||||
static bool GPS_newFrame(char c)
|
||||
{
|
||||
uint8_t frameOK = 0;
|
||||
static uint8_t param = 0, offset = 0, parity = 0;
|
||||
static char string[15];
|
||||
static uint8_t checksum_param, frame = 0;
|
||||
|
||||
if (c == '$') {
|
||||
param = 0;
|
||||
offset = 0;
|
||||
parity = 0;
|
||||
} else if (c == ',' || c == '*') {
|
||||
string[offset] = 0;
|
||||
if (param == 0) { //frame identification
|
||||
frame = 0;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
|
||||
frame = FRAME_GGA;
|
||||
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
|
||||
frame = FRAME_RMC;
|
||||
} else if (frame == FRAME_GGA) {
|
||||
if (param == 2) {
|
||||
GPS_latitude = GPS_coord_to_degrees(string);
|
||||
} else if (param == 3 && string[0] == 'S')
|
||||
GPS_latitude = -GPS_latitude;
|
||||
else if (param == 4) {
|
||||
GPS_longitude = GPS_coord_to_degrees(string);
|
||||
} else if (param == 5 && string[0] == 'W')
|
||||
GPS_longitude = -GPS_longitude;
|
||||
else if (param == 6) {
|
||||
GPS_fix = string[0] > '0';
|
||||
} else if (param == 7) {
|
||||
GPS_numSat = grab_fields(string, 0);
|
||||
} else if (param == 9) {
|
||||
GPS_altitude = grab_fields(string, 0);
|
||||
} // altitude in meters added by Mis
|
||||
} else if (frame == FRAME_RMC) {
|
||||
if (param == 7) {
|
||||
GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L;
|
||||
} // speed in cm/s added by Mis
|
||||
}
|
||||
param++;
|
||||
offset = 0;
|
||||
if (c == '*')
|
||||
checksum_param = 1;
|
||||
else
|
||||
parity ^= c;
|
||||
} else if (c == '\r' || c == '\n') {
|
||||
if (checksum_param) { //parity checksum
|
||||
uint8_t checksum = hex_c(string[0]);
|
||||
checksum <<= 4;
|
||||
checksum += hex_c(string[1]);
|
||||
if (checksum == parity)
|
||||
frameOK = 1;
|
||||
}
|
||||
checksum_param = 0;
|
||||
} else {
|
||||
if (offset < 15)
|
||||
string[offset++] = c;
|
||||
if (!checksum_param)
|
||||
parity ^= c;
|
||||
}
|
||||
return frameOK && (frame == FRAME_GGA);
|
||||
}
|
||||
12
src/main.c
12
src/main.c
|
|
@ -8,7 +8,7 @@ void throttleCalibration(void)
|
|||
uint8_t offset = useServo ? 2 : 0;
|
||||
uint8_t len = pwmGetNumOutputChannels() - offset;
|
||||
uint8_t i;
|
||||
|
||||
|
||||
LED1_ON;
|
||||
|
||||
// write maxthrottle (high)
|
||||
|
|
@ -28,7 +28,7 @@ void throttleCalibration(void)
|
|||
int main(void)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
|
||||
#if 0
|
||||
// using this to write asm for bootloader :)
|
||||
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
|
||||
|
|
@ -36,7 +36,7 @@ int main(void)
|
|||
AFIO->MAPR = 0x02000000;
|
||||
GPIOB->BRR = 0x18; // set low 4 & 3
|
||||
GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
|
||||
#endif
|
||||
#endif
|
||||
|
||||
systemInit();
|
||||
|
||||
|
|
@ -72,6 +72,10 @@ int main(void)
|
|||
if (feature(FEATURE_VBAT))
|
||||
batteryInit();
|
||||
|
||||
// Optional GPS - available only when using PPM, otherwise required pins won't be usable
|
||||
if (feature(FEATURE_PPM) && feature(FEATURE_GPS))
|
||||
gpsInit();
|
||||
|
||||
previousTime = micros();
|
||||
calibratingG = 400;
|
||||
#if defined(POWERMETER)
|
||||
|
|
@ -79,7 +83,7 @@ int main(void)
|
|||
pMeter[i] = 0;
|
||||
#endif
|
||||
|
||||
// loopy
|
||||
// loopy
|
||||
while (1) {
|
||||
loop();
|
||||
}
|
||||
|
|
|
|||
231
src/mw.c
231
src/mw.c
|
|
@ -10,23 +10,23 @@ int16_t debug1, debug2, debug3, debug4;
|
|||
uint8_t buzzerState = 0;
|
||||
uint32_t currentTime = 0;
|
||||
uint32_t previousTime = 0;
|
||||
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
||||
uint8_t GPSModeHome = 0; // if GPS RTH is activated
|
||||
uint8_t GPSModeHold = 0; // if GPS PH is activated
|
||||
uint8_t headFreeMode = 0; // if head free mode is a activated
|
||||
uint8_t passThruMode = 0; // if passthrough mode is activated
|
||||
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
||||
uint8_t GPSModeHome = 0; // if GPS RTH is activated
|
||||
uint8_t GPSModeHold = 0; // if GPS PH is activated
|
||||
uint8_t headFreeMode = 0; // if head free mode is a activated
|
||||
uint8_t passThruMode = 0; // if passthrough mode is activated
|
||||
int16_t headFreeModeHold;
|
||||
int16_t annex650_overrun_count = 0;
|
||||
uint8_t armed = 0;
|
||||
uint8_t vbat; // battery voltage in 0.1V steps
|
||||
uint8_t vbat; // battery voltage in 0.1V steps
|
||||
|
||||
volatile int16_t failsafeCnt = 0;
|
||||
int16_t failsafeEvents = 0;
|
||||
int16_t rcData[8]; // interval [1000;2000]
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
int16_t rcData[8]; // interval [1000;2000]
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
//uint8_t rcRate8;
|
||||
//uint8_t rcExpo8;
|
||||
int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||
int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||
|
||||
// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
|
||||
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
|
|
@ -37,9 +37,9 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
|
|||
// uint8_t activate2[CHECKBOXITEMS];
|
||||
uint8_t rcOptions[CHECKBOXITEMS];
|
||||
uint8_t okToArm = 0;
|
||||
uint8_t accMode = 0; // if level mode is a activated
|
||||
uint8_t magMode = 0; // if compass heading hold is a activated
|
||||
uint8_t baroMode = 0; // if altitude hold is activated
|
||||
uint8_t accMode = 0; // if level mode is a activated
|
||||
uint8_t magMode = 0; // if compass heading hold is a activated
|
||||
uint8_t baroMode = 0; // if altitude hold is activated
|
||||
|
||||
int16_t axisPID[3];
|
||||
volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
|
||||
|
|
@ -49,15 +49,16 @@ volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502
|
|||
// **********************
|
||||
int32_t GPS_latitude, GPS_longitude;
|
||||
int32_t GPS_latitude_home, GPS_longitude_home;
|
||||
int32_t GPS_latitude_hold, GPS_longitude_hold;
|
||||
uint8_t GPS_fix, GPS_fix_home = 0;
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_distanceToHome; // in meters
|
||||
int16_t GPS_directionToHome = 0; // in degrees
|
||||
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
|
||||
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
|
||||
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
|
||||
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
||||
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
||||
uint16_t GPS_distanceToHome; // in meters
|
||||
int16_t GPS_directionToHome = 0; // in degrees
|
||||
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
|
||||
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
|
||||
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
|
||||
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
||||
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
||||
|
||||
|
||||
//Automatic ACC Offset Calibration
|
||||
|
|
@ -72,11 +73,11 @@ uint16_t AccInflightCalibrationActive = 0;
|
|||
// power meter
|
||||
// **********************
|
||||
#define PMOTOR_SUM 8 // index into pMeter[] for sum
|
||||
uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum
|
||||
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
||||
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
||||
uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum
|
||||
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
||||
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
||||
// uint8_t powerTrigger1 = 0;
|
||||
uint16_t powerValue = 0; // last known current
|
||||
uint16_t powerValue = 0; // last known current
|
||||
uint16_t intPowerMeterSum, intPowerTrigger1;
|
||||
uint8_t batteryCellCount = 3; // cell count
|
||||
uint16_t batteryWarningVoltage; // annoying buzzer after this one, battery ready to be dead
|
||||
|
|
@ -84,10 +85,10 @@ uint16_t batteryWarningVoltage; // annoying buzzer after this one, battery ready
|
|||
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
|
||||
{
|
||||
uint8_t i, r;
|
||||
|
||||
|
||||
for (r = 0; r < repeat; r++) {
|
||||
for (i = 0; i < num; i++) {
|
||||
LED0_TOGGLE; // switch LEDPIN state
|
||||
LED0_TOGGLE; // switch LEDPIN state
|
||||
BEEP_ON;
|
||||
delay(wait);
|
||||
BEEP_OFF;
|
||||
|
|
@ -98,7 +99,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
|
|||
|
||||
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
|
||||
void annexCode(void)
|
||||
{
|
||||
{
|
||||
static uint32_t buzzerTime, calibratedAccTime;
|
||||
#if defined(LCD_TELEMETRY)
|
||||
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0;
|
||||
|
|
@ -122,7 +123,7 @@ void annexCode(void)
|
|||
if (rcData[THROTTLE] < 1500) {
|
||||
prop2 = 100;
|
||||
} else if (rcData[THROTTLE] < 2000) {
|
||||
prop2 = 100 - (uint16_t) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500;
|
||||
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - 1500) / 500;
|
||||
} else {
|
||||
prop2 = 100 - cfg.dynThrPID;
|
||||
}
|
||||
|
|
@ -139,7 +140,7 @@ void annexCode(void)
|
|||
if (axis != 2) { //ROLL & PITCH
|
||||
uint16_t tmp2 = tmp / 100;
|
||||
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
|
||||
prop1 = 100 - (uint16_t) cfg.rollPitchRate *tmp / 500;
|
||||
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t) prop1 *prop2 / 100;
|
||||
} else { //YAW
|
||||
rcCommand[axis] = tmp;
|
||||
|
|
@ -160,7 +161,6 @@ void annexCode(void)
|
|||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
||||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
}
|
||||
|
||||
#if defined(POWERMETER_HARD)
|
||||
if (!(++psensorTimer % PSENSORFREQ)) {
|
||||
pMeterRaw = analogRead(PSENSORPIN);
|
||||
|
|
@ -184,23 +184,22 @@ void annexCode(void)
|
|||
vbatRaw += vbatRawArray[i];
|
||||
vbat = batteryAdcToVoltage(vbatRaw / 8);
|
||||
}
|
||||
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
|
||||
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
|
||||
buzzerFreq = 7;
|
||||
} else if (((vbat > batteryWarningVoltage)
|
||||
#if defined(POWERMETER)
|
||||
#if defined(POWERMETER)
|
||||
&& ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0))
|
||||
#endif
|
||||
) || (vbat < cfg.vbatmincellvoltage))
|
||||
{ //VBAT ok AND powermeter ok, buzzer off
|
||||
#endif
|
||||
) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok AND powermeter ok, buzzer off
|
||||
buzzerFreq = 0;
|
||||
buzzerState = 0;
|
||||
BEEP_OFF;
|
||||
#if defined(POWERMETER)
|
||||
} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeter
|
||||
#if defined(POWERMETER)
|
||||
} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeter
|
||||
buzzerFreq = 4;
|
||||
#endif
|
||||
#endif
|
||||
} else
|
||||
buzzerFreq = 4; // low battery
|
||||
buzzerFreq = 4; // low battery
|
||||
if (buzzerFreq) {
|
||||
if (buzzerState && (currentTime > buzzerTime + 250000)) {
|
||||
buzzerState = 0;
|
||||
|
|
@ -214,7 +213,7 @@ void annexCode(void)
|
|||
}
|
||||
}
|
||||
|
||||
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
|
||||
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
|
||||
LED0_TOGGLE;
|
||||
} else {
|
||||
if (calibratedACC == 1) {
|
||||
|
|
@ -292,7 +291,7 @@ void computeRC(void)
|
|||
static int16_t rcData4Values[8][4], rcDataMean[8];
|
||||
static uint8_t rc4ValuesIndex = 0;
|
||||
uint8_t chan, a;
|
||||
|
||||
|
||||
#if defined(SBUS)
|
||||
readSBus();
|
||||
#endif
|
||||
|
|
@ -363,11 +362,11 @@ void loop(void)
|
|||
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] > cfg.maxcheck && armed == 0) {
|
||||
if (rcDelayCommand == 20) {
|
||||
if (cfg.mixerConfiguration == MULTITYPE_TRI) {
|
||||
servo[5] = 1500; // we center the yaw servo in conf mode
|
||||
servo[5] = 1500; // we center the yaw servo in conf mode
|
||||
writeServos();
|
||||
} else if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING) {
|
||||
servo[0] = cfg.wing_left_mid;
|
||||
servo[1] = cfg.wing_right_mid;
|
||||
servo[0] = cfg.wing_left_mid;
|
||||
servo[1] = cfg.wing_right_mid;
|
||||
writeServos();
|
||||
}
|
||||
#if defined(LCD_CONF)
|
||||
|
|
@ -459,11 +458,11 @@ void loop(void)
|
|||
#endif
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = 0;
|
||||
}
|
||||
if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
||||
if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
||||
if (!AccInflightCalibrationArmed) {
|
||||
AccInflightCalibrationArmed = 1;
|
||||
InflightcalibratingA = 50;
|
||||
|
|
@ -475,8 +474,9 @@ void loop(void)
|
|||
}
|
||||
}
|
||||
|
||||
for(i = 0; i < CHECKBOXITEMS; i++) {
|
||||
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
|
||||
for (i = 0; i < CHECKBOXITEMS; i++) {
|
||||
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i])
|
||||
|| (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
|
||||
}
|
||||
|
||||
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
|
|
@ -525,53 +525,55 @@ void loop(void)
|
|||
} else
|
||||
headFreeMode = 0;
|
||||
}
|
||||
#if defined(GPS)
|
||||
if (rcOptions[BOXGPSHOME]) {
|
||||
GPSModeHome = 1;
|
||||
} else
|
||||
GPSModeHome = 0;
|
||||
if (rcOptions[BOXGPSHOLD]) {
|
||||
if (GPSModeHold == 0) {
|
||||
GPSModeHold = 1;
|
||||
GPS_latitude_hold = GPS_latitude;
|
||||
GPS_longitude_hold = GPS_longitude;
|
||||
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
if (rcOptions[BOXGPSHOME]) {
|
||||
GPSModeHome = 1;
|
||||
} else
|
||||
GPSModeHome = 0;
|
||||
if (rcOptions[BOXGPSHOLD]) {
|
||||
if (GPSModeHold == 0) {
|
||||
GPSModeHold = 1;
|
||||
GPS_latitude_hold = GPS_latitude;
|
||||
GPS_longitude_hold = GPS_longitude;
|
||||
}
|
||||
} else {
|
||||
GPSModeHold = 0;
|
||||
}
|
||||
} else {
|
||||
GPSModeHold = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (rcOptions[BOXPASSTHRU]) {
|
||||
passThruMode = 1;
|
||||
} else
|
||||
passThruMode = 0;
|
||||
} else { // not in rc loop
|
||||
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
|
||||
} else { // not in rc loop
|
||||
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
|
||||
switch (taskOrder) {
|
||||
case 0:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_MAG))
|
||||
Mag_getADC();
|
||||
break;
|
||||
case 1:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
Baro_update();
|
||||
break;
|
||||
case 2:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
getEstimatedAltitude();
|
||||
break;
|
||||
case 3:
|
||||
taskOrder++;
|
||||
#if GPS
|
||||
GPS_NewData();
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
taskOrder = 0;
|
||||
break;
|
||||
case 0:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_MAG))
|
||||
Mag_getADC();
|
||||
break;
|
||||
case 1:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
Baro_update();
|
||||
break;
|
||||
case 2:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
getEstimatedAltitude();
|
||||
break;
|
||||
case 3:
|
||||
taskOrder++;
|
||||
#if 0 // GPS - not used as we read gps data in interrupt mode
|
||||
GPS_NewData();
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
taskOrder = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -590,7 +592,7 @@ void loop(void)
|
|||
if (dif >= +180)
|
||||
dif -= 360;
|
||||
if (smallAngle25)
|
||||
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
|
||||
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
|
||||
} else
|
||||
magHold = heading;
|
||||
}
|
||||
|
|
@ -598,47 +600,48 @@ void loop(void)
|
|||
if (sensors(SENSOR_BARO)) {
|
||||
if (baroMode) {
|
||||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
|
||||
baroMode = 0; // so that a new althold reference is defined
|
||||
baroMode = 0; // so that a new althold reference is defined
|
||||
}
|
||||
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
|
||||
}
|
||||
}
|
||||
#if GPS
|
||||
uint16_t GPS_dist;
|
||||
int16_t GPS_dir;
|
||||
|
||||
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
|
||||
GPS_angle[ROLL] = 0;
|
||||
GPS_angle[PITCH] = 0;
|
||||
} else {
|
||||
if (GPSModeHome == 1) {
|
||||
GPS_dist = GPS_distanceToHome;
|
||||
GPS_dir = GPS_directionToHome;
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
uint16_t GPS_dist = 0;
|
||||
int16_t GPS_dir = 0;
|
||||
|
||||
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
|
||||
GPS_angle[ROLL] = 0;
|
||||
GPS_angle[PITCH] = 0;
|
||||
} else {
|
||||
float radDiff;
|
||||
if (GPSModeHome == 1) {
|
||||
GPS_dist = GPS_distanceToHome;
|
||||
GPS_dir = GPS_directionToHome;
|
||||
}
|
||||
if (GPSModeHold == 1) {
|
||||
GPS_dist = GPS_distanceToHold;
|
||||
GPS_dir = GPS_directionToHold;
|
||||
}
|
||||
radDiff = (GPS_dir - heading) * 0.0174533f;
|
||||
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
|
||||
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
|
||||
}
|
||||
if (GPSModeHold == 1) {
|
||||
GPS_dist = GPS_distanceToHold;
|
||||
GPS_dir = GPS_directionToHold;
|
||||
}
|
||||
float radDiff = (GPS_dir - heading) * 0.0174533f;
|
||||
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
|
||||
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
|
||||
}
|
||||
#endif
|
||||
|
||||
//**** PITCH & ROLL & YAW PID ****
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (accMode == 1 && axis < 2) { //LEVEL MODE
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
|
||||
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
|
||||
#ifdef LEVEL_PDF
|
||||
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
|
||||
#else
|
||||
PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
#endif
|
||||
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
|
||||
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
||||
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
||||
} else { //ACRO MODE or YAW axis
|
||||
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
|
||||
error -= gyroData[axis];
|
||||
|
|
@ -648,9 +651,9 @@ void loop(void)
|
|||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
|
||||
if (abs(gyroData[axis]) > 640)
|
||||
errorGyroI[axis] = 0;
|
||||
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
||||
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
||||
}
|
||||
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
|
||||
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastGyro[axis] = gyroData[axis];
|
||||
|
|
@ -658,7 +661,7 @@ void loop(void)
|
|||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
|
||||
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
||||
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
||||
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
}
|
||||
|
|
|
|||
8
src/mw.h
8
src/mw.h
|
|
@ -216,10 +216,11 @@ extern uint8_t baroMode;
|
|||
extern uint16_t intPowerMeterSum, intPowerTrigger1;
|
||||
extern int32_t GPS_latitude, GPS_longitude;
|
||||
extern int32_t GPS_latitude_home, GPS_longitude_home;
|
||||
extern int32_t GPS_latitude_hold, GPS_longitude_hold;
|
||||
extern uint8_t GPS_fix, GPS_fix_home;
|
||||
extern uint8_t GPS_numSat;
|
||||
extern uint16_t GPS_distanceToHome;
|
||||
extern int16_t GPS_directionToHome;
|
||||
extern uint16_t GPS_distanceToHome, GPS_distanceToHold;
|
||||
extern int16_t GPS_directionToHome, GPS_directionToHold;
|
||||
extern uint8_t GPS_update;
|
||||
extern uint8_t GPSModeHome;
|
||||
extern uint8_t GPSModeHold;
|
||||
|
|
@ -278,3 +279,6 @@ uint32_t featureMask(void);
|
|||
|
||||
// cli
|
||||
void cliProcess(void);
|
||||
|
||||
// gps
|
||||
void gpsInit(void);
|
||||
|
|
|
|||
|
|
@ -152,7 +152,7 @@ void serialCom(void)
|
|||
for (i = 0; i < 8; i++)
|
||||
serialize16(rcData[i]);
|
||||
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
|
||||
serialize8(accMode | baroMode << 1 | magMode << 2 | (GPSModeHome | GPSModeHold) << 3);
|
||||
serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4);
|
||||
#if defined(LOG_VALUES)
|
||||
serialize16(cycleTimeMax);
|
||||
cycleTimeMax = 0;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue