Plane: updates for AHRS holding home position

ArduPlane/ArduPlane.pde

@@ -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
+// Flag for if we have g_gps lock and have set the home location in AHRS
-static struct   Location home;
-// Flag for if we have g_gps lock and have set the home location
 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
+    barometer.read();
-    //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;
-    }
 
     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,

ArduPlane/GCS_Mavlink.pde

@@ -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;

ArduPlane/Log.pde

@@ -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),                 

ArduPlane/commands.pde

@@ -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.lat             = ahrs.get_home().lat + 10;
-            next_WP.lng             = home.lng + 1000;                  // so we don't have bad calcs
+            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;
+    ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
-    home.lng        = g_gps->longitude;                                 // Lon * 10**7
-    home.lat        = g_gps->latitude;                                  // Lat * 10**7
-    home.alt        = max(g_gps->altitude_cm, 0);
     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
+    ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
-    home.lat        = g_gps->latitude;                                  // Lat * 10**7
-    home.alt        = max(g_gps->altitude_cm, 0);
     barometer.update_calibration();
 }

ArduPlane/commands_logic.pde

@@ -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.lat             = ahrs.get_home().lat + 10;
-    next_WP.lng             = home.lng + 1000;          // so we don't have bad calcs
+    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;
+        ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
-        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);
         home_is_set = true;
     }
 }

ArduPlane/geofence.pde

@@ -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;

ArduPlane/navigation.pde

@@ -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) {
+    if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < ahrs.get_home().alt + g.FBWB_min_altitude_cm) {
-        target_altitude_cm = 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);
 }
 

ArduPlane/rally.pde

@@ -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

ArduPlane/sensors.pde

@@ -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

ArduPlane/system.pde

@@ -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();
 }

ArduPlane/test.pde

@@ -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());
         }