From eab8896f168acbeeebcec9c636a323a8620518c2 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 26 May 2011 14:06:22 -0500 Subject: [PATCH] OP-378: INS code starts taking in sensors (timing not optimized at all yet) --- flight/INS/Makefile | 2 +- flight/INS/inc/pios_config.h | 1 + flight/INS/ins.c | 101 ++++++++++++------ flight/INS/pios_board.c | 2 +- .../OpenPilotOSX.xcodeproj/project.pbxproj | 20 ++-- 5 files changed, 80 insertions(+), 46 deletions(-) diff --git a/flight/INS/Makefile b/flight/INS/Makefile index 17ce68cb1..9dfb4f282 100644 --- a/flight/INS/Makefile +++ b/flight/INS/Makefile @@ -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 diff --git a/flight/INS/inc/pios_config.h b/flight/INS/inc/pios_config.h index ebe29398b..2ce0d71db 100644 --- a/flight/INS/inc/pios_config.h +++ b/flight/INS/inc/pios_config.h @@ -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 diff --git a/flight/INS/ins.c b/flight/INS/ins.c index 33678b026..c6871aecc 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -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; } diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c index 3ac3ca0fb..49d8c0f1b 100644 --- a/flight/INS/pios_board.c +++ b/flight/INS/pios_board.c @@ -36,7 +36,7 @@ #include /* 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). diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 8c4b5c069..85aea5cbe 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -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 */,