mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Added Toy Mode control
This commit is contained in:
parent
03879d9113
commit
4a85c40f03
@ -382,8 +382,11 @@ 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 && mode != OF_LOITER)
|
||||
if(mode == TOY || mode == OF_LOITER){
|
||||
// nothing
|
||||
}else if (mode > ALT_HOLD){
|
||||
mode = STABILIZE;
|
||||
}
|
||||
}
|
||||
|
||||
// nothing but OF_LOITER for OptFlow only
|
||||
@ -514,6 +517,12 @@ static void set_mode(byte mode)
|
||||
set_next_WP(¤t_loc);
|
||||
break;
|
||||
|
||||
case TOY:
|
||||
yaw_mode = YAW_TOY;
|
||||
roll_pitch_mode = ROLL_PITCH_TOY;
|
||||
throttle_mode = THROTTLE_MANUAL;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user