Finished merging GPS autoconfig + ubx protocol code.
use set gps_type=X in cli to configure, where X=0 if NMEA (no special config), X=1 if UBX (enters ubx binary mode), X=2 if MTK (sets up MTK for 5Hz operation). changed default GPS rate to 115200 baud added baudrate reset stuff to UART driver for GPS autoconfigure NONE of this is tested (except UBX working on my window) git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@204 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com
parent
80d7ba604b
commit
b2bc4bf9e6
4041
obj/baseflight.hex
4041
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
|
@ -49,6 +49,12 @@ typedef enum {
|
|||
FEATURE_TELEMETRY = 1 << 11,
|
||||
} AvailableFeatures;
|
||||
|
||||
typedef enum {
|
||||
GPS_NMEA = 0,
|
||||
GPS_UBLOX,
|
||||
GPS_MTK,
|
||||
} GPSHardware;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
|
||||
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||
typedef int32_t (* baroCalculateFuncPtr)(void); // baro calculation (returns altitude in cm based on static data collected)
|
||||
|
|
|
|||
|
|
@ -184,8 +184,8 @@ void checkFirstTime(bool reset)
|
|||
cfg.gimbal_roll_mid = 1500;
|
||||
|
||||
// gps/nav stuff
|
||||
cfg.gps_type = 0; // NMEA
|
||||
cfg.gps_baudrate = 9600;
|
||||
cfg.gps_type = GPS_NMEA;
|
||||
cfg.gps_baudrate = 115200;
|
||||
cfg.gps_wp_radius = 200;
|
||||
cfg.gps_lpf = 20;
|
||||
cfg.nav_slew_rate = 30;
|
||||
|
|
|
|||
|
|
@ -157,11 +157,25 @@ uint32_t tx2BufferTail = 0;
|
|||
uint32_t tx2BufferHead = 0;
|
||||
bool uart2RxOnly = false;
|
||||
|
||||
static void uart2Open(uint32_t speed)
|
||||
{
|
||||
USART_InitTypeDef USART_InitStructure;
|
||||
|
||||
USART_StructInit(&USART_InitStructure);
|
||||
USART_InitStructure.USART_BaudRate = speed;
|
||||
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 | (uart2RxOnly ? 0 : USART_Mode_Tx);
|
||||
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_Init(USART2, &USART_InitStructure);
|
||||
USART_Cmd(USART2, ENABLE);
|
||||
}
|
||||
|
||||
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
USART_InitTypeDef USART_InitStructure;
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
|
||||
|
||||
|
|
@ -184,21 +198,18 @@ void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly)
|
|||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
USART_InitStructure.USART_BaudRate = speed;
|
||||
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 | (rxOnly ? 0 : USART_Mode_Tx);
|
||||
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_Init(USART2, &USART_InitStructure);
|
||||
|
||||
uart2Open(speed);
|
||||
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
|
||||
if (!rxOnly)
|
||||
USART_ITConfig(USART2, USART_IT_TXE, ENABLE);
|
||||
USART_Cmd(USART2, ENABLE);
|
||||
uart2Callback = func;
|
||||
}
|
||||
|
||||
void uart2ChangeBaud(uint32_t speed)
|
||||
{
|
||||
uart2Open(speed);
|
||||
}
|
||||
|
||||
void uart2Write(uint8_t ch)
|
||||
{
|
||||
if (uart2RxOnly)
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@ uint8_t uartReadPoll(void);
|
|||
void uartWrite(uint8_t ch);
|
||||
void uartPrint(char *str);
|
||||
|
||||
// USART2 (
|
||||
// USART2 (GPS, Spektrum)
|
||||
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func, bool rxOnly);
|
||||
void uart2ChangeBaud(uint32_t speed);
|
||||
void uart2Write(uint8_t ch);
|
||||
|
|
|
|||
331
src/gps.c
331
src/gps.c
|
|
@ -5,20 +5,99 @@
|
|||
#define sq(x) ((x)*(x))
|
||||
#endif
|
||||
|
||||
uint32_t init_speed[5] = { 9600, 19200, 38400, 57600, 115200 };
|
||||
|
||||
static void GPS_NewData(uint16_t c);
|
||||
static void GPS_set_pids(void);
|
||||
static void gpsPrint(const char *str);
|
||||
|
||||
static const char *gpsInitStrings[] = {
|
||||
"$PUBX,41,1,0003,0001,19200,0*23\r\n", // UBX0..3
|
||||
"$PUBX,41,1,0003,0001,38400,0*26\r\n",
|
||||
"$PUBX,41,1,0003,0001,57600,0*2D\r\n",
|
||||
"$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[] = {
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19, // disable all default NMEA messages
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15,
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11,
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F,
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13,
|
||||
0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17,
|
||||
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, 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, 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
|
||||
};
|
||||
|
||||
void gpsInit(uint32_t baudrate)
|
||||
{
|
||||
int i;
|
||||
int offset = 0;
|
||||
|
||||
GPS_set_pids();
|
||||
uart2Init(baudrate, GPS_NewData, false);
|
||||
|
||||
if (cfg.gps_type == GPS_UBLOX)
|
||||
offset = 0;
|
||||
else if (cfg.gps_type == GPS_MTK)
|
||||
offset = 4;
|
||||
|
||||
if (cfg.gps_type != GPS_NMEA) {
|
||||
for (i = 0; i < 5; i++) {
|
||||
uart2ChangeBaud(init_speed[i]);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
uart2ChangeBaud(baudrate);
|
||||
if (cfg.gps_type == GPS_UBLOX) {
|
||||
for (i = 0; i < sizeof(ubloxInit); i++) {
|
||||
uart2Write(ubloxInit[i]); // send ubx init binary
|
||||
delay(4);
|
||||
}
|
||||
} else if (cfg.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(500);
|
||||
delay(1000);
|
||||
if (GPS_Present)
|
||||
sensorsSet(SENSOR_GPS);
|
||||
}
|
||||
|
||||
static void gpsPrint(const char *str)
|
||||
{
|
||||
while (*str) {
|
||||
uart2Write(*str);
|
||||
if (cfg.gps_type == GPS_UBLOX)
|
||||
delay(4);
|
||||
str++;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
*
|
||||
* Multiwii GPS code - revision: 1097
|
||||
|
|
@ -46,6 +125,8 @@ static void GPS_calc_nav_rate(int max_speed);
|
|||
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 int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
|
||||
int32_t wrap_18000(int32_t error);
|
||||
static int32_t wrap_36000(int32_t angle);
|
||||
|
|
@ -669,11 +750,11 @@ static uint8_t hex_c(uint8_t n)
|
|||
static bool GPS_newFrame(char c)
|
||||
{
|
||||
switch (cfg.gps_type) {
|
||||
case 0: // NMEA
|
||||
case GPS_NMEA: // NMEA
|
||||
case GPS_MTK: // MTK outputs NMEA too
|
||||
return GPS_NMEA_newFrame(c);
|
||||
|
||||
case 1: // UBX
|
||||
return false; // GPS_UBLOX_newFrame(c);
|
||||
case GPS_UBLOX: // UBX
|
||||
return GPS_UBLOX_newFrame(c);
|
||||
}
|
||||
|
||||
return false;
|
||||
|
|
@ -761,3 +842,243 @@ static bool GPS_NMEA_newFrame(char c)
|
|||
GPS_Present = 1;
|
||||
return frameOK && (frame == FRAME_GGA);
|
||||
}
|
||||
|
||||
|
||||
// UBX support
|
||||
typedef struct {
|
||||
uint8_t preamble1;
|
||||
uint8_t preamble2;
|
||||
uint8_t msg_class;
|
||||
uint8_t msg_id;
|
||||
uint16_t length;
|
||||
} ubx_header;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t longitude;
|
||||
int32_t latitude;
|
||||
int32_t altitude_ellipsoid;
|
||||
int32_t altitude_msl;
|
||||
uint32_t horizontal_accuracy;
|
||||
uint32_t vertical_accuracy;
|
||||
} ubx_nav_posllh;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time; // GPS msToW
|
||||
uint8_t fix_type;
|
||||
uint8_t fix_status;
|
||||
uint8_t differential_status;
|
||||
uint8_t res;
|
||||
uint32_t time_to_first_fix;
|
||||
uint32_t uptime; // milliseconds
|
||||
} ubx_nav_status;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time;
|
||||
int32_t time_nsec;
|
||||
int16_t week;
|
||||
uint8_t fix_type;
|
||||
uint8_t fix_status;
|
||||
int32_t ecef_x;
|
||||
int32_t ecef_y;
|
||||
int32_t ecef_z;
|
||||
uint32_t position_accuracy_3d;
|
||||
int32_t ecef_x_velocity;
|
||||
int32_t ecef_y_velocity;
|
||||
int32_t ecef_z_velocity;
|
||||
uint32_t speed_accuracy;
|
||||
uint16_t position_DOP;
|
||||
uint8_t res;
|
||||
uint8_t satellites;
|
||||
uint32_t res2;
|
||||
} ubx_nav_solution;
|
||||
|
||||
typedef struct {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t ned_north;
|
||||
int32_t ned_east;
|
||||
int32_t ned_down;
|
||||
uint32_t speed_3d;
|
||||
uint32_t speed_2d;
|
||||
int32_t heading_2d;
|
||||
uint32_t speed_accuracy;
|
||||
uint32_t heading_accuracy;
|
||||
} ubx_nav_velned;
|
||||
|
||||
enum {
|
||||
PREAMBLE1 = 0xb5,
|
||||
PREAMBLE2 = 0x62,
|
||||
CLASS_NAV = 0x01,
|
||||
CLASS_ACK = 0x05,
|
||||
CLASS_CFG = 0x06,
|
||||
MSG_ACK_NACK = 0x00,
|
||||
MSG_ACK_ACK = 0x01,
|
||||
MSG_POSLLH = 0x2,
|
||||
MSG_STATUS = 0x3,
|
||||
MSG_SOL = 0x6,
|
||||
MSG_VELNED = 0x12,
|
||||
MSG_CFG_PRT = 0x00,
|
||||
MSG_CFG_RATE = 0x08,
|
||||
MSG_CFG_SET_RATE = 0x01,
|
||||
MSG_CFG_NAV_SETTINGS = 0x24
|
||||
} ubs_protocol_bytes;
|
||||
|
||||
enum {
|
||||
FIX_NONE = 0,
|
||||
FIX_DEAD_RECKONING = 1,
|
||||
FIX_2D = 2,
|
||||
FIX_3D = 3,
|
||||
FIX_GPS_DEAD_RECKONING = 4,
|
||||
FIX_TIME = 5
|
||||
} ubs_nav_fix_type;
|
||||
|
||||
enum {
|
||||
NAV_STATUS_FIX_VALID = 1
|
||||
} ubx_nav_status_bits;
|
||||
|
||||
// Packet checksum accumulators
|
||||
static uint8_t _ck_a;
|
||||
static uint8_t _ck_b;
|
||||
|
||||
// State machine state
|
||||
static uint8_t _step;
|
||||
static uint8_t _msg_id;
|
||||
static uint16_t _payload_length;
|
||||
static uint16_t _payload_counter;
|
||||
|
||||
static bool next_fix;
|
||||
static uint8_t _class;
|
||||
|
||||
// do we have new position information?
|
||||
static bool _new_position;
|
||||
|
||||
// do we have new speed information?
|
||||
static bool _new_speed;
|
||||
|
||||
static uint8_t _disable_counter;
|
||||
|
||||
// Receive buffer
|
||||
static union {
|
||||
ubx_nav_posllh posllh;
|
||||
ubx_nav_status status;
|
||||
ubx_nav_solution solution;
|
||||
ubx_nav_velned velned;
|
||||
uint8_t bytes[64];
|
||||
} _buffer;
|
||||
|
||||
void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
|
||||
{
|
||||
while (len--) {
|
||||
*ck_a += *data;
|
||||
*ck_b += *ck_a;
|
||||
data++;
|
||||
}
|
||||
}
|
||||
|
||||
static bool GPS_UBLOX_newFrame(uint8_t data)
|
||||
{
|
||||
bool parsed = false;
|
||||
|
||||
switch (_step) {
|
||||
case 1:
|
||||
if (PREAMBLE2 == data) {
|
||||
_step++;
|
||||
break;
|
||||
}
|
||||
_step = 0;
|
||||
case 0:
|
||||
if (PREAMBLE1 == data)
|
||||
_step++;
|
||||
break;
|
||||
case 2:
|
||||
_step++;
|
||||
_class = data;
|
||||
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||
break;
|
||||
case 3:
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_msg_id = data;
|
||||
break;
|
||||
case 4:
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_payload_length = data; // payload length low byte
|
||||
break;
|
||||
case 5:
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_payload_length += (uint16_t) (data << 8);
|
||||
if (_payload_length > 512) {
|
||||
_payload_length = 0;
|
||||
_step = 0;
|
||||
}
|
||||
_payload_counter = 0; // prepare to receive payload
|
||||
break;
|
||||
case 6:
|
||||
_ck_b += (_ck_a += data); // checksum byte
|
||||
if (_payload_counter < sizeof(_buffer)) {
|
||||
_buffer.bytes[_payload_counter] = data;
|
||||
}
|
||||
if (++_payload_counter == _payload_length)
|
||||
_step++;
|
||||
break;
|
||||
case 7:
|
||||
_step++;
|
||||
if (_ck_a != data)
|
||||
_step = 0; // bad checksum
|
||||
break;
|
||||
case 8:
|
||||
_step = 0;
|
||||
if (_ck_b != data)
|
||||
break; // bad checksum
|
||||
GPS_Present = 1;
|
||||
if (UBLOX_parse_gps()) {
|
||||
parsed = true;
|
||||
}
|
||||
} //end switch
|
||||
return parsed;
|
||||
}
|
||||
|
||||
static bool UBLOX_parse_gps(void)
|
||||
{
|
||||
switch (_msg_id) {
|
||||
case MSG_POSLLH:
|
||||
//i2c_dataset.time = _buffer.posllh.time;
|
||||
GPS_coord[LON] = _buffer.posllh.longitude;
|
||||
GPS_coord[LAT] = _buffer.posllh.latitude;
|
||||
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
|
||||
f.GPS_FIX = next_fix;
|
||||
_new_position = true;
|
||||
break;
|
||||
case MSG_STATUS:
|
||||
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
|
||||
if (!next_fix)
|
||||
f.GPS_FIX = false;
|
||||
break;
|
||||
case MSG_SOL:
|
||||
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
|
||||
if (!next_fix)
|
||||
f.GPS_FIX = false;
|
||||
GPS_numSat = _buffer.solution.satellites;
|
||||
// GPS_hdop = _buffer.solution.position_DOP;
|
||||
// debug[3] = GPS_hdop;
|
||||
break;
|
||||
case MSG_VELNED:
|
||||
// speed_3d = _buffer.velned.speed_3d; // cm/s
|
||||
GPS_speed = _buffer.velned.speed_2d; // cm/s
|
||||
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
|
||||
_new_speed = true;
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
// we only return true when we get new position and speed data
|
||||
// this ensures we don't use stale data
|
||||
if (_new_position && _new_speed) {
|
||||
_new_speed = _new_position = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue