From b2f12c4854867a4fbe3ac7f8549c3fb0ec808f00 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Fri, 14 Sep 2012 21:05:20 +0900 Subject: [PATCH] ArduCopter: optical flow initialisation changes now that optical flow library automatically works on APM1, APM2 and APM2.5 --- ArduCopter/ArduCopter.pde | 62 ++++++++++++++------------------------- ArduCopter/sensors.pde | 10 +++++-- ArduCopter/system.pde | 2 ++ ArduCopter/test.pde | 2 +- 4 files changed, 32 insertions(+), 44 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 980946d455..7001007799 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -67,6 +67,7 @@ #include // ArduPilot GPS library #include // Arduino I2C lib #include // Arduino SPI lib +#include // SPI3 library #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include @@ -223,11 +224,7 @@ AP_Compass_HMC5843 compass; #endif #ifdef OPTFLOW_ENABLED - #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -AP_OpticalFlow_ADNS3080_APM2 optflow(OPTFLOW_CS_PIN); - #else AP_OpticalFlow_ADNS3080 optflow(OPTFLOW_CS_PIN); - #endif #else AP_OpticalFlow optflow; #endif @@ -1085,6 +1082,14 @@ static void fast_loop() calc_inertia(); #endif + // optical flow + // -------------------- +#ifdef OPTFLOW_ENABLED + if(g.optflow_enabled) { + update_optical_flow(); + } +#endif + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1258,14 +1263,6 @@ static void fifty_hz_loop() edf_toy(); #endif - // syncronise optical flow reads with altitude reads -#ifdef OPTFLOW_ENABLED - if(g.optflow_enabled) { - update_optical_flow(); - } -#endif - - #ifdef USERHOOK_50HZLOOP USERHOOK_50HZLOOP #endif @@ -1438,42 +1435,27 @@ static void super_slow_loop() */ } -// updated at 10 Hz +// called at 100hz but data from sensor only arrives at 20 Hz #ifdef OPTFLOW_ENABLED static void update_optical_flow(void) { + static uint32_t last_of_update = 0; static int log_counter = 0; - optflow.update(); - optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + // if new data has arrived, process it + if( optflow.last_update != last_of_update ) { + last_of_update = optflow.last_update; + optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow - // write to log - log_counter++; - if( log_counter >= 5 ) { - log_counter = 0; - if (g.log_bitmask & MASK_LOG_OPTFLOW) { - Log_Write_Optflow(); + // write to log at 5hz + log_counter++; + if( log_counter >= 4 ) { + log_counter = 0; + if (g.log_bitmask & MASK_LOG_OPTFLOW) { + Log_Write_Optflow(); + } } } - - /*if(g.optflow_enabled && current_loc.alt < 500){ - * if(GPS_enabled){ - * // if we have a GPS, we add some detail to the GPS - * // XXX this may not ne right - * current_loc.lng += optflow.vlon; - * current_loc.lat += optflow.vlat; - * - * // some sort of error correction routine - * //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction - * //current_loc.lng -= ERR_GAIN * (float)(current_loc.lng - x_GPS_speed); // error correction - * }else{ - * // if we do not have a GPS, use relative from 0 for lat and lon - * current_loc.lng = optflow.vlon; - * current_loc.lat = optflow.vlat; - * } - * // OK to run the nav routines - * nav_ok = true; - * }*/ } #endif diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index cd8b712fa3..0020b5fc7d 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -48,16 +48,20 @@ static void init_compass() static void init_optflow() { #ifdef OPTFLOW_ENABLED - if( optflow.init(false) == false ) { + if( optflow.init(false, &timer_scheduler) == false ) { g.optflow_enabled = false; SendDebug("\nFailed to Init OptFlow "); } + // suspend timer while we set-up SPI communication + timer_scheduler.suspend_timer(); + optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft optflow.set_frame_rate(2000); // set minimum update rate (which should lead to maximum low light performance optflow.set_resolution(OPTFLOW_RESOLUTION); // set optical flow sensor's resolution optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view - // setup timed read of sensor - //timer_scheduler.register_process(&AP_OpticalFlow::read); + + // resume timer + timer_scheduler.resume_timer(); #endif } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index ea3bc32065..13f85e15d7 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -103,6 +103,8 @@ static void init_ardupilot() #endif SPI.begin(); SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate + SPI3.begin(); + SPI3.setSpeed(SPI3_SPEED_2MHZ); // // Initialize the isr_registry. // diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index a204a64ece..9cf3c8605e 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -1209,7 +1209,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv) while(1) { delay(200); - optflow.update(); + optflow.update(millis()); Log_Write_Optflow(); Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), optflow.x,