mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
OP-1477 - move to PIOS_I2C
This commit is contained in:
Alessio Morale
parent
36bb58755b
commit
11522ccbb0
@ -51,7 +51,7 @@ SystemStatsData systemStats;
|
||||
|
||||
#define DEBUG_THIS_FILE
|
||||
extern uint32_t pios_com_main_id;
|
||||
//#define PIOS_COM_DEBUG pios_com_main_id
|
||||
// #define PIOS_COM_DEBUG pios_com_main_id
|
||||
#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(DEBUG_THIS_FILE)
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, format,##__VA_ARGS__)
|
||||
#else
|
||||
@ -64,13 +64,14 @@ extern uint32_t pios_com_main_id;
|
||||
#define GPS_I2C_STREAM_SIZE_LOW_REG 0xFE
|
||||
|
||||
// Private constants
|
||||
#define SYSTEM_UPDATE_PERIOD_MS 10
|
||||
#define SYSTEM_UPDATE_PERIOD_MS 2
|
||||
|
||||
#define STACK_SIZE_BYTES 450
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
#define BUFFER_SIZE 128
|
||||
uint8_t buffer[BUFFER_SIZE];
|
||||
char outbuf[40];
|
||||
|
||||
// Private types
|
||||
|
||||
@ -80,13 +81,12 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
|
||||
static bool mallocFailed;
|
||||
static void ReadGPS();
|
||||
static void ReadMag();
|
||||
static void InitGPS();
|
||||
|
||||
// Private functions
|
||||
static void updateStats();
|
||||
static void gps_lowlevel_init();
|
||||
|
||||
static void gpspSystemTask(void *parameters);
|
||||
|
||||
static uint32_t I2C_ReadBuffer(I2C_TypeDef* I2Cx,uint8_t* pBuffer, uint8_t reg, uint16_t* NumByteToRead, uint8_t deviceAddress);
|
||||
|
||||
/**
|
||||
* Create the module task.
|
||||
@ -109,7 +109,6 @@ int32_t GPSPSystemModStart(void)
|
||||
*/
|
||||
int32_t GPSPSystemModInitialize(void)
|
||||
{
|
||||
|
||||
GPSPSystemModStart();
|
||||
|
||||
return 0;
|
||||
@ -123,9 +122,6 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
/* create all modules thread */
|
||||
MODULE_TASKCREATE_ALL;
|
||||
InitGPS();
|
||||
/* start the delayed callback scheduler */
|
||||
// PIOS_CALLBACKSCHEDULER_Start();
|
||||
|
||||
if (mallocFailed) {
|
||||
/* We failed to malloc during task creation,
|
||||
@ -144,8 +140,6 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
|
||||
// Update the system statistics
|
||||
updateStats();
|
||||
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
|
||||
// Update the system alarms
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, (uint8_t *)&"0000", 4);
|
||||
vTaskDelay(SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
|
||||
ReadGPS();
|
||||
ReadMag();
|
||||
@ -158,7 +152,7 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
|
||||
*/
|
||||
static uint16_t GetFreeIrqStackSize(void)
|
||||
{
|
||||
uint32_t i = 0x200;
|
||||
uint32_t i = 0x150;
|
||||
|
||||
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
|
||||
extern uint32_t _irq_stack_top;
|
||||
@ -194,14 +188,18 @@ static uint16_t GetFreeIrqStackSize(void)
|
||||
static void updateStats()
|
||||
{
|
||||
// Get stats and update
|
||||
systemStats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
systemStats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
systemStats.HeapRemaining = xPortGetFreeHeapSize();
|
||||
systemStats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4;
|
||||
|
||||
// Get Irq stack status
|
||||
systemStats.IRQStackRemaining = GetFreeIrqStackSize();
|
||||
|
||||
systemStats.CPULoad = 100 - PIOS_TASK_MONITOR_GetIdlePercentage();
|
||||
uint8_t len = snprintf(outbuf, 40, "$POPSY,%u,%d,%d,%d*0\n",
|
||||
(uint)systemStats.FlightTime,
|
||||
systemStats.HeapRemaining,
|
||||
systemStats.IRQStackRemaining,
|
||||
systemStats.SystemModStackRemaining);
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, (uint8_t *)outbuf, len);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -247,170 +245,54 @@ void vApplicationMallocFailedHook(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
union {
|
||||
uint8_t outbuff[11];
|
||||
struct {
|
||||
uint8_t pre;
|
||||
uint8_t header[3];
|
||||
int16_t mag[3];
|
||||
uint8_t tail;
|
||||
} dataMag;
|
||||
struct {
|
||||
uint8_t pre;
|
||||
uint8_t header[3];
|
||||
uint8_t data[4];
|
||||
uint8_t tail;
|
||||
} dataId;
|
||||
} magbuf;
|
||||
|
||||
int16_t mag[3];
|
||||
uint8_t data[4];
|
||||
|
||||
void ReadMag()
|
||||
{
|
||||
PIOS_HMC5x83_ReadID(magbuf.dataId.data);
|
||||
magbuf.dataId.data[3] = '!';
|
||||
magbuf.dataId.pre = '\n';
|
||||
magbuf.dataId.header[0] = 'M';
|
||||
magbuf.dataId.header[1] = 'a';
|
||||
magbuf.dataId.header[2] = 'g';
|
||||
magbuf.dataId.tail = '\n';
|
||||
PIOS_HMC5x83_ReadID(data);
|
||||
|
||||
|
||||
if (PIOS_HMC5x83_ReadMag(magbuf.dataMag.mag) < 0) {
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, magbuf.outbuff, 9);
|
||||
if (PIOS_HMC5x83_ReadMag(mag) == 0) {
|
||||
uint8_t len = snprintf(outbuf, 40, "$POPMG,%s,%d,%d,%d,*0\n", data, mag[0], mag[1], mag[2]);
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, (uint8_t *)outbuf, len);
|
||||
return;
|
||||
}
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, magbuf.outbuff, 11);
|
||||
}
|
||||
|
||||
void ReadGPS(){
|
||||
uint16_t bytesToRead = 2;
|
||||
uint16_t bytesToSend = 0;
|
||||
void ReadGPS()
|
||||
{
|
||||
uint16_t bytesToRead = 2;
|
||||
uint8_t tmp[2];
|
||||
if(I2C_ReadBuffer(I2C1,tmp, GPS_I2C_STREAM_SIZE_HIGH_REG,&bytesToRead, GPS_I2C_ADDRESS) == 0){
|
||||
bytesToRead = (tmp[0] << 8) | tmp[1];
|
||||
bytesToRead = bytesToRead < 255 ? bytesToRead : 254;
|
||||
bytesToSend = bytesToRead;
|
||||
if(bytesToRead > 0 && I2C_ReadBuffer(I2C1,buffer, GPS_I2C_STREAM_REG,&bytesToRead, GPS_I2C_ADDRESS) == 0){
|
||||
PIOS_COM_SendBuffer(pios_com_main_id,buffer, bytesToSend - bytesToRead);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void InitGPS(){
|
||||
gps_lowlevel_init();
|
||||
I2C_InitTypeDef I2C_InitStructure;
|
||||
memset(buffer,0 ,32);
|
||||
/* I2C configuration */
|
||||
/* sEE_I2C configuration */
|
||||
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
|
||||
I2C_InitStructure.I2C_DigitalFilter = 0x00;
|
||||
I2C_InitStructure.I2C_OwnAddress1 = 0x00;
|
||||
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
||||
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||
I2C_InitStructure.I2C_Timing = 0x00210507;
|
||||
|
||||
/* Apply sEE_I2C configuration after enabling it */
|
||||
I2C_Init(I2C1, &I2C_InitStructure);
|
||||
|
||||
/* sEE_I2C Peripheral Enable */
|
||||
I2C_Cmd(I2C1, ENABLE);
|
||||
}
|
||||
|
||||
void gps_lowlevel_init(void)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
|
||||
/* Configure the I2C clock source. The clock is derived from the HSI */
|
||||
RCC_I2CCLKConfig(RCC_I2C1CLK_HSI);
|
||||
|
||||
/* sEE_I2C_SCL_GPIO_CLK and sEE_I2C_SDA_GPIO_CLK Periph clock enable */
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
|
||||
|
||||
/* sEE_I2C Periph clock enable */
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
|
||||
|
||||
/* Connect PXx to I2C_SCL*/
|
||||
GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_1);
|
||||
|
||||
/* Connect PXx to I2C_SDA*/
|
||||
GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_1);
|
||||
|
||||
/* GPIO configuration */
|
||||
/* Configure sEE_I2C pins: SCL */
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
/* Configure sEE_I2C pins: SDA */
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
}
|
||||
|
||||
#define sEE_FLAG_TIMEOUT ((uint32_t)0x1000)
|
||||
#define sEE_LONG_TIMEOUT ((uint32_t)(10 * sEE_FLAG_TIMEOUT))
|
||||
|
||||
uint32_t I2C_ReadBuffer(I2C_TypeDef* I2Cx,uint8_t* pBuffer, uint8_t reg, uint16_t* NumByteToRead, uint8_t deviceAddress)
|
||||
{
|
||||
static volatile uint32_t sEETimeout = sEE_LONG_TIMEOUT;
|
||||
uint32_t NumbOfSingle = 0, DataNum = 0;
|
||||
|
||||
/* Get number of reload cycles */
|
||||
NumbOfSingle = (*NumByteToRead) % 255;
|
||||
|
||||
|
||||
|
||||
/* Configure slave address, nbytes, reload and generate start */
|
||||
I2C_TransferHandling(I2Cx, deviceAddress, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write);
|
||||
|
||||
/* Wait until TXIS flag is set */
|
||||
sEETimeout = sEE_LONG_TIMEOUT;
|
||||
while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET)
|
||||
{
|
||||
if((sEETimeout--) == 0)
|
||||
return -2;
|
||||
}
|
||||
|
||||
/* Send memory address */
|
||||
I2C_SendData(I2Cx, (uint8_t)reg);
|
||||
|
||||
/* Wait until TC flag is set */
|
||||
sEETimeout = sEE_LONG_TIMEOUT;
|
||||
while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET)
|
||||
{
|
||||
if((sEETimeout--) == 0)
|
||||
return -2;
|
||||
}
|
||||
|
||||
/* Update CR2 : set Slave Address , set read request, generate Start and set end mode */
|
||||
I2C_TransferHandling(I2Cx, deviceAddress, (uint32_t)(NumbOfSingle), I2C_AutoEnd_Mode, I2C_Generate_Start_Read);
|
||||
|
||||
/* Reset local variable */
|
||||
DataNum = 0;
|
||||
|
||||
/* Wait until all data are received */
|
||||
while (DataNum != NumbOfSingle)
|
||||
{
|
||||
/* Wait until RXNE flag is set */
|
||||
sEETimeout = sEE_LONG_TIMEOUT;
|
||||
while(I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET)
|
||||
uint8_t addr_buffer[1] = { GPS_I2C_STREAM_SIZE_HIGH_REG };
|
||||
struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
if((sEETimeout--) == 0)
|
||||
return -2;
|
||||
.info = __func__,
|
||||
.addr = GPS_I2C_ADDRESS,
|
||||
.rw = PIOS_I2C_TXN_WRITE,
|
||||
.len = 1,
|
||||
.buf = addr_buffer,
|
||||
}
|
||||
,
|
||||
{
|
||||
.info = __func__,
|
||||
.addr = GPS_I2C_ADDRESS,
|
||||
.rw = PIOS_I2C_TXN_READ,
|
||||
.len = 2,
|
||||
.buf = tmp,
|
||||
}
|
||||
};
|
||||
|
||||
/* Read data from RXDR */
|
||||
pBuffer[DataNum]= I2C_ReceiveData(I2Cx);
|
||||
|
||||
/* Update number of received data */
|
||||
DataNum++;
|
||||
(*NumByteToRead)--;
|
||||
if (PIOS_I2C_Transfer(PIOS_I2C_GPS, txn_list, NELEMENTS(txn_list)) == 0) {
|
||||
bytesToRead = (tmp[0] << 8) | tmp[1];
|
||||
bytesToRead = bytesToRead < 255 ? bytesToRead : 254;
|
||||
addr_buffer[0] = GPS_I2C_STREAM_REG;
|
||||
txn_list[1].len = bytesToRead > BUFFER_SIZE ? BUFFER_SIZE : bytesToRead;
|
||||
txn_list[1].buf = buffer;
|
||||
if (bytesToRead > 0 && PIOS_I2C_Transfer(PIOS_I2C_GPS, txn_list, NELEMENTS(txn_list)) == 0) {
|
||||
PIOS_COM_SendBuffer(pios_com_main_id, buffer, bytesToRead);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Loading…
Reference in New Issue
Block a user