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

Plane: updates for AHRS holding home position

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-02 22:06:25 +11:00
parent 392995ef84
commit 11337ab2df
11 changed files with 36 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/ArduPlane.pde
View File

@ -622,9 +622,7 @@ static int16_t condition_rate;
// 3D Location vectors
// Location structure defined in AP_Common
////////////////////////////////////////////////////////////////////////////////
// The home location used for RTL. The location is set when we first get stable GPS lock
static struct Location home;
// Flag for if we have g_gps lock and have set the home location
// 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
static struct Location prev_WP;
@ -1042,7 +1040,7 @@ static void update_GPS_50Hz(void)
static void update_GPS_10Hz(void)
{
// get position from AHRS
have_position = ahrs.get_projected_position(current_loc);
have_position = ahrs.get_position(current_loc);
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
g_gps->new_data = false;
@ -1361,17 +1359,7 @@ static void update_navigation()
static void update_alt()
{
// this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO;
if (barometer.healthy) {
// alt_MSL centimeters (centimeters)
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude_cm;
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
} else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {
// alt_MSL centimeters (centimeters)
current_loc.alt = g_gps->altitude_cm;
}
barometer.read();
geofence_check(true);
@ -1389,7 +1377,7 @@ static void update_alt()
}
}
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - ahrs.get_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 += home.alt;
guided_WP.alt += ahrs.get_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 += home.alt;
tell_command.alt += ahrs.get_home().alt;
}
next_WP.alt = tell_command.alt;

6
ArduPlane/Log.pde
View File

@ -320,8 +320,6 @@ struct PACKED log_Performance {
uint32_t loop_time;
uint16_t main_loop_count;
uint32_t g_dt_max;
uint8_t renorm_count;
uint8_t renorm_blowup;
int16_t gyro_drift_x;
int16_t gyro_drift_y;
int16_t gyro_drift_z;
@ -337,8 +335,6 @@ static void Log_Write_Performance()
loop_time : millis() - perf_mon_timer,
main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max,
renorm_count : ahrs.renorm_range_count,
renorm_blowup : ahrs.renorm_blowup_count,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000),
gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000),
@ -683,7 +679,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),
"ATT", "IccCCC", "TimeMS,Roll,Pitch,Yaw,ErrorRP,ErrorYaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IHIBBhhhBH", "LTime,MLC,gDt,RNCnt,RNBl,GDx,GDy,GDz,I2CErr,INSErr" },
"PM", "IHIhhhBH", "LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),

25
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 = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
next_WP.lat = ahrs.get_home().lat + 10;
next_WP.lng = ahrs.get_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 += home.alt;
temp.alt += ahrs.get_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 + home.alt;
return g.RTL_altitude_cm + ahrs.get_home().alt;
}
@ -219,25 +219,22 @@ static void init_home()
#endif
}
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude_cm, 0);
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)home.alt);
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)ahrs.get_home().alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
set_cmd_with_index(ahrs.get_home(), 0);
// Save prev loc
// -------------
next_WP = prev_WP = home;
next_WP = prev_WP = ahrs.get_home();
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP = ahrs.get_home();
guided_WP.alt += g.RTL_altitude_cm;
}
@ -248,8 +245,6 @@ static void init_home()
*/
static void update_home()
{
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude_cm, 0);
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
barometer.update_calibration();
}

13
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, home);
next_nav_command = rally_find_best_location(current_loc, ahrs.get_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, home);
next_WP = rally_find_best_location(current_loc, ahrs.get_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 = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
next_WP.lat = ahrs.get_home().lat + 10;
next_WP.lng = ahrs.get_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
}
@ -608,10 +608,7 @@ static void do_set_home()
if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_nonnav_command.lng; // Lon * 10**7
home.lat = next_nonnav_command.lat; // Lat * 10**7
home.alt = max(next_nonnav_command.alt, 0);
ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
home_is_set = true;
}
}

12
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) + home.alt);
return (adjusted_altitude_cm() < (g.fence_minalt*100.0) + ahrs.get_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) + home.alt);
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0) + ahrs.get_home().alt);
}
@ -214,7 +214,7 @@ static void geofence_check(bool altitude_check_only)
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only && ahrs.get_projected_position(loc)) {
} else if (!altitude_check_only && ahrs.get_position(loc)) {
Vector2l location;
location.x = loc.lat;
location.y = loc.lng;
@ -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 = home.alt + 100.0*g.fence_retalt;
guided_WP.alt = ahrs.get_home().alt + 100.0*g.fence_retalt;
} else if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP.alt = home.alt + g.RTL_altitude_cm;
guided_WP.alt = ahrs.get_home().alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
guided_WP.alt = ahrs.get_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 < home.alt + g.FBWB_min_altitude_cm) {
target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
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;
}
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 != home.alt &&
prev_WP.alt != ahrs.get_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 - home.alt) * 0.01f;
return (current_loc.alt - ahrs.get_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 - home.alt);
return labs(current_loc.alt - ahrs.get_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, home, ral_loc)) {
if (find_best_rally_point(myloc, ahrs.get_home(), ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
ret = rally_location_to_location(ral_loc, home);
ret = rally_location_to_location(ral_loc, ahrs.get_home());
} else {
ret = homeloc;
// Altitude to hold over home

18
ArduPlane/sensors.pde
View File

@ -1,31 +1,13 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// filter altitude from the barometer with a low pass filter
static LowPassFilterInt32 altitude_filter;
static void init_barometer(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
barometer.calibrate();
// filter at 100ms sampling, with 0.7Hz cutoff frequency
altitude_filter.set_cutoff_frequency(0.1, 0.7);
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
}
// read the barometer and return the updated altitude in centimeters
// above the calibration altitude
static int32_t read_barometer(void)
{
barometer.read();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
return altitude_filter.apply(barometer.get_altitude() * 100.0);
}
static void init_sonar(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1

2
ArduPlane/system.pde
View File

@ -483,8 +483,6 @@ static void update_notify()
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
}

5
ArduPlane/test.pde
View File

@ -584,19 +584,16 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
home.alt = 0;
wp_distance = 0;
init_barometer();
while(1) {
delay(100);
current_loc.alt = read_barometer() + home.alt;
if (!barometer.healthy) {
cliSerial->println_P(PSTR("not healthy"));
} else {
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
current_loc.alt / 100.0,
barometer.get_altitude(),
barometer.get_pressure(),
barometer.get_temperature());
}