diff --git a/conf/modules/gps_i2c.xml b/conf/modules/gps_i2c.xml
deleted file mode 100644
index 5db107fc91..0000000000
--- a/conf/modules/gps_i2c.xml
+++ /dev/null
@@ -1,21 +0,0 @@
-
-
-
-
-
- U-blox GPS (I2C)
- (apparently currently broken)
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/gps_ubx_i2c.xml b/conf/modules/gps_ubx_i2c.xml
new file mode 100644
index 0000000000..ce51226f26
--- /dev/null
+++ b/conf/modules/gps_ubx_i2c.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+ U-blox GPS (I2C)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/gps/gps_i2c.c b/sw/airborne/modules/gps/gps_i2c.c
deleted file mode 100644
index 07241cce31..0000000000
--- a/sw/airborne/modules/gps/gps_i2c.c
+++ /dev/null
@@ -1,150 +0,0 @@
-/*
- * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "modules/gps/gps_i2c.h"
-#include "mcu_periph/i2c.h"
-#include "subsystems/gps.h"
-
-struct i2c_transaction i2c_gps_trans;
-
-
-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();
-#endif
-}
-
-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;
- }
- */
-
-}
-
-void gps_i2c_event(void)
-{
- /*
- * 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];
-
- // Start i2c transmit
- i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done);
- gps_i2c_done = FALSE;
-
- // 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;
- }
- }
- 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;
- 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;
- 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:
- return;
- }
- */
-
-}
diff --git a/sw/airborne/modules/gps/gps_i2c.h b/sw/airborne/modules/gps/gps_i2c.h
deleted file mode 100644
index f89fe03d9f..0000000000
--- a/sw/airborne/modules/gps/gps_i2c.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef GPS_I2C
-#define GPS_I2C
-
-#include "std.h"
-
-/// Default address for u-blox (and others?)
-#define GPS_I2C_SLAVE_ADDR (0x42 << 1)
-
-#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;
-
-extern bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit;
-
-void gps_i2c_init(void);
-void gps_i2c_event(void);
-void gps_i2c_periodic(void);
-
-#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; \
- }
-#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
diff --git a/sw/airborne/modules/gps/gps_ubx_i2c.c b/sw/airborne/modules/gps/gps_ubx_i2c.c
new file mode 100644
index 0000000000..781ab1ba15
--- /dev/null
+++ b/sw/airborne/modules/gps/gps_ubx_i2c.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger,
+ * 2016 Michael Sierra
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/gps/gps_ubx_i2c.c
+ * pprz link device for Ublox over I2C
+ *
+ * This module adds i2c functionality for the ublox using existing
+ * driver
+ */
+
+#include "mcu_periph/i2c.h"
+#include "modules/gps/gps_ubx_i2c.h"
+#include "subsystems/datalink/downlink.h"
+#include
+
+// ublox i2c address
+#define GPS_I2C_SLAVE_ADDR (0x42 << 1)
+
+#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; ///< ucenter finished configuring flag
+uint16_t gps_ubx_i2c_bytes_to_read; ///< ublox bytes to read
+
+/** null function
+ * @param p unused
+ */
+void null_function(struct GpsUbxI2C *p);
+/** Put byte into transmit buffer.
+ * @param p unused
+ * @param data byte to put in buffer
+ */
+void gps_i2c_put_byte(struct GpsUbxI2C *p, uint8_t data);
+
+/** send buffer when ready
+ * @param p unused
+ */
+void gps_i2c_msg_ready(struct GpsUbxI2C *p);
+/** check if a new character is available
+ * @param p unused
+ */
+uint8_t gps_i2c_char_available(struct GpsUbxI2C *p);
+/** get a new char
+ * @param p unused
+ */
+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 null_function(struct GpsUbxI2C *p __attribute__((unused))) {}
+
+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_msg_ready(struct GpsUbxI2C *p __attribute__((unused)))
+{
+ gps_i2c.write_state = gps_i2c_write_cfg;
+ gps_i2c.tx_rdy = FALSE;
+}
+
+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);
+}
+
+bool_t gps_i2c_tx_is_ready(void)
+{
+ return gps_i2c.tx_rdy;
+}
+
+void gps_i2c_begin(void)
+{
+ gps_ubx_i2c_ucenter_done = TRUE;
+}
+
+uint8_t gps_i2c_getch(struct GpsUbxI2C *p __attribute__((unused)))
+{
+ return gps_i2c.rx_buf[gps_i2c.rx_buf_idx++];
+}
+
+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_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_write_cfg:
+ 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;
+ break;
+
+ default:
+ break;
+ }
+ }
+}
+
+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;
+}
diff --git a/sw/airborne/modules/gps/gps_ubx_i2c.h b/sw/airborne/modules/gps/gps_ubx_i2c.h
new file mode 100644
index 0000000000..1a691fda96
--- /dev/null
+++ b/sw/airborne/modules/gps/gps_ubx_i2c.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger,
+ * 2016 Michael Sierra
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/gps/gps_ubx_i2c.h
+ * pprz link device for Ublox over I2C
+ *
+ * This module adds i2c functionality for the ublox using existing
+ * driver
+ */
+
+#ifndef GPS_UBX_I2C_H
+#define GPS_UBX_I2C_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+#include "pprzlink/pprzlink_device.h"
+
+#define GPS_I2C_BUF_SIZE 255
+
+/** read states
+ */
+typedef enum GpsI2CReadState
+{
+ gps_i2c_read_standby, ///< dont read anything
+ gps_i2c_read_sizeof, ///< read size of ubx buffer
+ gps_i2c_read_data ///< read data from ubx buffer
+} GpsI2CReadState;
+
+/** write states
+ */
+typedef enum GpsI2CWriteState
+{
+ gps_i2c_write_standby, ///< wait for gps_ubx to read buffer or ucenter to transmit
+ gps_i2c_write_request_size, ///< request size of ubx buffer
+ gps_i2c_write_cfg ///< send a config msg and get reply
+} GpsI2CWriteState;
+
+/** ubx_i2c state
+ */
+struct GpsUbxI2C
+{
+ GpsI2CReadState read_state;
+ GpsI2CWriteState write_state;
+
+ uint8_t rx_buf[GPS_I2C_BUF_SIZE]; ///< receive buffer
+ uint8_t tx_buf[GPS_I2C_BUF_SIZE]; ///< transmit buffer
+
+ uint16_t rx_buf_avail; ///< how many bytes are waiting to be read
+ uint16_t rx_buf_idx; ///< rx buf index
+ uint16_t tx_buf_idx; ///< tx buf index
+
+ bool_t tx_rdy; ///< are we ready to transmit
+
+ struct i2c_transaction trans; ///< i2c transaction
+
+ int baudrate; ///< baudrate, unused
+
+ struct link_device device; ///< ppz link device
+};
+
+extern struct GpsUbxI2C gps_i2c;
+/** init function
+ */
+extern void gps_ubx_i2c_init(void);
+/** handle message sending
+ */
+extern void gps_ubx_i2c_periodic(void);
+/** handle message reception
+ */
+extern void gps_ubx_i2c_read_event(void);
+/** is driver ready to send a message
+ */
+extern bool_t gps_i2c_tx_is_ready(void);
+/** config is done, begin reading messages
+ */
+extern void gps_i2c_begin(void);
+
+/** i2c event
+ */
+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;
+ }
+}
+
+#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..2f7dabc254 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_ubx_i2c.h"
+#endif
+
#include "mcu_periph/uart.h"
#ifndef PRIMARY_GPS