mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Merge pull request #1580 from masierra/ms_i2cgps
[gps_i2c] gps_i2c module rewritten, added ubx ucenter compatibility
This commit is contained in:
Gautier Hattenberger
@@ -1,21 +0,0 @@
|
|||||||
<!DOCTYPE module SYSTEM "module.dtd">
|
|
||||||
|
|
||||||
<module name="gps_i2c" dir="gps">
|
|
||||||
<doc>
|
|
||||||
<description>
|
|
||||||
U-blox GPS (I2C)
|
|
||||||
(apparently currently broken)
|
|
||||||
</description>
|
|
||||||
</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">
|
|
||||||
<file name="gps_i2c.c"/>
|
|
||||||
<define name="GPS_CONFIGURE" />
|
|
||||||
</makefile>
|
|
||||||
</module>
|
|
||||||
|
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="gps_ubx_i2c" dir="gps">
|
||||||
|
<doc>
|
||||||
|
<description>
|
||||||
|
U-blox GPS (I2C)
|
||||||
|
</description>
|
||||||
|
<configure name="GPS_UBX_I2C_DEV" value="i2cX" description="set i2c peripheral (default: i2c0)"/>
|
||||||
|
</doc>
|
||||||
|
|
||||||
|
<header>
|
||||||
|
<file name="gps_ubx_i2c.h"/>
|
||||||
|
</header>
|
||||||
|
<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_ubx_i2c.c"/>
|
||||||
|
</makefile>
|
||||||
|
|
||||||
|
</module>
|
||||||
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -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
|
|
||||||
@@ -0,0 +1,223 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger,
|
||||||
|
* 2016 Michael Sierra <sierramichael.a@gmail.com>
|
||||||
|
*
|
||||||
|
* 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 <math.h>
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,111 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2009 ENAC, Pascal Brisset, Michel Gorraz,Gautier Hattenberger,
|
||||||
|
* 2016 Michael Sierra <sierramichael.a@gmail.com>
|
||||||
|
*
|
||||||
|
* 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
|
||||||
@@ -43,13 +43,15 @@
|
|||||||
//////////////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////////////
|
||||||
//
|
//
|
||||||
// UCENTER: init, periodic and event
|
// UCENTER: init, periodic and event
|
||||||
|
#ifndef GPS_I2C
|
||||||
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
|
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr);
|
||||||
|
#endif
|
||||||
static bool_t gps_ubx_ucenter_configure(uint8_t nr);
|
static bool_t gps_ubx_ucenter_configure(uint8_t nr);
|
||||||
|
|
||||||
#define GPS_UBX_UCENTER_STATUS_STOPPED 0
|
#define GPS_UBX_UCENTER_STATUS_STOPPED 0
|
||||||
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
|
#define GPS_UBX_UCENTER_STATUS_AUTOBAUD 1
|
||||||
#define GPS_UBX_UCENTER_STATUS_CONFIG 2
|
#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_NONE 0
|
||||||
#define GPS_UBX_UCENTER_REPLY_ACK 1
|
#define GPS_UBX_UCENTER_REPLY_ACK 1
|
||||||
@@ -101,8 +103,13 @@ void gps_ubx_ucenter_periodic(void)
|
|||||||
// Save processing time inflight
|
// Save processing time inflight
|
||||||
case GPS_UBX_UCENTER_STATUS_STOPPED:
|
case GPS_UBX_UCENTER_STATUS_STOPPED:
|
||||||
return;
|
return;
|
||||||
|
break;
|
||||||
// Automatically Determine Current Baudrate
|
// Automatically Determine Current Baudrate
|
||||||
case GPS_UBX_UCENTER_STATUS_AUTOBAUD:
|
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) {
|
if (gps_ubx_ucenter_autobaud(gps_ubx_ucenter.cnt) == FALSE) {
|
||||||
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
|
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG;
|
||||||
gps_ubx_ucenter.cnt = 0;
|
gps_ubx_ucenter.cnt = 0;
|
||||||
@@ -116,14 +123,27 @@ void gps_ubx_ucenter_periodic(void)
|
|||||||
} else {
|
} else {
|
||||||
gps_ubx_ucenter.cnt++;
|
gps_ubx_ucenter.cnt++;
|
||||||
}
|
}
|
||||||
|
#endif /* GPS_I2C */
|
||||||
break;
|
break;
|
||||||
// Send Configuration
|
// Send Configuration
|
||||||
case GPS_UBX_UCENTER_STATUS_CONFIG:
|
case GPS_UBX_UCENTER_STATUS_CONFIG:
|
||||||
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) {
|
if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) {
|
||||||
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
|
gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED;
|
||||||
|
#ifdef GPS_I2C
|
||||||
|
gps_i2c_begin();
|
||||||
|
#endif
|
||||||
gps_ubx_ucenter.cnt = 0;
|
gps_ubx_ucenter.cnt = 0;
|
||||||
} else {
|
} else {
|
||||||
gps_ubx_ucenter.cnt++;
|
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;
|
break;
|
||||||
default:
|
default:
|
||||||
@@ -147,10 +167,10 @@ void gps_ubx_ucenter_event(void)
|
|||||||
// Read Configuration Reply's
|
// Read Configuration Reply's
|
||||||
switch (gps_ubx.msg_class) {
|
switch (gps_ubx.msg_class) {
|
||||||
case UBX_ACK_ID:
|
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;
|
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
|
||||||
DEBUG_PRINT("ACK\n");
|
DEBUG_PRINT("ACK\n");
|
||||||
} else {
|
} else if (gps_ubx.msg_id & UBX_ACK_NAK_ID) {
|
||||||
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
|
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
|
||||||
DEBUG_PRINT("NACK\n");
|
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
|
* @param nr Autobaud step number to perform
|
||||||
* @return FALSE when completed
|
* @return FALSE when completed
|
||||||
*/
|
*/
|
||||||
|
#ifndef GPS_I2C
|
||||||
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
|
static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
|
||||||
{
|
{
|
||||||
switch (nr) {
|
switch (nr) {
|
||||||
@@ -300,7 +321,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr)
|
|||||||
}
|
}
|
||||||
return TRUE;
|
return TRUE;
|
||||||
}
|
}
|
||||||
|
#endif /* GPS_I2C */
|
||||||
/////////////////////////////////////
|
/////////////////////////////////////
|
||||||
// UBlox internal Navigation Solution
|
// UBlox internal Navigation Solution
|
||||||
|
|
||||||
@@ -393,7 +414,7 @@ static inline void gps_ubx_ucenter_config_port(void)
|
|||||||
// I2C Interface
|
// I2C Interface
|
||||||
case GPS_PORT_DDC:
|
case GPS_PORT_DDC:
|
||||||
#ifdef GPS_I2C
|
#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
|
#else
|
||||||
DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
|
DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n");
|
||||||
#endif
|
#endif
|
||||||
@@ -457,6 +478,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
|
|||||||
gps_ubx_ucenter_config_port();
|
gps_ubx_ucenter_config_port();
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
|
#ifndef GPS_I2C
|
||||||
#if PRINT_DEBUG_GPS_UBX_UCENTER
|
#if PRINT_DEBUG_GPS_UBX_UCENTER
|
||||||
if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) {
|
if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) {
|
||||||
DEBUG_PRINT("ublox did not acknowledge port configuration.\n");
|
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
|
// Now the GPS baudrate should have changed
|
||||||
uart_periph_set_baudrate(&(UBX_GPS_LINK), gps_ubx_ucenter.baud_target);
|
uart_periph_set_baudrate(&(UBX_GPS_LINK), gps_ubx_ucenter.baud_target);
|
||||||
gps_ubx_ucenter.baud_run = UART_SPEED(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);
|
UbxSend_MON_GET_VER(gps_ubx_ucenter.dev);
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
|
|||||||
@@ -33,6 +33,10 @@
|
|||||||
#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE"
|
#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef GPS_I2C
|
||||||
|
#include "modules/gps/gps_ubx_i2c.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "mcu_periph/uart.h"
|
#include "mcu_periph/uart.h"
|
||||||
|
|
||||||
#ifndef PRIMARY_GPS
|
#ifndef PRIMARY_GPS
|
||||||
|
|||||||
Reference in New Issue
Block a user