Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

OpticalFlow - added OF_LOITER flightmode

Browse Source
This commit is contained in:
Randy Mackay 2012-01-28 09:27:16 +09:00
parent 1dcd50e3be
commit 122623f64b
4 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/ArduCopter.pde
View File

@ -292,7 +292,8 @@ static const char* flight_mode_strings[] = {
"RTL",
"CIRCLE",
"POSITION",
"LAND"};
"LAND",
"OF_LOITER"};
/* Radio values
Channel assignments
@ -1440,15 +1441,9 @@ void update_roll_pitch_mode(void)
if(do_simple && new_radio_frame){
update_simple_mode();
}
// in this mode, nav_roll and nav_pitch = the iterm
#if WIND_COMP_STAB == 1
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in + nav_roll));
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in + nav_pitch));
#else
// mix in user control with optical flow
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in));
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in));
#endif
break;
}

12
ArduCopter/config.h
View File

@ -503,6 +503,18 @@
# define SUPER_SIMPLE DISABLED
#endif
// LOITER Mode
#ifndef OF_LOITER_YAW
# define OF_LOITER_YAW YAW_HOLD
#endif
#ifndef OF_LOITER_RP
# define OF_LOITER_RP ROLL_PITCH_STABLE_OF
#endif
#ifndef OF_LOITER_THR
# define OF_LOITER_THR THROTTLE_HOLD
#endif
//////////////////////////////////////////////////////////////////////////////
// Attitude Control

3
ArduCopter/defines.h
View File

@ -122,7 +122,8 @@
#define CIRCLE 7 // AUTO control
#define POSITION 8 // AUTO control
#define LAND 9 // AUTO control
#define NUM_MODES 10
#define OF_LOITER 10 // Hold a single location using optical flow sensor
#define NUM_MODES 11
#define INITIALISING 9 // in startup routines

13
ArduCopter/system.pde
View File

@ -408,13 +408,13 @@ static void set_mode(byte mode)
// if we don't have GPS lock
if(home_is_set == false){
// our max mode should be
if (mode > ALT_HOLD)
if (mode > ALT_HOLD && mode != OF_LOITER)
mode = STABILIZE;
}
// nothing but Loiter for OptFlow only
// nothing but OF_LOITER for OptFlow only
if (g.optflow_enabled && GPS_enabled == false){
if (mode > ALT_HOLD && mode != LOITER)
if (mode > ALT_HOLD && mode != OF_LOITER)
mode = STABILIZE;
}
@ -520,6 +520,13 @@ static void set_mode(byte mode)
do_RTL();
break;
case OF_LOITER:
yaw_mode = OF_LOITER_YAW;
roll_pitch_mode = OF_LOITER_RP;
throttle_mode = OF_LOITER_THR;
set_next_WP(&current_loc);
break;
default:
break;
}