diff --git a/conf/modules/gps_i2c.xml b/conf/modules/gps_i2c.xml index 5db107fc91..8711e3a114 100644 --- a/conf/modules/gps_i2c.xml +++ b/conf/modules/gps_i2c.xml @@ -4,18 +4,25 @@ U-blox GPS (I2C) - (apparently currently broken) + +
- - - - + + + + + + + + + + - + diff --git a/sw/airborne/modules/gps/gps_i2c.c b/sw/airborne/modules/gps/gps_i2c.c index 07241cce31..37c4d76759 100644 --- a/sw/airborne/modules/gps/gps_i2c.c +++ b/sw/airborne/modules/gps/gps_i2c.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger + * Copyright (C) 2016 Michael Sierra * * This file is part of paparazzi. * @@ -19,132 +19,211 @@ * Boston, MA 02111-1307, USA. * */ - -#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 -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) +/* + * put byte into transmit buffer + */ +void gps_i2c_put_byte(struct GpsUbxI2C *p __attribute__((unused)), uint8_t data) { - /* - 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; - } - */ - + 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; + default: break; + } + } +} - case GPS_I2C_STATUS_READING_DATA: { - uint8_t i; - for(i = 0; i < data_buf_len; i++) { - gps_i2c_AddCharToRxBuf(i2c0_buf[i]); +/* + * 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; } - if (gps_i2c_nb_avail_bytes) - goto continue_reading; - else - gps_i2c_status = GPS_I2C_STATUS_IDLE; - break; - } + gps_i2c.write_state = gps_i2c_write_standby; + gps_i2c.read_state = gps_i2c_read_standby; + break; default: - return; - } - */ + break; + } + // Transaction has been read + gps_i2c.trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/gps/gps_i2c.h b/sw/airborne/modules/gps/gps_i2c.h index f89fe03d9f..707b0b2d93 100644 --- a/sw/airborne/modules/gps/gps_i2c.h +++ b/sw/airborne/modules/gps/gps_i2c.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger + * Copyright (C) 2016 Michael Sierra * * 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; + +typedef enum GpsI2cWriteState +{ + gps_i2c_write_standby, + gps_i2c_write_request_size, + gps_i2c_write_cfg, + gps_i2c_write_get_ack +} GpsI2cWriteState; -extern bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit; +struct GpsUbxI2C +{ + GpsI2cReadState read_state; + GpsI2cWriteState write_state; -void gps_i2c_init(void); -void gps_i2c_event(void); -void gps_i2c_periodic(void); + uint8_t rx_buf[GPS_I2C_BUF_SIZE]; + uint8_t tx_buf[GPS_I2C_BUF_SIZE]; -#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; \ + 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 \ No newline at end of file diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index 0b4b623666..0ea9de2bc2 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -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,16 +123,29 @@ 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: // stop this module now... // todo @@ -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: diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index f59c377364..63c4baf04c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -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