Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: removed next_WP 

Use the wp_nav.get_target_alt or controller_desired_alt in it's place
This commit is contained in:
Randy Mackay 2013-03-22 21:59:17 +09:00
parent c40080e76a
commit 5b93990e41
5 changed files with 39 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/ArduCopter.pde
View File

@ -491,8 +491,16 @@ union float_int {
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
// This is the angle from the copter to the next waypoint in centi-degrees
static int32_t wp_bearing;
// The original bearing to the next waypoint. used to check if we've passed the waypoint
static int32_t original_wp_bearing;
// The location of home in relation to the copter in centi-degrees
static int32_t home_bearing;
// distance between plane and home in cm
static int32_t home_distance;
// distance between plane and next waypoint in cm. is not static because AP_Camera uses it
uint32_t wp_distance;
// 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
@ -646,18 +654,6 @@ static int16_t angle_boost;
static uint16_t land_detector;
////////////////////////////////////////////////////////////////////////////////
// Navigation general
////////////////////////////////////////////////////////////////////////////////
// The location of home in relation to the copter, updated every GPS read
static int32_t home_bearing;
// distance between plane and home in cm
static int32_t home_distance;
// distance between plane and next_WP in cm
// is not static because AP_Camera uses it
uint32_t wp_distance;
////////////////////////////////////////////////////////////////////////////////
// 3D Location vectors
////////////////////////////////////////////////////////////////////////////////
@ -666,22 +662,12 @@ uint32_t wp_distance;
static struct Location home;
// Current location of the copter
static struct Location current_loc;
// Next WP is the desired location of the copter - the next waypoint or loiter location
static struct Location next_WP;
// Holds the current loaded command from the EEPROM for navigation
static struct Location command_nav_queue;
// Holds the current loaded command from the EEPROM for conditional scripts
static struct Location command_cond_queue;
////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_wp_bearing;
////////////////////////////////////////////////////////////////////////////////
// Navigation Roll/Pitch functions
////////////////////////////////////////////////////////////////////////////////
@ -1806,7 +1792,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_HOLD:
case THROTTLE_AUTO:
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
set_new_altitude(current_loc.alt); // by default hold the current altitude
set_new_altitude(controller_desired_alt); // by default hold the current altitude
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in));
@ -1819,7 +1805,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
land_detector = 0; // A counter that goes up if our climb rate stalls out.
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
// Set target altitude to LAND_START_ALT if we are high, below this altitude the get_throttle_rate_stabilized will take care of setting the next_WP.alt
if (current_loc.alt >= LAND_START_ALT) {
if (controller_desired_alt >= LAND_START_ALT) {
set_new_altitude(LAND_START_ALT);
}
throttle_initialised = true;
@ -1983,7 +1969,7 @@ void update_throttle_mode(void)
case THROTTLE_AUTO:
// auto pilot altitude controller with target altitude held in next_WP.alt
if(motors.auto_armed() == true) {
get_throttle_althold_with_slew(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max);
get_throttle_althold_with_slew(wp_nav.get_target_alt(), g.auto_velocity_z_min, g.auto_velocity_z_max);
}
break;

14
ArduCopter/Log.pde
View File

@ -533,8 +533,8 @@ struct log_Control_Tuning {
LOG_PACKET_HEADER;
int16_t throttle_in;
int16_t sonar_alt;
int16_t baro_alt;
int16_t next_wp_alt;
int32_t baro_alt;
float next_wp_alt;
int16_t nav_throttle;
int16_t angle_boost;
int16_t climb_rate;
@ -549,8 +549,8 @@ static void Log_Write_Control_Tuning()
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
throttle_in : g.rc_3.control_in,
sonar_alt : sonar_alt,
baro_alt : (int16_t) baro_alt,
next_wp_alt : (int16_t) next_WP.alt,
baro_alt : baro_alt,
next_wp_alt : wp_nav.get_target_alt(),
nav_throttle : nav_throttle,
angle_boost : angle_boost,
climb_rate : climb_rate,
@ -567,11 +567,11 @@ static void Log_Read_Control_Tuning()
DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1 2 3 4 5 6 7 8 9
cliSerial->printf_P(PSTR("CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
cliSerial->printf_P(PSTR("CTUN, %d, %d, %ld, %4.0f, %d, %d, %d, %d, %d\n"),
(int)pkt.throttle_in,
(int)pkt.sonar_alt,
(int)pkt.baro_alt,
(int)pkt.next_wp_alt,
(long)pkt.baro_alt,
(float)pkt.next_wp_alt,
(int)pkt.nav_throttle,
(int)pkt.angle_boost,
(int)pkt.climb_rate,

19
ArduCopter/navigation.pde
View File

@ -187,27 +187,28 @@ static bool check_missed_wp()
return (labs(temp) > 9000); // we passed the waypoint by 90 degrees
}
static void force_new_altitude(int32_t new_alt)
static void force_new_altitude(float new_alt)
{
next_WP.alt = new_alt;
// update new target altitude
wp_nav.set_target_alt(new_alt);
set_alt_change(REACHED_ALT);
}
static void set_new_altitude(int32_t new_alt)
static void set_new_altitude(float new_alt)
{
// if no change exit immediately
if(new_alt == next_WP.alt) {
if(new_alt == wp_nav.get_target_alt()) {
return;
}
// update new target altitude
next_WP.alt = new_alt;
wp_nav.set_target_alt(new_alt);
if(next_WP.alt > (current_loc.alt + 80)) {
if(new_alt > (current_loc.alt + 80)) {
// we are below, going up
set_alt_change(ASCENDING);
}else if(next_WP.alt < (current_loc.alt - 80)) {
}else if(new_alt < (current_loc.alt - 80)) {
// we are above, going down
set_alt_change(DESCENDING);
@ -222,12 +223,12 @@ static void verify_altitude()
{
if(alt_change_flag == ASCENDING) {
// we are below, going up
if(current_loc.alt > next_WP.alt - 50) {
if(current_loc.alt > wp_nav.get_target_alt() - 50) {
set_alt_change(REACHED_ALT);
}
}else if (alt_change_flag == DESCENDING) {
// we are above, going down
if(current_loc.alt <= next_WP.alt + 50){
if(current_loc.alt <= wp_nav.get_target_alt() + 50){
set_alt_change(REACHED_ALT);
}
}

3
ArduCopter/test.pde
View File

@ -943,8 +943,7 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
current_loc.lat = 389539260;
current_loc.lng = -1199540200;
next_WP.lat = 389538528;
next_WP.lng = -1199541248;
wp_nav.set_destination(pv_latlon_to_vector(389538528,-1199541248,0));
// got 23506;, should be 22800
update_navigation();

16
ArduCopter/toy.pde
View File

@ -52,21 +52,21 @@ void update_toy_altitude()
if(false == CH7_toy_flag && input > 1666) {
CH7_toy_flag = true;
// go up
if(next_WP.alt >= 400) {
force_new_altitude(next_WP.alt + TOY_ALT_LARGE);
if(controller_desired_alt >= 400) {
force_new_altitude(controller_desired_alt + TOY_ALT_LARGE);
}else{
force_new_altitude(next_WP.alt + TOY_ALT_SMALL);
force_new_altitude(controller_desired_alt + TOY_ALT_SMALL);
}
// Trigger downward alt change
}else if(false == CH7_toy_flag && input < 1333) {
CH7_toy_flag = true;
// go down
if(next_WP.alt >= (400 + TOY_ALT_LARGE)) {
force_new_altitude(next_WP.alt - TOY_ALT_LARGE);
}else if(next_WP.alt >= TOY_ALT_SMALL) {
force_new_altitude(next_WP.alt - TOY_ALT_SMALL);
}else if(next_WP.alt < TOY_ALT_SMALL) {
if(controller_desired_alt >= (400 + TOY_ALT_LARGE)) {
force_new_altitude(controller_desired_alt - TOY_ALT_LARGE);
}else if(controller_desired_alt >= TOY_ALT_SMALL) {
force_new_altitude(controller_desired_alt - TOY_ALT_SMALL);
}else if(controller_desired_alt < TOY_ALT_SMALL) {
force_new_altitude(0);
}