From f5d17f756a4d082bf4bd0bbc9363f430aad20bfe Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Thu, 21 Jul 2011 23:14:53 +0000 Subject: [PATCH] ACM - partial integration of optical flow sensor git-svn-id: https://arducopter.googlecode.com/svn/trunk@2934 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 91 ++++++++++++++++++++++++++++++- ArduCopterMega/Log.pde | 37 +++++++++++++ ArduCopterMega/Parameters.h | 5 +- ArduCopterMega/config.h | 15 +++++ ArduCopterMega/defines.h | 2 + ArduCopterMega/setup.pde | 51 +++++++++++++++++ ArduCopterMega/system.pde | 16 ++++++ ArduCopterMega/test.pde | 37 +++++++++++++ 8 files changed, 251 insertions(+), 3 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index eb047a33c4..b317ec9c0c 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -61,6 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // PID library #include // RC Channel Library #include // Range finder library +#include // Optical Flow library #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions @@ -132,6 +133,9 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #else #error Unrecognised MAG_PROTOCOL setting. #endif + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; + #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -452,9 +456,11 @@ static struct Location next_WP; // next waypoint static struct Location target_WP; // where do we want to you towards? static struct Location simple_WP; // static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint +static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location +static struct Location optflow_offset; // optical flow base position +static boolean new_location; // flag to tell us if location has been updated // IMU variables @@ -1003,7 +1009,88 @@ static void update_GPS(void) } } -static void update_current_flight_mode(void) +#ifdef OPTFLOW_ENABLED +// blend gps and optical flow location +void update_location(void) +{ + //static int count = 0; + // get GPS position + if(GPS_enabled){ + update_GPS(); + } + + if( g.optflow_enabled ) { + int32_t temp_lat, temp_lng, diff_lat, diff_lng; + + // get optical flow position + optflow.read(); + optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt); + + // write to log + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } + + temp_lat = optflow_offset.lat + optflow.lat; + temp_lng = optflow_offset.lng + optflow.lng; + + // if we have good GPS values, don't let optical flow position stray too far + if( GPS_enabled && g_gps->fix ) { + // ensure current location is within 3m of gps location + diff_lat = g_gps->latitude - temp_lat; + diff_lng = g_gps->longitude - temp_lng; + if( diff_lat > 300 ) { + optflow_offset.lat += diff_lat - 300; + //Serial.println("lat inc!"); + } + if( diff_lat < -300 ) { + optflow_offset.lat += diff_lat + 300; + //Serial.println("lat dec!"); + } + if( diff_lng > 300 ) { + optflow_offset.lng += diff_lng - 300; + //Serial.println("lng inc!"); + } + if( diff_lng < -300 ) { + optflow_offset.lng += diff_lng + 300; + //Serial.println("lng dec!"); + } + } + + // update the current position + current_loc.lat = optflow_offset.lat + optflow.lat; + current_loc.lng = optflow_offset.lng + optflow.lng; + + /*count++; + if( count >= 20 ) { + count = 0; + Serial.println(); + Serial.print("lat:"); + Serial.print(current_loc.lat); + Serial.print("\tlng:"); + Serial.print(current_loc.lng); + Serial.print("\tr:"); + Serial.print(nav_roll); + Serial.print("\tp:"); + Serial.print(nav_pitch); + Serial.println(); + }*/ + + // indicate we have a new position for nav functions + new_location = true; + + }else{ + // get current position from gps + current_loc.lng = g_gps->longitude; // Lon * 10 * *7 + current_loc.lat = g_gps->latitude; // Lat * 10 * *7 + + new_location = g_gps->new_data; + } +} +#endif + + +void update_current_flight_mode(void) { if(control_mode == AUTO){ diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 15f33bc9d5..4f9b3131a3 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -71,6 +71,7 @@ print_log_menu(void) if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD")); if (g.log_bitmask & MASK_LOG_CURRENT) Serial.printf_P(PSTR(" CURRENT")); if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS")); + if (g.log_bitmask & MASK_LOG_OPTFLOW) Serial.printf_P(PSTR(" OPTFLOW")); } Serial.println(); @@ -181,6 +182,7 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(CMD); TARG(CURRENT); TARG(MOTORS); + TARG(OPTFLOW); #undef TARG } @@ -494,6 +496,34 @@ static void Log_Read_Motors() DataFlash.ReadInt()); } +#ifdef OPTFLOW_ENABLED +// Write an optical flow packet. Total length : 18 bytes +static void Log_Write_Optflow() +{ + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_OPTFLOW_MSG); + DataFlash.WriteInt((int)optflow.dx); + DataFlash.WriteInt((int)optflow.dy); + DataFlash.WriteInt((int)optflow.surface_quality); + DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng); + DataFlash.WriteByte(END_BYTE); +} +#endif + +// Read an attitude packet +static void Log_Read_Optflow() +{ + Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + (float)DataFlash.ReadLong(),// / t7, + (float)DataFlash.ReadLong() // / t7 + ); +} + static void Log_Write_Nav_Tuning() { Matrix3f tempmat = dcm.get_dcm_matrix(); @@ -820,6 +850,10 @@ static void Log_Read(int start_page, int end_page) Log_Read_Motors(); break; + case LOG_OPTFLOW_MSG: + Log_Read_Optflow(); + break; + case LOG_GPS_MSG: Log_Read_GPS(); break; @@ -842,6 +876,9 @@ static void Log_Write_Raw() {} static void Log_Write_GPS() {} static void Log_Write_Current() {} static void Log_Write_Attitude() {} +#ifdef OPTFLOW_ENABLED +static void Log_Write_Optflow() {} +#endif static void Log_Write_Nav_Tuning() {} static void Log_Write_Control_Tuning() {} static void Log_Write_Motors() {} diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 578e64d624..64b19dd811 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 104; + static const uint16_t k_format_version = 105; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -75,6 +75,7 @@ public: k_param_sonar, k_param_frame_orientation, k_param_top_bottom_ratio, + k_param_optflow_enabled, // // 160: Navigation parameters @@ -233,6 +234,7 @@ public: AP_Int8 esc_calibrate; AP_Int8 frame_orientation; AP_Float top_bottom_ratio; + AP_Int8 optflow_enabled; #if FRAME_CONFIG == HELI_FRAME // Heli @@ -288,6 +290,7 @@ public: battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), + optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index f8bcbb8ce0..e6c9c3062d 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -200,6 +200,21 @@ ////////////////////////////////////////////////////////////////////////////// +// OPTICAL_FLOW +#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included + #define OPTFLOW_ENABLED +#endif + +#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) +# define OPTFLOW DISABLED +#endif +#ifndef OPTFLOW_ORIENTATION +# define OPTFLOW_ORIENTATION AP_OPTICALFLOW_ADNS3080_PINS_FORWARD +#endif +#ifndef OPTFLOW_FOV +# define OPTFLOW_FOV AP_OPTICALFLOW_ADNS3080_12_FOV +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 09627cfc56..906c6aa232 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -261,6 +261,7 @@ #define LOG_CURRENT_MSG 0x09 #define LOG_STARTUP_MSG 0x0A #define LOG_MOTORS_MSG 0x0B +#define LOG_OPTFLOW_MSG 0x0C #define LOG_INDEX_MSG 0xF0 #define MAX_NUM_LOGS 50 @@ -275,6 +276,7 @@ #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CURRENT (1<<9) #define MASK_LOG_MOTORS (1<<10) +#define MASK_LOG_OPTFLOW (1<<11) // Waypoint Modes // ---------------- diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 10a23c5dc4..f978d30fa2 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -16,6 +16,9 @@ static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); +#ifdef OPTFLOW_ENABLED +static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); +#endif static int8_t setup_show (uint8_t argc, const Menu::arg *argv); #if FRAME_CONFIG == HELI_FRAME @@ -40,6 +43,9 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { {"compass", setup_compass}, // {"offsets", setup_mag_offset}, {"declination", setup_declination}, +#ifdef OPTFLOW_ENABLED + {"optflow", setup_optflow}, +#endif #if FRAME_CONFIG == HELI_FRAME {"heli", setup_heli}, {"gyro", setup_gyro}, @@ -93,6 +99,9 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_flight_modes(); report_imu(); report_compass(); +#ifdef OPTFLOW_ENABLED + report_optflow(); +#endif #if FRAME_CONFIG == HELI_FRAME report_heli(); report_gyro(); @@ -704,6 +713,32 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) } */ +#ifdef OPTFLOW_ENABLED +static int8_t +setup_optflow(uint8_t argc, const Menu::arg *argv) +{ + if (!strcmp_P(argv[1].str, PSTR("on"))) { + g.optflow_enabled = true; + init_optflow(); + + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { + g.optflow_enabled = false; + + //} else if(argv[1].i > 10){ + // g.optflow_fov.set_and_save(argv[1].i); + // optflow.set_field_of_view(g.optflow_fov.get()); + + }else{ + Serial.printf_P(PSTR("\nOptions:[on, off]\n")); + report_optflow(); + return 0; + } + + g.optflow_enabled.save(); + report_optflow(); + return 0; +} +#endif /***************************************************************************/ // CLI reports @@ -894,6 +929,22 @@ static void report_flight_modes() print_blanks(2); } +#ifdef OPTFLOW_ENABLED +void report_optflow() +{ + Serial.printf_P(PSTR("OptFlow\n")); + print_divider(); + + print_enabled(g.optflow_enabled); + + // field of view + //Serial.printf_P(PSTR("FOV: %4.0f\n"), + // degrees(g.optflow_fov)); + + print_blanks(2); +} +#endif + #if FRAME_CONFIG == HELI_FRAME static void report_heli() { diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index f05f2b26df..1bf8b92aa0 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -215,6 +215,13 @@ static void init_ardupilot() } #endif +#ifdef OPTFLOW_ENABLED + // init the optical flow sensor + if(g.optflow_enabled) { + init_optflow(); + } +#endif + // Logging: // -------- // DataFlash log initialization @@ -459,6 +466,15 @@ init_compass() Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference } +#ifdef OPTFLOW_ENABLED +static void +init_optflow() +{ + bool junkbool = optflow.init(); + optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft + optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view +} +#endif /* This function gets the current value of the heap and stack pointers. * The stack pointer starts at the top of RAM and grows downwards. The heap pointer diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index ba39ab90a4..1e0443e401 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -24,6 +24,9 @@ static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_baro(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); +#ifdef OPTFLOW_ENABLED +static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); +#endif static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); @@ -71,6 +74,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = { #endif {"sonar", test_sonar}, {"compass", test_mag}, +#ifdef OPTFLOW_ENABLED + {"optflow", test_optflow}, +#endif {"xbee", test_xbee}, {"eedump", test_eedump}, {"rawgps", test_rawgps}, @@ -972,6 +978,37 @@ test_sonar(uint8_t argc, const Menu::arg *argv) return (0); } +#ifdef OPTFLOW_ENABLED +static int8_t +test_optflow(uint8_t argc, const Menu::arg *argv) +{ + if(g.optflow_enabled) { + Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); + print_hit_enter(); + + while(1){ + delay(200); + optflow.read(); + Log_Write_Optflow(); + Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"), + optflow.x, + optflow.dx, + optflow.y, + optflow.dy, + optflow.surface_quality); + + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("OptFlow: ")); + print_enabled(false); + return (0); + } +} +#endif + static int8_t test_mission(uint8_t argc, const Menu::arg *argv) {