Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[gps_i2c] gps_i2c module rewritten, added ubx ucenter compatibility

Browse Source
This commit is contained in:
masierra
2016-03-29 14:57:35 -07:00
parent 0b13661a13
commit a3c7563b54
5 changed files with 286 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/gps_i2c.xml
+13 -6
View File
@@ -4,18 +4,25 @@
<doc>
<description>
U-blox GPS (I2C)
(apparently currently broken)
</description>
<configure name="GPS_UBX_I2C_DEV" value="i2cX" description="set i2c peripheral (default: i2c0)"/>
</doc>
<header>
<file name="gps_i2c.h"/>
</header>
<init fun="gps_i2c_init()"/>
<periodic fun="gps_i2c_periodic()" freq="4." delay="4" autorun="TRUE"/>
<event fun="gps_i2cEvent()"/>
<makefile target="ap">
<init fun="gps_ubx_i2c_init()"/>
<periodic fun="gps_ubx_i2c_periodic()" freq="10."/>
<event fun="GpsUbxi2cEvent()"/>
<makefile>
<configure name="GPS_UBX_I2C_DEV" default="i2c2" case="upper|lower"/>
<define name="USE_$(GPS_UBX_I2C_DEV_UPPER)"/>
<define name="GPS_UBX_I2C_DEV" value="$(GPS_UBX_I2C_DEV_LOWER)"/>
<define name="GPS_I2C"/>
<define name="I2C_BUF_LEN" value="255"/>
<file name="gps_i2c.c"/>
<define name="GPS_CONFIGURE" />
</makefile>
</module>
sw/airborne/modules/gps/gps_i2c.c
+183 -104
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger
* Copyright (C) 2016 Michael Sierra <sierramichael.a@gmail.com>
*
* This file is part of paparazzi.
*
@@ -20,131 +20,210 @@
*
*/
#include "modules/gps/gps_i2c.h"
#include "mcu_periph/i2c.h"
#include "subsystems/gps.h"
#include "mcu_periph/uart.h"
#include "mcu_periph/sys_time.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
#include <math.h>
struct i2c_transaction i2c_gps_trans;
// ublox i2c address
#define GPS_I2C_SLAVE_ADDR (0x42 << 1)
uint8_t gps_i2c_rx_buf[GPS_I2C_BUF_SIZE];
uint8_t gps_i2c_rx_insert_idx, gps_i2c_rx_extract_idx;
uint8_t gps_i2c_tx_buf[GPS_I2C_BUF_SIZE];
uint8_t gps_i2c_tx_insert_idx, gps_i2c_tx_extract_idx;
bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit;
/* u-blox5 protocole, page 4 */
#define GPS_I2C_ADDR_NB_AVAIL_BYTES 0xFD
#define GPS_I2C_ADDR_DATA 0xFF
#define GPS_I2C_STATUS_IDLE 0
#define GPS_I2C_STATUS_ASKING_DATA 1
#define GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES 2
#define GPS_I2C_STATUS_READING_NB_AVAIL_BYTES 3
#define GPS_I2C_STATUS_READING_DATA 4
#define gps_i2c_AddCharToRxBuf(_x) { \
gps_i2c_rx_buf[gps_i2c_rx_insert_idx] = _x; \
gps_i2c_rx_insert_idx++; /* size=256, No check for buf overflow */ \
}
static uint8_t gps_i2c_status;
//static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */
//static uint8_t data_buf_len;
void gps_i2c_init(void)
{
gps_i2c_status = GPS_I2C_STATUS_IDLE;
gps_i2c_done = TRUE;
gps_i2c_data_ready_to_transmit = FALSE;
gps_i2c_rx_insert_idx = 0;
gps_i2c_rx_extract_idx = 0;
gps_i2c_tx_insert_idx = 0;
#ifdef GPS_CONFIGURE
/* The call in main_ap.c is made before the modules init (too early) */
gps_configure_uart();
#ifndef GPS_UBX_I2C_DEV
#define GPS_UBX_I2C_DEV i2c2
#endif
PRINT_CONFIG_VAR(GPS_UBX_I2C_DEV)
#define GPS_I2C_ADDR_NB_AVAIL_BYTES 0xFD // number of bytes available register
#define GPS_I2C_ADDR_DATA 0xFF // data stream register
// Global variables
struct GpsUbxI2C gps_i2c;
// Local variables
bool_t gps_ubx_i2c_ucenter_done;
uint16_t gps_ubx_i2c_bytes_to_read;
static void null_function(struct GpsUbxI2C *p __attribute__((unused))) {}
void gps_i2c_put_byte(struct GpsUbxI2C *p, uint8_t data);
void gps_i2c_msg_ready(struct GpsUbxI2C *p);
uint8_t gps_i2c_char_available(struct GpsUbxI2C *p);
uint8_t gps_i2c_getch(struct GpsUbxI2C *p);
void gps_ubx_i2c_init(void)
{
gps_ubx_i2c_ucenter_done = FALSE;
gps_i2c.read_state = gps_i2c_read_standby;
gps_i2c.write_state = gps_i2c_write_standby;
gps_i2c.trans.status = I2CTransDone;
gps_i2c.rx_buf_avail = 0;
gps_i2c.rx_buf_idx = 0;
gps_i2c.tx_buf_idx = 0;
gps_i2c.tx_rdy = TRUE;
gps_i2c.device.periph = (void *)&gps_i2c;
gps_i2c.device.check_free_space = (check_free_space_t)null_function; ///< check if transmit buffer is not full
gps_i2c.device.put_byte = (put_byte_t)gps_i2c_put_byte; ///< put one byte
gps_i2c.device.send_message = (send_message_t)gps_i2c_msg_ready; ///< send completed buffer
gps_i2c.device.char_available = (char_available_t)gps_i2c_char_available; ///< check if a new character is available
gps_i2c.device.get_byte = (get_byte_t)gps_i2c_getch; ///< get a new char
gps_i2c.device.set_baudrate = (set_baudrate_t)null_function; ///< set device baudrate
}
void gps_i2c_periodic(void)
{
/*
if (gps_i2c_done && gps_i2c_status == GPS_I2C_STATUS_IDLE) {
i2c0_buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES;
i2c0_transmit_no_stop(GPS_I2C_SLAVE_ADDR, 1, &gps_i2c_done);
gps_i2c_done = FALSE;
gps_i2c_status = GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES;
}
/*
* put byte into transmit buffer
*/
void gps_i2c_put_byte(struct GpsUbxI2C *p __attribute__((unused)), uint8_t data)
{
gps_i2c.tx_buf[gps_i2c.tx_buf_idx++] = data;
}
void gps_i2c_event(void)
/*
* send buffer when ready
*/
void gps_i2c_msg_ready(struct GpsUbxI2C *p __attribute__((unused)))
{
/*
* switch (gps_i2c_status) {
case GPS_I2C_STATUS_IDLE:
if (gps_i2c_data_ready_to_transmit) {
// Copy data from our buffer to the i2c buffer
uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN);
uint8_t i;
for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++)
i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx];
gps_i2c.write_state = gps_i2c_write_cfg;
gps_i2c.tx_rdy = FALSE;
}
// Start i2c transmit
i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done);
gps_i2c_done = FALSE;
/*
* send message and get reply
*/
void gps_i2c_send(void)
{
memcpy(&gps_i2c.trans.buf, gps_i2c.tx_buf, gps_i2c.tx_buf_idx);
i2c_transmit(&GPS_UBX_I2C_DEV, &gps_i2c.trans, GPS_I2C_SLAVE_ADDR, gps_i2c.tx_buf_idx);
gps_i2c.tx_buf_idx = 0;
gps_i2c.read_state = gps_i2c_read_standby;
gps_i2c.write_state = gps_i2c_write_request_size;
}
// Reset flag if finished
if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) {
gps_i2c_data_ready_to_transmit = FALSE;
gps_i2c_tx_insert_idx = 0;
/*
* is driver ready to send a message
*/
bool_t gps_i2c_tx_is_ready(void)
{
return gps_i2c.tx_rdy;
}
/*
* config is done, begin reading messages
*/
void gps_i2c_begin(void)
{
gps_ubx_i2c_ucenter_done = TRUE;
}
/*
* check if a new character is available
*/
uint8_t gps_i2c_char_available(struct GpsUbxI2C *p __attribute__((unused)))
{
return (((int)gps_i2c.rx_buf_avail - (int)gps_i2c.rx_buf_idx) > 0);
}
/*
* get a new char
*/
uint8_t gps_i2c_getch(struct GpsUbxI2C *p __attribute__((unused)))
{
return gps_i2c.rx_buf[gps_i2c.rx_buf_idx++];
}
/*
* handle message sending
*/
void gps_ubx_i2c_periodic(void)
{
if (gps_i2c.trans.status == I2CTransDone)
{
switch(gps_i2c.write_state)
{
case gps_i2c_write_standby:
if (!gps_i2c_char_available(&gps_i2c))
{
if (gps_ubx_i2c_bytes_to_read > GPS_I2C_BUF_SIZE || gps_ubx_i2c_ucenter_done)
{
gps_i2c.write_state = gps_i2c_write_request_size;
} else {
gps_i2c.tx_rdy = TRUE;
}
}
break;
case GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES:
i2c0_receive(GPS_I2C_SLAVE_ADDR, 2, &gps_i2c_done);
gps_i2c_done = FALSE;
gps_i2c_status = GPS_I2C_STATUS_READING_NB_AVAIL_BYTES;
case gps_i2c_write_request_size:
gps_i2c.trans.buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES;
i2c_transceive(&GPS_UBX_I2C_DEV, &gps_i2c.trans, GPS_I2C_SLAVE_ADDR, 1, 2);
gps_i2c.write_state = gps_i2c_write_standby;
gps_i2c.read_state = gps_i2c_read_sizeof;
break;
case GPS_I2C_STATUS_READING_NB_AVAIL_BYTES:
gps_i2c_nb_avail_bytes = (i2c0_buf[0]<<8) | i2c0_buf[1];
if (gps_i2c_nb_avail_bytes)
goto continue_reading;
else
gps_i2c_status = GPS_I2C_STATUS_IDLE;
case gps_i2c_write_cfg:
gps_i2c_send();
break;
continue_reading:
case GPS_I2C_STATUS_ASKING_DATA:
data_buf_len = Min(gps_i2c_nb_avail_bytes, I2C0_BUF_LEN);
gps_i2c_nb_avail_bytes -= data_buf_len;
i2c0_receive(GPS_I2C_SLAVE_ADDR, data_buf_len, &gps_i2c_done);
gps_i2c_done = FALSE;
gps_i2c_status = GPS_I2C_STATUS_READING_DATA;
break;
case GPS_I2C_STATUS_READING_DATA: {
uint8_t i;
for(i = 0; i < data_buf_len; i++) {
gps_i2c_AddCharToRxBuf(i2c0_buf[i]);
}
if (gps_i2c_nb_avail_bytes)
goto continue_reading;
else
gps_i2c_status = GPS_I2C_STATUS_IDLE;
break;
}
default:
break;
}
}
}
/*
* handle message reception
*/
void gps_ubx_i2c_read_event(void)
{
switch(gps_i2c.read_state)
{
case gps_i2c_read_standby:
break;
break;
// how many bytes are available
case gps_i2c_read_sizeof:
gps_ubx_i2c_bytes_to_read = ((uint16_t)(gps_i2c.trans.buf[0]) << 7) | (uint16_t)(gps_i2c.trans.buf[1]);
gps_i2c.trans.status = I2CTransDone;
if (gps_ubx_i2c_bytes_to_read == 0xFFFF || gps_ubx_i2c_bytes_to_read == 0x0000)
{
gps_i2c.write_state = gps_i2c_write_request_size;
return;
} else if (gps_ubx_i2c_bytes_to_read > GPS_I2C_BUF_SIZE)
{
gps_i2c.trans.buf[0] = GPS_I2C_ADDR_DATA;
i2c_transceive(&GPS_UBX_I2C_DEV, &gps_i2c.trans, GPS_I2C_SLAVE_ADDR, 1, GPS_I2C_BUF_SIZE);
gps_i2c.read_state = gps_i2c_read_data;
} else
{
gps_i2c.trans.buf[0] = GPS_I2C_ADDR_DATA;
i2c_transceive(&GPS_UBX_I2C_DEV, &gps_i2c.trans, GPS_I2C_SLAVE_ADDR, 1, gps_ubx_i2c_bytes_to_read);
gps_i2c.read_state = gps_i2c_read_data;
return;
}
*/
break;
case gps_i2c_read_data:
if (gps_ubx_i2c_bytes_to_read > GPS_I2C_BUF_SIZE)
{
memcpy(&gps_i2c.rx_buf, &gps_i2c.trans.buf, GPS_I2C_BUF_SIZE);
gps_i2c.rx_buf_idx = 0;
gps_i2c.rx_buf_avail = GPS_I2C_BUF_SIZE;
} else
{
memcpy(&gps_i2c.rx_buf, &gps_i2c.trans.buf, gps_ubx_i2c_bytes_to_read);
gps_i2c.rx_buf_idx = 0;
gps_i2c.rx_buf_avail = gps_ubx_i2c_bytes_to_read;
}
gps_i2c.write_state = gps_i2c_write_standby;
gps_i2c.read_state = gps_i2c_read_standby;
break;
default:
break;
}
// Transaction has been read
gps_i2c.trans.status = I2CTransDone;
}
sw/airborne/modules/gps/gps_i2c.h
+60 -27
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger
* Copyright (C) 2016 Michael Sierra <sierramichael.a@gmail.com>
*
* This file is part of paparazzi.
*
@@ -20,38 +20,71 @@
*
*/
#ifndef GPS_I2C
#define GPS_I2C
#ifndef GPS_UBX_I2C_H
#define GPS_UBX_I2C_H
#include "std.h"
#include "mcu_periph/i2c.h"
#include "pprzlink/pprzlink_device.h"
/// Default address for u-blox (and others?)
#define GPS_I2C_SLAVE_ADDR (0x42 << 1)
#define GPS_I2C_BUF_SIZE 255
#define GPS_I2C_BUF_SIZE 256
extern uint8_t gps_i2c_rx_buf[GPS_I2C_BUF_SIZE];
extern uint8_t gps_i2c_rx_insert_idx, gps_i2c_rx_extract_idx;
extern uint8_t gps_i2c_tx_buf[GPS_I2C_BUF_SIZE];
extern uint8_t gps_i2c_tx_insert_idx, gps_i2c_tx_extract_idx;
typedef enum GpsI2cReadState
{
gps_i2c_read_standby,
gps_i2c_read_sizeof,
gps_i2c_read_data,
gps_i2c_read_ack
} GpsI2cReadState;
extern bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit;
typedef enum GpsI2cWriteState
{
gps_i2c_write_standby,
gps_i2c_write_request_size,
gps_i2c_write_cfg,
gps_i2c_write_get_ack
} GpsI2cWriteState;
void gps_i2c_init(void);
void gps_i2c_event(void);
void gps_i2c_periodic(void);
struct GpsUbxI2C
{
GpsI2cReadState read_state;
GpsI2cWriteState write_state;
#define gps_i2cEvent() { if (gps_i2c_done) gps_i2c_event(); }
#define gps_i2cChAvailable() (gps_i2c_rx_insert_idx != gps_i2c_rx_extract_idx)
#define gps_i2cGetch() (gps_i2c_rx_buf[gps_i2c_rx_extract_idx++])
#define gps_i2cTransmit(_char) { \
if (! gps_i2c_data_ready_to_transmit) /* Else transmitting, overrun*/ \
gps_i2c_tx_buf[gps_i2c_tx_insert_idx++] = _char; \
uint8_t rx_buf[GPS_I2C_BUF_SIZE];
uint8_t tx_buf[GPS_I2C_BUF_SIZE];
uint16_t rx_buf_avail;
uint16_t rx_buf_idx;
uint16_t tx_buf_idx;
bool_t tx_rdy;
uint16_t bytes_to_read;
struct i2c_transaction trans;
int baudrate; //not actually used duhh
struct link_device device;
};
extern struct GpsUbxI2C gps_i2c;
void gps_i2c_send(void);
extern void gps_ubx_i2c_init(void);
extern void gps_ubx_i2c_periodic(void);
extern void gps_ubx_i2c_read_event(void);
extern bool_t gps_i2c_tx_is_ready(void);
extern void gps_i2c_begin(void);
static inline void GpsUbxi2cEvent(void)
{
if (gps_i2c.trans.status == I2CTransSuccess) {
gps_ubx_i2c_read_event();
} else if (gps_i2c.trans.status == I2CTransFailed) {
// if transaction failed, mark as done so can be retried
gps_i2c.trans.status = I2CTransDone;
}
#define gps_i2cSendMessage() { \
gps_i2c_data_ready_to_transmit = TRUE; \
gps_i2c_tx_extract_idx = 0; \
}
// #define gps_i2cTxRunning (gps_i2c_data_ready_to_transmit)
// #define gps_i2cInitParam(_baud, _uart_prm1, _uart_prm2) {}
}
#endif // GPS_I2C
#endif // GPS_UBX_I2C_H
sw/airborne/modules/gps/gps_ubx_ucenter.c
+28 -5
View File
@@ -43,13 +43,15 @@
//////////////////////////////////////////////////////////////////////////////////////
//
// UCENTER: init, periodic and event
#ifndef GPS_I2C
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
#endif
static bool_t gps_ubx_ucenter_configure(uint8_t nr);
#define GPS_UBX_UCENTER_STATUS_STOPPED 0
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
#define GPS_UBX_UCENTER_STATUS_CONFIG 2
#define GPS_UBX_UCENTER_STATUS_WAITING 3
#define GPS_UBX_UCENTER_REPLY_NONE 0
#define GPS_UBX_UCENTER_REPLY_ACK 1
@@ -101,8 +103,13 @@ void gps_ubx_ucenter_periodic(void)
// Save processing time inflight
case GPS_UBX_UCENTER_STATUS_STOPPED:
return;
break;
// Automatically Determine Current Baudrate
case GPS_UBX_UCENTER_STATUS_AUTOBAUD:
#ifdef GPS_I2C
gps_ubx_ucenter.cnt = 0;
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
#else
if (gps_ubx_ucenter_autobaud(gps_ubx_ucenter.cnt) == FALSE) {
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
gps_ubx_ucenter.cnt = 0;
@@ -116,14 +123,27 @@ void gps_ubx_ucenter_periodic(void)
} else {
gps_ubx_ucenter.cnt++;
}
#endif /* GPS_I2C */
break;
// Send Configuration
case GPS_UBX_UCENTER_STATUS_CONFIG:
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) {
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
#ifdef GPS_I2C
gps_i2c_begin();
#endif
gps_ubx_ucenter.cnt = 0;
} else {
gps_ubx_ucenter.cnt++;
#ifdef GPS_I2C
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_WAITING;
}
break;
case GPS_UBX_UCENTER_STATUS_WAITING:
if (gps_i2c_tx_is_ready())
{
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
#endif /*GPS_I2C*/
}
break;
default:
@@ -147,10 +167,10 @@ void gps_ubx_ucenter_event(void)
// Read Configuration Reply's
switch (gps_ubx.msg_class) {
case UBX_ACK_ID:
if (gps_ubx.msg_id == UBX_ACK_ACK_ID) {
if (gps_ubx.msg_id & UBX_ACK_ACK_ID) {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
DEBUG_PRINT("ACK\n");
} else {
} else if (gps_ubx.msg_id & UBX_ACK_NAK_ID) {
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
DEBUG_PRINT("NACK\n");
}
@@ -226,6 +246,7 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t
* @param nr Autobaud step number to perform
* @return FALSE when completed
*/
#ifndef GPS_I2C
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
{
switch (nr) {
@@ -300,7 +321,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
}
return TRUE;
}
#endif /* GPS_I2C */
/////////////////////////////////////
// UBlox internal Navigation Solution
@@ -393,7 +414,7 @@ static inline void gps_ubx_ucenter_config_port(void)
// I2C Interface
case GPS_PORT_DDC:
#ifdef GPS_I2C
UbxSend_CFG_PRT(gps_ubx_ucenter.dev, gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK | NMEA_PROTO_MASK, UBX_PROTO_MASK| NMEA_PROTO_MASK, 0x0, 0x0);
UbxSend_CFG_PRT(gps_ubx_ucenter.dev, gps_ubx_ucenter.port_id, 0x0, 0x0, (0x42<<1), 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#else
DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
#endif
@@ -457,6 +478,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
gps_ubx_ucenter_config_port();
break;
case 1:
#ifndef GPS_I2C
#if PRINT_DEBUG_GPS_UBX_UCENTER
if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) {
DEBUG_PRINT("ublox did not acknowledge port configuration.\n");
@@ -467,6 +489,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
// Now the GPS baudrate should have changed
uart_periph_set_baudrate(&(UBX_GPS_LINK), gps_ubx_ucenter.baud_target);
gps_ubx_ucenter.baud_run = UART_SPEED(gps_ubx_ucenter.baud_target);
#endif /*GPS_I2C*/
UbxSend_MON_GET_VER(gps_ubx_ucenter.dev);
break;
case 2:
sw/airborne/subsystems/gps/gps_ubx.h
+4
View File
@@ -33,6 +33,10 @@
#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE"
#endif
#ifdef GPS_I2C
#include "modules/gps/gps_i2c.h"
#endif
#include "mcu_periph/uart.h"
#ifndef PRIMARY_GPS