1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00

OP-378: Small changes to get drivers working again. Fix I2C port for IMU3000

and change convention so that 0 is success, < 0 is error.
This commit is contained in:
James Cotton 2011-05-18 19:38:08 -05:00
parent ab3127a0ea
commit 750097ea73
9 changed files with 135 additions and 125 deletions

View File

@ -204,7 +204,7 @@ LINKERSCRIPTPATH = $(PIOSSTM32F10X)
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
ifeq ($(DEBUG),YES)
CFLAGS += -Os
CFLAGS += -O0
CFLAGS += -DGENERAL_COV
else
CFLAGS += -Os

View File

@ -413,11 +413,18 @@ void print_ekf_binary() {}
extern void PIOS_Board_Init(void);
static void panic()
{
while(1) {
PIOS_LED_Toggle(LED2);
PIOS_DELAY_WaitmS(100);
}
}
/**
* @brief INS Main function
*/
int main()
{
{
gps_data.quality = -1;
uint32_t up_time_real = 0;
uint32_t up_time = 0;
@ -433,16 +440,36 @@ int main()
reset_values();
PIOS_Board_Init();
PIOS_LED_Off(LED1);
PIOS_LED_On(LED2);
#if defined(PIOS_INCLUDE_HMC5883) && defined(PIOS_INCLUDE_I2C)
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5883_ReadID(mag_data.id);
#endif
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
while(!AhrsLinkReady()) {
AhrsPoll();
}
// Sensor test
if(PIOS_IMU3000_Test() == 0)
panic();
/*
if(PIOS_BMA180_Test() == 0)
panic();
if(PIOS_HMC5883_Test() == 0)
panic();
if(PIOS_BMP085_Test() == 0)
panic(); */
//while(!AhrsLinkReady()) {
// AhrsPoll();
//}
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
@ -538,41 +565,25 @@ int main()
*/
bool get_accel_gyro_data()
{
// struct pios_bma180_data accel;
// struct pios_imu3000_data gyro;
struct pios_bma180_data accel;
struct pios_imu3000_data gyro;
//PIOS_BMA180_Read(&accel);
//PIOS_IMU3000_ReadGyros(&gyro);
PIOS_BMA180_Read(&accel);
PIOS_IMU3000_ReadGyros(&gyro);
accel_data.raw.x=0;
accel_data.raw.y=0;
accel_data.raw.z=-9;
gyro_data.raw.x=0;
gyro_data.raw.y=0;
gyro_data.raw.z=0;
accel_data.filtered.x = 0;
accel_data.filtered.y = 0;
accel_data.filtered.z = -9.8;
gyro_data.filtered.x = 0;
gyro_data.filtered.y = 0;
gyro_data.filtered.z = 0;
/*
PIOS_BMA180_Read(&accel[1]);
accel_data.raw.x=accel[1].x;
accel_data.raw.y=accel[1].y;
accel_data.raw.z=accel[1].z;
PIOS_IMU3000_Read(&gyro[1]);
gyro_data.raw.x=gyro[1].x;
gyro_data.raw.y=gyro[1].y;
gyro_data.raw.z=gyro[1].z;
accel_data.raw.x=accel.x;
accel_data.raw.y=accel.y;
accel_data.raw.z=accel.z;
gyro_data.raw.x=gyro.x;
gyro_data.raw.y=gyro.y;
gyro_data.raw.z=gyro.z;
accel_data.filtered.x = accel.x;
accel_data.filtered.y = accel.y;
accel_data.filtered.z = accel.z;
gyro_data.filtered.x = gyro.x;
gyro_data.filtered.y = gyro.y;
gyro_data.filtered.z = gyro.z;
accel_data.filtered.x = ((float)(accel[0].x + accel[1].x)) / 2;
accel_data.filtered.y = ((float)(accel[0].y + accel[1].y)) / 2;
accel_data.filtered.z = ((float)(accel[0].z + accel[1].z)) / 2;
gyro_data.filtered.x = ((float)(gyro[0].x + gyro[1].x)) / 2;
gyro_data.filtered.y = ((float)(gyro[0].y + gyro[1].y)) / 2;
gyro_data.filtered.z = ((float)(gyro[0].z + gyro[1].z)) / 2;
*/
return true;
}

View File

@ -94,7 +94,7 @@ void blink(int led, int times)
void test_accel()
{
if(PIOS_BMA180_Test())
if(PIOS_BMA180_Test() == 0)
blink(LED1, 1);
else
blink(LED2, 1);
@ -103,7 +103,7 @@ void test_accel()
#if defined (PIOS_INCLUDE_HMC5883)
void test_mag()
{
if(PIOS_HMC5883_Test())
if(PIOS_HMC5883_Test() == 0)
blink(LED1, 2);
else
blink(LED2, 2);
@ -113,7 +113,7 @@ void test_mag()
#if defined (PIOS_INCLUDE_BMP085)
void test_pressure()
{
if(PIOS_BMP085_Test())
if(PIOS_BMP085_Test() == 0)
blink(LED1, 3);
else
blink(LED2, 3);
@ -123,7 +123,7 @@ void test_pressure()
#if defined (PIOS_INCLUDE_IMU3000)
void test_imu()
{
if(PIOS_IMU3000_Test())
if(PIOS_IMU3000_Test() == 0)
blink(LED1, 4);
else
blink(LED2, 4);
@ -168,51 +168,6 @@ int main()
}
/**
* @brief Populate fields with initial values
*/
void reset_values()
{
accel_data.calibration.scale[0][1] = 0;
accel_data.calibration.scale[1][0] = 0;
accel_data.calibration.scale[0][2] = 0;
accel_data.calibration.scale[2][0] = 0;
accel_data.calibration.scale[1][2] = 0;
accel_data.calibration.scale[2][1] = 0;
accel_data.calibration.scale[0][0] = 0.0359;
accel_data.calibration.scale[1][1] = 0.0359;
accel_data.calibration.scale[2][2] = 0.0359;
accel_data.calibration.scale[0][3] = -73.5;
accel_data.calibration.scale[1][3] = -73.5;
accel_data.calibration.scale[2][3] = -73.5;
accel_data.calibration.variance[0] = 1e-4;
accel_data.calibration.variance[1] = 1e-4;
accel_data.calibration.variance[2] = 1e-4;
gyro_data.calibration.scale[0] = -0.014;
gyro_data.calibration.scale[1] = 0.014;
gyro_data.calibration.scale[2] = -0.014;
gyro_data.calibration.bias[0] = -24;
gyro_data.calibration.bias[1] = -24;
gyro_data.calibration.bias[2] = -24;
gyro_data.calibration.variance[0] = 1;
gyro_data.calibration.variance[1] = 1;
gyro_data.calibration.variance[2] = 1;
mag_data.calibration.scale[0] = 1;
mag_data.calibration.scale[1] = 1;
mag_data.calibration.scale[2] = 1;
mag_data.calibration.bias[0] = 0;
mag_data.calibration.bias[1] = 0;
mag_data.calibration.bias[2] = 0;
mag_data.calibration.variance[0] = 50;
mag_data.calibration.variance[1] = 50;
mag_data.calibration.variance[2] = 50;
}
/**
* @}
*/

View File

@ -204,6 +204,9 @@ extern uint32_t pios_com_aux_id;
#define PIOS_GPIO_CLKS { PIOS_GPIO_1_GPIO_CLK }
#define PIOS_GPIO_NUM 1
//------------------------
// BMA180
//------------------------
#define PIOS_BMA_ENABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,0)
#define PIOS_BMA_DISABLE PIOS_SPI_RC_PinSet(PIOS_SPI_ACCEL,1)

View File

@ -187,23 +187,26 @@ uint8_t PIOS_BMA180_Read(struct pios_bma180_data * data)
/**
* @brief Test SPI and chip functionality by reading chip ID register
* @return 0 if test failed, any other value signals test succeeded.
* @return 0 if success, -1 if failure.
*
*/
uint8_t PIOS_BMA180_Test()
{
uint8_t data = 0;
uint8_t pass = 0;
PIOS_BMA180_ClaimBus();
data = PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,(0x80 | BMA_CHIPID_ADDR) );
data &= 0x07;
if(0x03 == data)
pass = 1;
data = PIOS_SPI_TransferByte(PIOS_SPI_ACCEL,(0x80 | BMA_VERSION_ADDR) );
if(0x12 == data)
pass = 1 && pass; // Only passes if first and second test passS
// Read chip ID then version ID
uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0};
uint8_t rec[3] = {0,0, 0};
PIOS_BMA180_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
PIOS_BMA180_ReleaseBus();
return pass;
if((rec[1] & 0x07) != 0x3)
return -1;
if(rec[2] != 0x12)
return -1;
return 0;
}
/**

View File

@ -322,18 +322,15 @@ static bool PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if test failed
* \return non-zero value if test succeeded
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5883_Test(void)
{
/* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5883_ReadID((uint8_t *)id);
if(!strncmp("H43\0",id,4))
return 0;
else
return 1;
if(strncmp("H43\0",id,4) != 0) // match H43
return -1;
int32_t passed = 1;
uint8_t registers[3] = {0,0,0};

View File

@ -50,8 +50,8 @@ typedef struct {
/* Local Variables */
static void PIOS_IMU3000_Config(PIOS_IMU3000_ConfigTypeDef * IMU3000_Config_Struct);
static bool PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len);
static bool PIOS_IMU3000_Write(uint8_t address, uint8_t buffer);
static int32_t PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len);
static int32_t PIOS_IMU3000_Write(uint8_t address, uint8_t buffer);
/**
* @brief Initialize the IMU3000 3-axis gyro sensor.
@ -109,22 +109,22 @@ static void PIOS_IMU3000_Config(PIOS_IMU3000_ConfigTypeDef * IMU3000_Config_Stru
// TODO: Add checks against current config so we only update what has changed
// FIFO storage
while (!PIOS_IMU3000_Write(PIOS_IMU3000_FIFO_EN_REG, IMU3000_Config_Struct->Fifo_store)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_FIFO_EN_REG, IMU3000_Config_Struct->Fifo_store) != 0);
// Sample rate divider
while (!PIOS_IMU3000_Write(PIOS_IMU3000_SMPLRT_DIV_REG, IMU3000_Config_Struct->Smpl_rate_div)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_SMPLRT_DIV_REG, IMU3000_Config_Struct->Smpl_rate_div) != 0) ;
// Digital low-pass filter and scale
while (!PIOS_IMU3000_Write(PIOS_IMU3000_DLPF_CFG_REG, IMU3000_Config_Struct->DigLPF_Scale)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_DLPF_CFG_REG, IMU3000_Config_Struct->DigLPF_Scale) != 0) ;
// Interrupt configuration
while (!PIOS_IMU3000_Write(PIOS_IMU3000_INT_CFG_REG, IMU3000_Config_Struct->Interrupt_cfg)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_INT_CFG_REG, IMU3000_Config_Struct->Interrupt_cfg) != 0) ;
// Interrupt configuration
while (!PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, IMU3000_Config_Struct->User_ctl)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, IMU3000_Config_Struct->User_ctl) != 0) ;
// Interrupt configuration
while (!PIOS_IMU3000_Write(PIOS_IMU3000_PWR_MGMT_REG, IMU3000_Config_Struct->Pwr_mgmt_clk)) ;
while (PIOS_IMU3000_Write(PIOS_IMU3000_PWR_MGMT_REG, IMU3000_Config_Struct->Pwr_mgmt_clk) != 0) ;
}
/**
@ -140,12 +140,13 @@ uint8_t PIOS_IMU3000_ReadGyros(struct pios_imu3000_data * data)
/**
* @brief Read the identification bytes from the IMU3000 sensor
* \return ID read from IMU3000
* \return ID read from IMU3000 or -1 if failure
*/
uint8_t PIOS_IMU3000_ReadID()
int32_t PIOS_IMU3000_ReadID()
{
uint8_t id;
while (!PIOS_IMU3000_Read(0x00, &id, 1)) ;
if(PIOS_IMU3000_Read(0x00, &id, 1) != 0)
return -1;
return id;
}
@ -158,7 +159,7 @@ uint8_t PIOS_IMU3000_ReadID()
* \return -1 if error during I2C transfer
* \return -4 if invalid length
*/
static bool PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len)
static int32_t PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len)
{
uint8_t addr_buffer[] = {
address,
@ -182,7 +183,7 @@ static bool PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len)
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list)) ? 0 : -1;
}
/**
@ -192,7 +193,7 @@ static bool PIOS_IMU3000_Read(uint8_t address, uint8_t * buffer, uint8_t len)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
static bool PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
static int32_t PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
{
uint8_t data[] = {
address,
@ -210,7 +211,7 @@ static bool PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
,
};
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
return PIOS_I2C_Transfer(PIOS_I2C_GYRO_ADAPTER, txn_list, NELEMENTS(txn_list)) ? 0 : -1;
}
/**
@ -221,7 +222,15 @@ static bool PIOS_IMU3000_Write(uint8_t address, uint8_t buffer)
uint8_t PIOS_IMU3000_Test(void)
{
/* Verify that ID matches (IMU3000 ID is 0x69) */
return ( !(0x69 ^ PIOS_IMU3000_ReadID()) );
int32_t id = 0;
id = PIOS_IMU3000_ReadID();
if(id < 0)
return -1;
if(id != PIOS_IMU3000_I2C_ADDR)
return -2;
return 0;
}
/**

View File

@ -120,7 +120,7 @@ struct pios_imu3000_data {
extern void PIOS_IMU3000_Init(void);
extern bool PIOS_IMU3000_NewDataAvailable(void);
extern uint8_t PIOS_IMU3000_ReadGyros(struct pios_imu3000_data * data);
extern uint8_t PIOS_IMU3000_ReadID();
extern int32_t PIOS_IMU3000_ReadID();
extern uint8_t PIOS_IMU3000_Test();
#endif /* PIOS_IMU3000_H */

View File

@ -143,6 +143,22 @@
659885AD138450D2006777AA /* link_STM3210E_INS_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = link_STM3210E_INS_sections.ld; sourceTree = "<group>"; };
659885AE138450D2006777AA /* link_STM3210E_INS_BL_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = link_STM3210E_INS_BL_sections.ld; sourceTree = "<group>"; };
659885AF138450D2006777AA /* link_STM3210E_INS_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = link_STM3210E_INS_memory.ld; sourceTree = "<group>"; };
659885CF13845855006777AA /* pios_imu3000.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_imu3000.c; sourceTree = "<group>"; };
659885D013845855006777AA /* pios_hmc5883.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_hmc5883.c; sourceTree = "<group>"; };
659885D113845855006777AA /* pios_bma180.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bma180.c; sourceTree = "<group>"; };
659885D213845855006777AA /* pios_bl_helper.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_bl_helper.c; sourceTree = "<group>"; };
659885D313845855006777AA /* pios_i2c_esc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_i2c_esc.c; sourceTree = "<group>"; };
659885D413845855006777AA /* pios_hcsr04.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_hcsr04.c; sourceTree = "<group>"; };
65988600138467D4006777AA /* pios_imu3000.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_imu3000.h; sourceTree = "<group>"; };
65988601138467D4006777AA /* pios_ppm_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_ppm_priv.h; sourceTree = "<group>"; };
65988602138467D4006777AA /* pios_hmc5883.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_hmc5883.h; sourceTree = "<group>"; };
65988603138467D4006777AA /* pios_bma180.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_bma180.h; sourceTree = "<group>"; };
65988604138467D4006777AA /* pios_bl_helper.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_bl_helper.h; sourceTree = "<group>"; };
65988605138467D4006777AA /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = "<group>"; };
65988606138467D4006777AA /* pios_initcall.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_initcall.h; sourceTree = "<group>"; };
65988607138467D4006777AA /* pios_iap.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_iap.h; sourceTree = "<group>"; };
65988608138467D4006777AA /* pios_i2c_esc.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_i2c_esc.h; sourceTree = "<group>"; };
65988609138467D4006777AA /* pios_hcsr04.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_hcsr04.h; sourceTree = "<group>"; };
659ED317122226B60011010E /* ahrssettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrssettings.xml; sourceTree = "<group>"; };
65B35D7F121C261E003EAD18 /* bin.pro */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = bin.pro; sourceTree = "<group>"; };
65B35D80121C261E003EAD18 /* openpilotgcs */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.sh; path = openpilotgcs; sourceTree = "<group>"; };
@ -7750,17 +7766,23 @@
65E8F03011EFF25C00BBF654 /* Common */ = {
isa = PBXGroup;
children = (
654612D812B5E9A900B719D0 /* pios_iap.c */,
6528CCB412E406B800CF5144 /* pios_adxl345.c */,
659885D213845855006777AA /* pios_bl_helper.c */,
659885D113845855006777AA /* pios_bma180.c */,
65E8F03111EFF25C00BBF654 /* pios_bmp085.c */,
65E8F03211EFF25C00BBF654 /* pios_com.c */,
6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */,
65FF4D5E137EDEC100146BE4 /* pios_flashfs_objlist.c */,
65E8F03311EFF25C00BBF654 /* pios_hmc5843.c */,
659885D013845855006777AA /* pios_hmc5883.c */,
659885D413845855006777AA /* pios_hcsr04.c */,
659885CF13845855006777AA /* pios_imu3000.c */,
659885D313845855006777AA /* pios_i2c_esc.c */,
654612D812B5E9A900B719D0 /* pios_iap.c */,
65E8F03411EFF25C00BBF654 /* pios_opahrs.c */,
65E8F03511EFF25C00BBF654 /* pios_opahrs_proto.c */,
65E8F03611EFF25C00BBF654 /* pios_sdcard.c */,
65E8F03711EFF25C00BBF654 /* printf-stdarg.c */,
6528CCB412E406B800CF5144 /* pios_adxl345.c */,
6512D60712ED4CB8008175E5 /* pios_flash_w25x.c */,
65FF4D5E137EDEC100146BE4 /* pios_flashfs_objlist.c */,
);
name = Common;
path = ../../PiOS/Common;
@ -7769,6 +7791,16 @@
65E8F03811EFF25C00BBF654 /* inc */ = {
isa = PBXGroup;
children = (
65988600138467D4006777AA /* pios_imu3000.h */,
65988601138467D4006777AA /* pios_ppm_priv.h */,
65988602138467D4006777AA /* pios_hmc5883.h */,
65988603138467D4006777AA /* pios_bma180.h */,
65988604138467D4006777AA /* pios_bl_helper.h */,
65988605138467D4006777AA /* pios_spektrum_priv.h */,
65988606138467D4006777AA /* pios_initcall.h */,
65988607138467D4006777AA /* pios_iap.h */,
65988608138467D4006777AA /* pios_i2c_esc.h */,
65988609138467D4006777AA /* pios_hcsr04.h */,
6528CCE212E40F6700CF5144 /* pios_adxl345.h */,
6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */,
6526645B122DF972006F9A3C /* pios_wdg.h */,