Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM - partial integration of optical flow sensor

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2934 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2011-07-21 23:14:53 +00:00
parent e80ed6d687
commit f5d17f756a
8 changed files with 251 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
ArduCopterMega/ArduCopterMega.pde
View File

@ -61,6 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // 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){

37
ArduCopterMega/Log.pde
View File

@ -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() {}

5
ArduCopterMega/Parameters.h
View File

@ -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")),

15
ArduCopterMega/config.h
View File

@ -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
//////////////////////////////////////////////////////////////////////////////

2
ArduCopterMega/defines.h
View File

@ -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
// ----------------

51
ArduCopterMega/setup.pde
View File

@ -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()
{

16
ArduCopterMega/system.pde
View File

@ -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

37
ArduCopterMega/test.pde
View File

@ -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)
{