aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: added set_nav_mode to control initialisation of nav controllers

Browse Source 
Renamed run_navigation_controllers() to run_autopilot()
Renamed update_nav_wp() to update_nav_mode()
Renamed wp_control to nav_mode to be more consistent with roll-pitch,
yaw and throttle controllers
This commit is contained in:
Randy Mackay 2013-01-24 13:36:55 +09:00 committed by rmackay9
parent 8af605cafc
commit fd02cfe706
5 changed files with 160 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/ArduCopter.pde
View File

@ -510,9 +510,8 @@ union float_int {
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t wp_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static uint8_t wp_control;
// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WP
static uint8_t nav_mode;
// Register containing the index of the current navigation command in the mission script
static int16_t command_nav_index;
// Register containing the index of the previous navigation command in the mission script
@ -1766,6 +1765,20 @@ void update_simple_mode(void)
g.rc_2.control_in = _pitch;
}
// update_super_simple_beading - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void update_super_simple_beading()
{
// are we in SIMPLE mode?
if(ap.simple_mode && g.super_simple) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = wrap_360(home_bearing+18000);
}
}
}
// set_throttle_mode - sets the throttle mode and initialises any variables as required
bool set_throttle_mode( uint8_t new_throttle_mode )
{

36
ArduCopter/commands_logic.pde
View File

@ -226,7 +226,7 @@ static void do_RTL(void)
set_throttle_mode(RTL_THR);
// set navigation mode
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
// initial climb starts at current location
set_next_WP(&current_loc);
@ -242,7 +242,7 @@ static void do_RTL(void)
// do_takeoff - initiate takeoff navigation command
static void do_takeoff()
{
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
// Start with current location
Location temp = current_loc;
@ -263,7 +263,7 @@ static void do_takeoff()
// do_nav_wp - initiate move to next waypoint
static void do_nav_wp()
{
wp_control = WP_MODE;
set_nav_mode(NAV_WP);
set_next_WP(&command_nav_queue);
@ -292,7 +292,7 @@ static void do_land()
{
// hold at our current location
set_next_WP(&current_loc);
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
// hold current heading
set_yaw_mode(YAW_HOLD);
@ -302,22 +302,22 @@ static void do_land()
static void do_loiter_unlimited()
{
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
//cliSerial->println("dloi ");
if(command_nav_queue.lat == 0) {
set_next_WP(&current_loc);
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
}else{
set_next_WP(&command_nav_queue);
wp_control = WP_MODE;
set_nav_mode(NAV_WP);
}
}
// do_loiter_turns - initiate moving in a circle
static void do_loiter_turns()
{
wp_control = CIRCLE_MODE;
set_nav_mode(NAV_CIRCLE);
if(command_nav_queue.lat == 0) {
// allow user to specify just the altitude
@ -344,11 +344,11 @@ static void do_loiter_turns()
static void do_loiter_time()
{
if(command_nav_queue.lat == 0) {
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
loiter_time = millis();
set_next_WP(&current_loc);
}else{
wp_control = WP_MODE;
set_nav_mode(NAV_WP);
set_next_WP(&command_nav_queue);
}
@ -407,7 +407,7 @@ static bool verify_nav_wp()
// we have reached our goal
// loiter at the WP
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
@ -428,9 +428,9 @@ static bool verify_nav_wp()
static bool verify_loiter_unlimited()
{
if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) {
if(nav_mode == NAV_WP && wp_distance <= (g.waypoint_radius * 100)) {
// switch to position hold
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
}
return false;
}
@ -438,16 +438,16 @@ static bool verify_loiter_unlimited()
// verify_loiter_time - check if we have loitered long enough
static bool verify_loiter_time()
{
if(wp_control == LOITER_MODE) {
if(nav_mode == NAV_LOITER) {
if ((millis() - loiter_time) > loiter_time_max) {
return true;
}
}
if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) {
if(nav_mode == NAV_WP && wp_distance <= (g.waypoint_radius * 100)) {
// reset our loiter time
loiter_time = millis();
// switch to position hold
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
}
return false;
}
@ -487,7 +487,7 @@ static bool verify_RTL()
set_new_altitude(get_RTL_alt());
// set navigation mode
wp_control = WP_MODE;
set_nav_mode(NAV_WP);
// set yaw mode
set_yaw_mode(RTL_YAW);
@ -501,7 +501,7 @@ static bool verify_RTL()
// if we've reached home initiate loiter
if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) {
rtl_state = RTL_STATE_LOITERING_AT_HOME;
wp_control = LOITER_MODE;
set_nav_mode(NAV_LOITER);
// set loiter timer
rtl_loiter_start_time = millis();

10
ArduCopter/defines.h
View File

@ -194,11 +194,11 @@
#define NO_COMMAND 0
// Navigation modes held in wp_control variable
#define LOITER_MODE 1
#define WP_MODE 2
#define CIRCLE_MODE 3
#define NO_NAV_MODE 4
// Navigation modes held in nav_mode variable
#define NAV_NONE 0
#define NAV_CIRCLE 1
#define NAV_LOITER 2
#define NAV_WP 3
// Yaw override behaviours - used for setting yaw_override_behaviour
#define YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT 0 // auto pilot takes back yaw control at next waypoint

229
ArduCopter/navigation.pde
View File

@ -78,7 +78,8 @@ static void run_nav_updates(void)
calc_distance_and_bearing();
nav_updates.need_dist_bearing = 0;
} else if (nav_updates.need_nav_controllers) {
run_navigation_controllers();
run_autopilot();
update_nav_mode();
nav_updates.need_nav_controllers = 0;
} else if (nav_updates.need_nav_pitch_roll) {
calc_nav_pitch_roll();
@ -158,6 +159,9 @@ static void calc_distance_and_bearing()
wp_bearing = get_bearing_cd(&current_loc, &next_WP);
home_bearing = get_bearing_cd(&current_loc, &home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_beading();
// bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
yaw_look_at_WP_bearing = get_bearing_cd(&current_loc, &yaw_look_at_WP);
}
@ -180,115 +184,84 @@ static void calc_location_error(struct Location *next_loc)
lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North
}
// called after a GPS read
static void run_navigation_controllers()
// run_autopilot - highest level call to process mission commands
static void run_autopilot()
{
// wp_distance is in CM
// --------------------
switch(control_mode) {
case AUTO:
// note: wp_control is handled by commands_logic
verify_commands();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case GUIDED:
// switch to loiter once we've reached the target location and altitude
if(verify_nav_wp()) {
set_mode(LOITER);
}
update_nav_wp();
break;
case RTL:
// execute the RTL state machine
verify_RTL();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
// switch passthrough to LOITER
case LOITER:
case POSITION:
// This feature allows us to reposition the quad when the user lets
// go of the sticks
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500){
ap.loiter_override = true;
switch( control_mode ) {
case AUTO:
// majority of command logic is in commands_logic.pde
verify_commands();
break;
case GUIDED:
// switch to loiter once we've reached the target location and altitude
if(verify_nav_wp()) {
set_nav_mode(NAV_LOITER);
}
}
// Allow the user to take control temporarily,
if(ap.loiter_override) {
// this sets the copter to not try and nav while we control it
wp_control = NO_NAV_MODE;
// reset LOITER to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
wp_control = LOITER_MODE;
ap.loiter_override = false;
}
}else{
wp_control = LOITER_MODE;
}
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case LAND:
verify_land();
// calculates the desired Roll and Pitch
update_nav_wp();
break;
case CIRCLE:
wp_control = CIRCLE_MODE;
update_nav_wp();
break;
case STABILIZE:
case TOY_A:
case TOY_M:
wp_control = NO_NAV_MODE;
update_nav_wp();
break;
}
// are we in SIMPLE mode?
if(ap.simple_mode && g.super_simple) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = wrap_360(home_bearing+18000);
}
case RTL:
verify_RTL();
break;
}
}
// update_nav_wp - high level calculation of nav_lat and nav_lon based on wp_control
// called after gps read from run_navigation_controller
static void update_nav_wp()
// set_nav_mode - update nav mode and initialise any variables as required
static bool set_nav_mode(uint8_t new_nav_mode)
{
// boolean to ensure proper initialisation of nav modes
bool nav_initialised = false;
// return immediately if no change
if( new_nav_mode == nav_mode ) {
return true;
}
switch( new_nav_mode ) {
case NAV_NONE:
nav_initialised = true;
break;
case NAV_CIRCLE:
// start circling around current location
set_next_WP(&current_loc);
circle_WP = next_WP;
circle_angle = 0;
nav_initialised = true;
break;
case NAV_LOITER:
// set target to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
nav_initialised = true;
break;
case NAV_WP:
nav_initialised = true;
break;
}
// if initialisation has been successful update the yaw mode
if( nav_initialised ) {
nav_mode = new_nav_mode;
}
// return success or failure
return nav_initialised;
}
// update_nav_mode - run navigation controller based on nav_mode
static void update_nav_mode()
{
int16_t loiter_delta;
int16_t speed;
switch( wp_control ) {
case LOITER_MODE:
// calc error to target
calc_location_error(&next_WP);
switch( nav_mode ) {
// use error as the desired rate towards the target
calc_loiter(long_error, lat_error);
case NAV_NONE:
// do nothing
break;
case CIRCLE_MODE:
case NAV_CIRCLE:
// check if we have missed the WP
loiter_delta = (wp_bearing - old_wp_bearing)/100;
@ -316,37 +289,65 @@ static void update_nav_wp()
// use error as the desired rate towards the target
// nav_lon, nav_lat is calculated
// if the target location is >4m use waypoint controller
if(wp_distance > 400) {
calc_nav_rate(get_desired_speed(g.waypoint_speed_max));
}else{
// calc the lat and long error to the target
calc_location_error(&next_WP);
// call loiter controller
calc_loiter(long_error, lat_error);
}
break;
case WP_MODE:
// calc error to target
calc_location_error(&next_WP);
case NAV_LOITER:
// check if user is overriding the loiter controller
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500) {
if(wp_distance > 500){
ap.loiter_override = true;
}
}
// check if user has release sticks
if(ap.loiter_override) {
if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) {
ap.loiter_override = false;
// reset LOITER to current position
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
}
// We bring copy over our Iterms for wind control, but we don't navigate
nav_lon = g.pid_loiter_rate_lon.get_integrator();
nav_lat = g.pid_loiter_rate_lon.get_integrator();
nav_lon = constrain(nav_lon, -2000, 2000);
nav_lat = constrain(nav_lat, -2000, 2000);
}else{
// calc error to target
calc_location_error(&next_WP);
// use error as the desired rate towards the target
calc_loiter(long_error, lat_error);
}
break;
case NAV_WP:
// calc position error to target
calc_location_error(&next_WP);
// calc speed to target
speed = get_desired_speed(g.waypoint_speed_max);
// use error as the desired rate towards the target
calc_nav_rate(speed);
break;
case NO_NAV_MODE:
// clear out our nav so we can do things like land straight down
// or change Loiter position
// We bring copy over our Iterms for wind control, but we don't navigate
nav_lon = g.pid_loiter_rate_lon.get_integrator();
nav_lat = g.pid_loiter_rate_lon.get_integrator();
nav_lon = constrain(nav_lon, -2000, 2000); // 20 degrees
nav_lat = constrain(nav_lat, -2000, 2000); // 20 degrees
break;
}
/*
// To-Do: check that we haven't broken toy mode
case TOY_A:
case TOY_M:
set_nav_mode(NAV_NONE);
update_nav_wp();
break;
}
*/
}
static bool check_missed_wp()

10
ArduCopter/system.pde
View File

@ -405,6 +405,7 @@ static void set_mode(uint8_t mode)
set_yaw_mode(YAW_ACRO);
set_roll_pitch_mode(ROLL_PITCH_ACRO);
set_throttle_mode(THROTTLE_MANUAL);
set_nav_mode(NAV_NONE);
// reset acro axis targets to current attitude
if(g.axis_enabled){
roll_axis = ahrs.roll_sensor;
@ -419,6 +420,7 @@ static void set_mode(uint8_t mode)
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
set_throttle_mode(STABILIZE_THROTTLE);
set_nav_mode(NAV_NONE);
break;
case ALT_HOLD:
@ -427,6 +429,7 @@ static void set_mode(uint8_t mode)
set_yaw_mode(ALT_HOLD_YAW);
set_roll_pitch_mode(ALT_HOLD_RP);
set_throttle_mode(ALT_HOLD_THR);
set_nav_mode(NAV_NONE);
break;
case AUTO:
@ -480,7 +483,7 @@ static void set_mode(uint8_t mode)
set_yaw_mode(GUIDED_YAW);
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
wp_control = WP_MODE;
set_nav_mode(NAV_WP);
wp_verify_byte = 0;
set_next_WP(&guided_WP);
break;
@ -525,11 +528,10 @@ static void set_mode(uint8_t mode)
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_throttle_mode(THROTTLE_AUTO);
wp_control = NO_NAV_MODE;
set_nav_mode(NAV_NONE);
// save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in;
break;
case TOY_M:
@ -537,7 +539,7 @@ static void set_mode(uint8_t mode)
ap.manual_attitude = true;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
wp_control = NO_NAV_MODE;
set_nav_mode(NAV_NONE);
set_throttle_mode(THROTTLE_HOLD);
break;