Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -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", "RTL",
"CIRCLE", "CIRCLE",
"POSITION", "POSITION",
"LAND"}; "LAND",
"OF_LOITER"};
/* Radio values /* Radio values
Channel assignments Channel assignments
@ -1440,15 +1441,9 @@ void update_roll_pitch_mode(void)
if(do_simple && new_radio_frame){ if(do_simple && new_radio_frame){
update_simple_mode(); update_simple_mode();
} }
// mix in user control with optical flow
// 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
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in)); 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)); g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in));
#endif
break; break;
} }

12
ArduCopter/config.h
View File

@ -503,6 +503,18 @@
# define SUPER_SIMPLE DISABLED # define SUPER_SIMPLE DISABLED
#endif #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 // Attitude Control

3
ArduCopter/defines.h
View File

@ -122,7 +122,8 @@
#define CIRCLE 7 // AUTO control #define CIRCLE 7 // AUTO control
#define POSITION 8 // AUTO control #define POSITION 8 // AUTO control
#define LAND 9 // 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 #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 we don't have GPS lock
if(home_is_set == false){ if(home_is_set == false){
// our max mode should be // our max mode should be
if (mode > ALT_HOLD) if (mode > ALT_HOLD && mode != OF_LOITER)
mode = STABILIZE; mode = STABILIZE;
} }
// nothing but Loiter for OptFlow only // nothing but OF_LOITER for OptFlow only
if (g.optflow_enabled && GPS_enabled == false){ if (g.optflow_enabled && GPS_enabled == false){
if (mode > ALT_HOLD && mode != LOITER) if (mode > ALT_HOLD && mode != OF_LOITER)
mode = STABILIZE; mode = STABILIZE;
} }
@ -520,6 +520,13 @@ static void set_mode(byte mode)
do_RTL(); do_RTL();
break; 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: default:
break; break;
} }