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

Made Baro_alt int32_t

Browse Source 
Added home to copter bearing, home to copter distance
Added Land mode
This commit is contained in:
Jason Short 2011-12-09 15:52:34 -08:00
parent 69f1841d8e
commit 7cecca74bc
1 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/ArduCopter.pde
View File

@ -279,7 +279,8 @@ static const char* flight_mode_strings[] = {
"LOITER", "LOITER",
"RTL", "RTL",
"CIRCLE", "CIRCLE",
"POSITION"}; "POSITION",
"LAND"};
/* Radio values /* Radio values
Channel assignments Channel assignments
@ -424,8 +425,8 @@ static int16_t sonar_alt;
static int16_t old_sonar_alt; static int16_t old_sonar_alt;
static int16_t sonar_rate; static int16_t sonar_rate;
static int16_t baro_alt; static int32_t baro_alt;
static int16_t old_baro_alt; static int32_t old_baro_alt;
static int16_t baro_rate; static int16_t baro_rate;
@ -460,6 +461,7 @@ static unsigned loiter_time_max; // millis : how long to stay in LOITER mode
static int32_t nav_roll; // deg * 100 : target roll angle static int32_t nav_roll; // deg * 100 : target roll angle
static int32_t nav_pitch; // deg * 100 : target pitch angle static int32_t nav_pitch; // deg * 100 : target pitch angle
static int32_t nav_yaw; // deg * 100 : target yaw angle static int32_t nav_yaw; // deg * 100 : target yaw angle
static int32_t home_to_copter_bearing; // deg * 100 : target yaw angle
static int32_t auto_yaw; // deg * 100 : target yaw angle static int32_t auto_yaw; // deg * 100 : target yaw angle
static int32_t nav_lat; // for error calcs static int32_t nav_lat; // for error calcs
static int32_t nav_lon; // for error calcs static int32_t nav_lon; // for error calcs
@ -483,6 +485,7 @@ static int16_t auto_level_counter;
// Waypoints // Waypoints
// --------- // ---------
static int32_t home_distance; // meters - distance between plane and next waypoint
static int32_t wp_distance; // meters - distance between plane and next waypoint static int32_t wp_distance; // meters - distance between plane and next waypoint
static int32_t wp_totalDistance; // meters - distance between old and next waypoint static int32_t wp_totalDistance; // meters - distance between old and next waypoint
//static byte next_wp_index; // Current active command index //static byte next_wp_index; // Current active command index
@ -1210,8 +1213,12 @@ static void update_navigation()
case RTL: case RTL:
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// lets just jump to Loiter Mode after RTL //lets just jump to Loiter Mode after RTL
//if(land after RTL)
//set_mode(LAND);
//else
set_mode(LOITER); set_mode(LOITER);
}else{ }else{
// calculates desired Yaw // calculates desired Yaw
// XXX this is an experiment // XXX this is an experiment
@ -1229,6 +1236,7 @@ static void update_navigation()
// switch passthrough to LOITER // switch passthrough to LOITER
case LOITER: case LOITER:
case POSITION: case POSITION:
case LAND:
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
@ -1246,6 +1254,16 @@ static void update_navigation()
} }
// are we in SIMPLE mode?
if(do_simple){
// get distance to home
if(home_distance > 1000){
// 10m
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
}
}
if(yaw_mode == YAW_LOOK_AT_HOME){ if(yaw_mode == YAW_LOOK_AT_HOME){
if(home_is_set){ if(home_is_set){
//nav_yaw = point_at_home_yaw(); //nav_yaw = point_at_home_yaw();