rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-378: INS code starts taking in sensors (timing not optimized at all yet)

Browse Source
This commit is contained in:
James Cotton 2011-05-26 14:06:22 -05:00
parent 9ede84680e
commit eab8896f16
5 changed files with 80 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/INS/Makefile
View File

@ -86,7 +86,7 @@ OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# use file-extension c for "c-only"-files
## INS:
SRC = test.c
SRC = ins.c
SRC += pios_board.c
SRC += ahrs_timer.c
SRC += insgps13state.c

1
flight/INS/inc/pios_config.h
View File

@ -47,6 +47,7 @@
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_COM_AUX
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_BMP085
#define PIOS_INCLUDE_IMU3000

101
flight/INS/ins.c
View File

@ -33,6 +33,13 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/*
TODO:
BMP085 - Pressure
IMU3000 interrupt
BMA180 interrupt
*/
/* OpenPilot Includes */
#include "ins.h"
#include "pios.h"
@ -413,16 +420,29 @@ void print_ekf_binary() {}
extern void PIOS_Board_Init(void);
static void panic()
static void panic(int blinks)
{
int blinked = 0;
while(1) {
PIOS_LED_Toggle(LED2);
PIOS_DELAY_WaitmS(100);
blinked++;
if(blinked > blinks) {
blinked = 0;
PIOS_DELAY_WaitmS(1000);
}
PIOS_LED_On(LED2);
PIOS_DELAY_WaitmS(200);
PIOS_LED_Off(LED2);
PIOS_DELAY_WaitmS(200);
}
}
/**
* @brief INS Main function
*/
int16_t accel_data_glob[3];
int16_t gyro_data_glob[3];
int16_t mag_data_glob[3];
int main()
{
gps_data.quality = -1;
@ -450,26 +470,39 @@ int main()
#endif
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
// Sensors need a second to start
PIOS_DELAY_WaitmS(100);
// Sensor test
if(PIOS_IMU3000_Test() == 0)
panic();
if(PIOS_IMU3000_Test() != 0)
panic(1);
if(PIOS_BMA180_Test() != 0)
panic(2);
/*
if(PIOS_BMA180_Test() == 0)
panic();
if(PIOS_HMC5883_Test() == 0)
panic();
if(PIOS_HMC5883_Test() != 0)
panic(3);
if(PIOS_BMP085_Test() != 0)
panic(4);
if(PIOS_BMP085_Test() == 0)
panic(); */
//while(!AhrsLinkReady()) {
// AhrsPoll();
//}
// Flash warning light while trying to connect
uint16_t time_val = PIOS_DELAY_GetuS();
uint16_t ms_count = 0;
extern int32_t PIOS_DELAY_DiffuS(uint16_t ref);
while(!AhrsLinkReady()) {
AhrsPoll();
if(PIOS_DELAY_DiffuS(time_val) > 10000) {
ms_count += 10;
time_val = PIOS_DELAY_GetuS();
}
if(ms_count > 100) {
PIOS_LED_Toggle(LED2);
ms_count = 0;
}
}
PIOS_LED_Off(LED2);
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
@ -565,24 +598,24 @@ int main()
*/
bool get_accel_gyro_data()
{
struct pios_bma180_data accel;
struct pios_imu3000_data gyro;
int16_t accel[3];
int16_t gyro[3];
PIOS_BMA180_Read(&accel);
PIOS_IMU3000_ReadGyros(&gyro);
PIOS_BMA180_ReadAccels(accel);
PIOS_IMU3000_ReadGyros(gyro);
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.raw.x = accel[0];
accel_data.raw.y = accel[1];
accel_data.raw.z = accel[2];
gyro_data.raw.x = gyro[0];
gyro_data.raw.y = gyro[1];
gyro_data.raw.z = gyro[2];
accel_data.filtered.x = accel[0];
accel_data.filtered.y = accel[1];
accel_data.filtered.z = accel[2];
gyro_data.filtered.x = gyro[0];
gyro_data.filtered.y = gyro[1];
gyro_data.filtered.z = gyro[2];
return true;
}

2
flight/INS/pios_board.c
View File

@ -36,7 +36,7 @@
#include <pios_spi_priv.h>
/* SPI2 Interface
* - Used for mainboard communications and magnetometer
* - Used for mainboard communications
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).

20
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -7791,17 +7791,17 @@
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 */,
65988604138467D4006777AA /* pios_bl_helper.h */,
65988603138467D4006777AA /* pios_bma180.h */,
65988609138467D4006777AA /* pios_hcsr04.h */,
65988602138467D4006777AA /* pios_hmc5883.h */,
65988608138467D4006777AA /* pios_i2c_esc.h */,
65988607138467D4006777AA /* pios_iap.h */,
65988600138467D4006777AA /* pios_imu3000.h */,
65988606138467D4006777AA /* pios_initcall.h */,
65988601138467D4006777AA /* pios_ppm_priv.h */,
65988605138467D4006777AA /* pios_spektrum_priv.h */,
6512D60512ED4CA2008175E5 /* pios_flash_w25x.h */,
6526645B122DF972006F9A3C /* pios_wdg.h */,
651CF9EF120B700D00EEFD70 /* pios_usb_hid_desc.h */,