mirror of https://github.com/ArduPilot/ardupilot
minor navigation updates, added new copter friendly altitude control for navigating waypoints.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1518 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
e26b2dab67
commit
02fae1d48d
|
|
@ -81,11 +81,9 @@ void increment_WP_index()
|
|||
{
|
||||
if(wp_index < wp_total){
|
||||
wp_index++;
|
||||
Serial.print("MSG WP index is incremented to ");
|
||||
Serial.println(wp_index,DEC);
|
||||
Serial.printf_P(PSTR("WP index is incremented to %d\n"),wp_index);
|
||||
}else{
|
||||
Serial.print("MSG Failed to increment WP index of ");
|
||||
Serial.println(wp_index,DEC);
|
||||
Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),wp_index);
|
||||
}
|
||||
}
|
||||
void decrement_WP_index()
|
||||
|
|
@ -104,6 +102,9 @@ void decrement_WP_index()
|
|||
void
|
||||
return_to_launch(void)
|
||||
{
|
||||
//so we know where we are navigating from
|
||||
next_WP = current_loc;
|
||||
|
||||
// home is WP 0
|
||||
// ------------
|
||||
wp_index = 0;
|
||||
|
|
@ -122,9 +123,9 @@ struct
|
|||
Location get_LOITER_home_wp()
|
||||
{
|
||||
// read home position
|
||||
struct Location temp = get_wp_with_index(0);
|
||||
temp.id = CMD_LOITER;
|
||||
temp.alt = read_alt_to_hold();
|
||||
struct Location temp = get_wp_with_index(0);
|
||||
temp.id = CMD_LOITER;
|
||||
temp.alt = read_alt_to_hold();
|
||||
return temp;
|
||||
}
|
||||
|
||||
|
|
@ -142,9 +143,7 @@ It looks to see what the next command type is and finds the last command.
|
|||
void
|
||||
set_next_WP(struct Location *wp)
|
||||
{
|
||||
//send_message(SEVERITY_LOW,"load WP");
|
||||
Serial.print("MSG set_next_WP, wp_index: ");
|
||||
Serial.println(wp_index,DEC);
|
||||
Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), wp_index);
|
||||
send_message(MSG_COMMAND, wp_index);
|
||||
|
||||
// copy the current WP into the OldWP slot
|
||||
|
|
@ -194,17 +193,14 @@ set_next_WP(struct Location *wp)
|
|||
// -------------------------------
|
||||
void init_home()
|
||||
{
|
||||
Serial.println("MSG: init home");
|
||||
|
||||
// Extra read just in case
|
||||
// -----------------------
|
||||
//GPS.Read();
|
||||
Serial.printf_P(PSTR("init home\n"));
|
||||
|
||||
// block until we get a good fix
|
||||
// -----------------------------
|
||||
while (!GPS.new_data || !GPS.fix) {
|
||||
GPS.update();
|
||||
}
|
||||
|
||||
home.id = CMD_WAYPOINT;
|
||||
home.lng = GPS.longitude; // Lon * 10**7
|
||||
home.lat = GPS.latitude; // Lat * 10**7
|
||||
|
|
@ -213,8 +209,6 @@ void init_home()
|
|||
|
||||
// ground altitude in centimeters for pressure alt calculations
|
||||
// ------------------------------------------------------------
|
||||
//ground_alt = GPS.altitude;
|
||||
//pressure_altitude = GPS.altitude; // Set initial value for filter
|
||||
save_EEPROM_pressure();
|
||||
|
||||
// Save Home to EEPROM
|
||||
|
|
|
|||
|
|
@ -123,22 +123,21 @@ void calc_distance_error()
|
|||
// calculated at 50 hz
|
||||
void calc_altitude_error()
|
||||
{
|
||||
altitude_error = next_WP.alt - current_loc.alt;
|
||||
if(control_mode == AUTO && offset_altitude != 0) {
|
||||
// limit climb rates - we draw a straight line between first location and edge of wp_radius
|
||||
target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - wp_radius));
|
||||
|
||||
// stay within a certain range
|
||||
if(prev_WP.alt > next_WP.alt){
|
||||
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
|
||||
}else{
|
||||
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
|
||||
}
|
||||
} else {
|
||||
target_altitude = next_WP.alt;
|
||||
}
|
||||
|
||||
// limit climb rates
|
||||
//target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
|
||||
|
||||
//if(prev_WP.alt > next_WP.alt){
|
||||
// target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
|
||||
// }else{
|
||||
// target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
|
||||
// }
|
||||
|
||||
// calc the GPS/Abs pressure altitude
|
||||
//if(GPS.fix)
|
||||
// pressure_altitude += altitude_gain * (float)(GPS.altitude - pressure_altitude);
|
||||
//current_loc.alt = pressure_altitude;
|
||||
// altitude_error = target_altitude - current_loc.alt;
|
||||
altitude_error = target_altitude - current_loc.alt;
|
||||
|
||||
//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -211,6 +211,9 @@ void startup_ground(void)
|
|||
Serial.println("*")
|
||||
}
|
||||
*/
|
||||
// read the radio to set trims
|
||||
// ---------------------------
|
||||
trim_radio();
|
||||
|
||||
if (log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
|
@ -228,13 +231,7 @@ void startup_ground(void)
|
|||
|
||||
//IMU ground start
|
||||
//------------------------
|
||||
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
|
||||
init_pressure_ground();
|
||||
#endif
|
||||
|
||||
// read the radio to set trims
|
||||
// ---------------------------
|
||||
trim_radio();
|
||||
|
||||
// Warm up and read Gyro offsets
|
||||
// -----------------------------
|
||||
|
|
|
|||
Loading…
Reference in New Issue