Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: use a const reference for home

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-03 22:33:40 +11:00
parent c493d980d8
commit 57e0eb4db5
7 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/ArduPlane.pde
View File

@ -622,6 +622,9 @@ static int16_t condition_rate;
// 3D Location vectors
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
// reference to AHRS home
static const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location in AHRS
static bool home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
@ -1379,7 +1382,7 @@ static void update_alt()
}
}
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - ahrs.get_home().alt + (int32_t(g.alt_offset)*100),
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
target_airspeed_cm,
flight_stage,
takeoff_pitch_cd,

4
ArduPlane/GCS_Mavlink.pde
View File

@ -1853,7 +1853,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) {
guided_WP.alt += ahrs.get_home().alt;
guided_WP.alt += home.alt;
}
set_mode(GUIDED);
@ -1872,7 +1872,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// add home alt if needed
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
tell_command.alt += ahrs.get_home().alt;
tell_command.alt += home.alt;
}
next_WP.alt = tell_command.alt;

16
ArduPlane/commands.pde
View File

@ -17,8 +17,8 @@ static void update_auto()
if (g.command_index >= g.command_total) {
handle_no_commands();
if(g.command_total == 0) {
next_WP.lat = ahrs.get_home().lat + 10;
next_WP.lng = ahrs.get_home().lng + 10;
next_WP.lat = home.lat + 10;
next_WP.lng = home.lng + 10;
}
} else {
if(g.command_index != 0) {
@ -82,7 +82,7 @@ static struct Location get_cmd_with_index(int16_t i)
if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
(temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
(temp.lat != 0 || temp.lng != 0 || temp.alt != 0)) {
temp.alt += ahrs.get_home().alt;
temp.alt += home.alt;
}
return temp;
@ -125,7 +125,7 @@ static int32_t read_alt_to_hold()
if (g.RTL_altitude_cm < 0) {
return current_loc.alt;
}
return g.RTL_altitude_cm + ahrs.get_home().alt;
return g.RTL_altitude_cm + home.alt;
}
@ -222,19 +222,19 @@ static void init_home()
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
home_is_set = true;
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)ahrs.get_home().alt);
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(ahrs.get_home(), 0);
set_cmd_with_index(home, 0);
// Save prev loc
// -------------
next_WP = prev_WP = ahrs.get_home();
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = ahrs.get_home();
guided_WP = home;
guided_WP.alt += g.RTL_altitude_cm;
}

8
ArduPlane/commands_logic.pde
View File

@ -152,7 +152,7 @@ static void handle_process_do_command()
static void handle_no_commands()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = rally_find_best_location(current_loc, ahrs.get_home());
next_nav_command = rally_find_best_location(current_loc, home);
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
@ -236,7 +236,7 @@ static void do_RTL(void)
{
control_mode = RTL;
prev_WP = current_loc;
next_WP = rally_find_best_location(current_loc, ahrs.get_home());
next_WP = rally_find_best_location(current_loc, home);
if (g.loiter_radius < 0) {
loiter.direction = -1;
@ -256,8 +256,8 @@ static void do_takeoff()
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch_cd = (int)next_nav_command.p1 * 100;
takeoff_altitude_cm = next_nav_command.alt;
next_WP.lat = ahrs.get_home().lat + 10;
next_WP.lng = ahrs.get_home().lng + 10;
next_WP.lat = home.lat + 10;
next_WP.lng = home.lng + 10;
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable
}

10
ArduPlane/geofence.pde
View File

@ -154,7 +154,7 @@ static bool geofence_check_minalt(void)
if (g.fence_minalt == 0) {
return false;
}
return (adjusted_altitude_cm() < (g.fence_minalt*100.0) + ahrs.get_home().alt);
return (adjusted_altitude_cm() < (g.fence_minalt*100.0) + home.alt);
}
/*
@ -168,7 +168,7 @@ static bool geofence_check_maxalt(void)
if (g.fence_maxalt == 0) {
return false;
}
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0) + ahrs.get_home().alt);
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0) + home.alt);
}
@ -268,14 +268,14 @@ static void geofence_check(bool altitude_check_only)
case FENCE_ACTION_GUIDED_THR_PASS:
if (g.fence_retalt > 0) {
//fly to the return point using fence_retalt
guided_WP.alt = ahrs.get_home().alt + 100.0*g.fence_retalt;
guided_WP.alt = home.alt + 100.0*g.fence_retalt;
} else if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP.alt = ahrs.get_home().alt + g.RTL_altitude_cm;
guided_WP.alt = home.alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP.alt = ahrs.get_home().alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP.id = 0;
guided_WP.p1 = 0;

10
ArduPlane/navigation.pde
View File

@ -218,8 +218,8 @@ static void update_fbwb_speed_height(void)
}
// check for FBWB altitude limit
if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < ahrs.get_home().alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = ahrs.get_home().alt + g.FBWB_min_altitude_cm;
if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
}
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
@ -256,7 +256,7 @@ static void setup_glide_slope(void)
case AUTO:
if (prev_WP.id != MAV_CMD_NAV_TAKEOFF &&
prev_WP.alt != ahrs.get_home().alt &&
prev_WP.alt != home.alt &&
(next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
offset_altitude_cm = next_WP.alt - prev_WP.alt;
} else {
@ -274,7 +274,7 @@ static void setup_glide_slope(void)
*/
static float relative_altitude(void)
{
return (current_loc.alt - ahrs.get_home().alt) * 0.01f;
return (current_loc.alt - home.alt) * 0.01f;
}
/*
@ -282,6 +282,6 @@ static float relative_altitude(void)
*/
static int32_t relative_altitude_abs_cm(void)
{
return labs(current_loc.alt - ahrs.get_home().alt);
return labs(current_loc.alt - home.alt);
}

4
ArduPlane/rally.pde
View File

@ -86,9 +86,9 @@ static Location rally_find_best_location(const Location &myloc, const Location &
{
RallyLocation ral_loc = {};
Location ret = {};
if (find_best_rally_point(myloc, ahrs.get_home(), ral_loc)) {
if (find_best_rally_point(myloc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
ret = rally_location_to_location(ral_loc, ahrs.get_home());
ret = rally_location_to_location(ral_loc, home);
} else {
ret = homeloc;
// Altitude to hold over home