mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +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
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);
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue