ArduPlane: Silent Arrow split of drop-july-2023

ArduPlane/AP_Arming.cpp

@@ -323,6 +323,20 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
 
     send_arm_disarm_statustext("Throttle armed");
 
+    plane.auto_state.arming_time_ms = AP_HAL::millis();
+    plane.auto_state.emergency_land = false;
+    plane.auto_state.land_alt_amsl = -1;
+
+    if (plane.gps.status() < AP_GPS::GPS_OK_FIX_3D) {
+        AP_Mission::Mission_Command cmd;
+        uint16_t idx;
+        if (plane.mission.find_command(MAV_CMD_DO_LAND_START, 0, idx, cmd) &&
+            cmd.content.location.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Init location to DO_LAND_START");
+            plane.ahrs.init_posvel(plane.aparm.airspeed_cruise, cmd.content.location);
+        }
+    }
+
     return true;
 }
 

ArduPlane/ArduPlane.cpp

@@ -433,8 +433,15 @@ void Plane::airspeed_ratio_update(void)
  */
 void Plane::update_GPS_50Hz(void)
 {
+    const auto old_status = gps.status();
     gps.update();
 
+    const auto status = gps.status();
+    if (old_status < AP_GPS::GPS_OK_FIX_3D && status >= AP_GPS::GPS_OK_FIX_3D) {
+        // remember when we got a 3D fix
+        auto_state.started_3D_fix_ms = AP_HAL::millis();
+    }
+
     update_current_loc();
 }
 
@@ -596,6 +603,16 @@ void Plane::update_alt()
             tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
         }
 
+        /*
+          project forward the desired height
+         */
+        float groundspeed = sqrtf(sq(vel.x)+sq(vel.y));
+        float desired_climb_m = (next_WP_loc.alt - current_loc.alt) * 0.01;
+        float wp_time = auto_state.wp_distance / MAX(groundspeed,1);
+        float desired_climb_rate = desired_climb_m / MAX(wp_time,0.1);
+        float projection_time = 5;
+        tecs_target_alt_cm = tecs_target_alt_cm + desired_climb_rate*projection_time*100;
+
         TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
                                                  target_airspeed_cm,
                                                  flight_stage,
@@ -625,7 +642,7 @@ void Plane::update_flight_stage(void)
             if (auto_state.takeoff_complete == false) {
                 set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
                 return;
-            } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
+            } else if (in_auto_land()) {
                 if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
                     // abort mode is sticky, it must complete while executing NAV_LAND
                     set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
@@ -857,6 +874,75 @@ bool Plane::get_target_location(Location& target_loc)
     return false;
 }
 
+/*
+  check if we should be in auto land (fixed wing) mode. Allows for
+  land mid-waypoint based on rangefinder data
+ */
+bool Plane::in_auto_land(void)
+{
+    if (control_mode != &mode_auto) {
+        auto_state.started_landing = false;
+        return false;
+    }
+    if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
+        return true;
+    }
+    if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_WAYPOINT) {
+        return false;
+    }
+    if (!(g2.flight_options & FlightOptions::AUTO_LAND_ALT)) {
+        auto_state.started_landing = false;
+        return false;
+    }
+
+    if (!hal.util->get_soft_armed() ||
+        (AP_HAL::millis() - auto_state.arming_time_ms < 20000)) {
+        // don't do emergency landing when not armed for at least 20s
+        return false;
+    }
+
+    // const float rangefinder_last_change = fabsf(rangefinder_state.prev_distance - rangefinder_state.last_distance);
+    // if (g.rangefinder_landing && rangefinder_state.in_range &&
+    //     rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good &&
+    //     rangefinder_state.height_estimate < landing.get_preflare_alt() &&
+    //     smoothed_airspeed >= aparm.airspeed_min &&
+    //     rangefinder_last_change > 0 &&
+    //     rangefinder_last_change < 10) {
+    //     auto_state.emergency_land = true;
+    // }
+
+    // also trigger emergency landing by GPS alt above NAV_LAND
+    if (is_equal(auto_state.land_alt_amsl,-1.0f)) {
+        AP_Mission::Mission_Command cmd;
+        uint16_t idx;
+        int32_t alt_amsl_cm;
+        if (mission.find_command(MAV_CMD_NAV_LAND, 0, idx, cmd) &&
+            cmd.content.location.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm)) {
+            auto_state.land_alt_amsl = alt_amsl_cm * 0.01;
+        }
+    }
+
+    // const float gps_error_margin = 20;
+    // if (!is_equal(auto_state.land_alt_amsl,-1.0f) &&
+    //     gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
+    //     AP_HAL::millis() - auto_state.started_3D_fix_ms > 10000 &&
+    //     gps.location().alt*0.01 < auto_state.land_alt_amsl + (landing.get_preflare_alt()-gps_error_margin)) {
+    //     auto_state.emergency_land = true;
+    // }
+
+
+    // check if we have dropped below preflare height while flying, if
+    // so then start landing immediately
+    if (auto_state.started_landing || auto_state.emergency_land) {
+        if (!auto_state.started_landing) {
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Emergency Land: %.1fm", rangefinder_state.height_estimate);
+            auto_state.started_landing = true;
+        }
+        return true;
+    }
+    return false;
+}
+
 /*
   update_target_location() works in all auto navigation modes
  */
@@ -990,3 +1076,4 @@ void Plane::precland_update(void)
 #endif
 
 AP_HAL_MAIN_CALLBACKS(&plane);
+

ArduPlane/Attitude.cpp

@@ -120,6 +120,19 @@ void Plane::stabilize_roll()
         if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
     }
 
+    if (!fly_inverted() && !righting_mode && labs(ahrs.roll_sensor) > 9500) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Auto righting start roll=%d pitch=%d",
+                        int(ahrs.roll_sensor/100),
+                        int(ahrs.pitch_sensor/100));
+        righting_mode = true;
+    }
+    if (righting_mode && labs(ahrs.roll_sensor) < 6000) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Auto righting done roll=%d pitch=%d",
+                        int(ahrs.roll_sensor/100),
+                        int(ahrs.pitch_sensor/100));
+        righting_mode = false;
+    }
+
     const float roll_out = stabilize_roll_get_roll_out();
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
 }
@@ -226,6 +239,11 @@ float Plane::stabilize_pitch_get_pitch_out()
         throttle_at_zero()) {
         demanded_pitch = landing.get_pitch_cd();
     }
+
+    if (righting_mode) {
+        // try to put nose down 45 degrees
+        nav_pitch_cd = demanded_pitch = -4500;
+    }
 
     return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator,
                                          ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
@@ -601,6 +619,10 @@ void Plane::calc_nav_pitch()
 void Plane::calc_nav_roll()
 {
     int32_t commanded_roll = nav_controller->nav_roll_cd();
+    if (g2.lim_roll_auto > 0) {
+        commanded_roll = constrain_int32(commanded_roll, -g2.lim_roll_auto*100, g2.lim_roll_auto*100);
+    }
+
     nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
     update_load_factor();
 }
@@ -614,6 +636,7 @@ void Plane::calc_nav_roll()
 void Plane::adjust_nav_pitch_throttle(void)
 {
     int8_t throttle = throttle_percentage();
+    throttle = MIN(throttle, aparm.throttle_max);
     if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_FixedWing::FlightStage::VTOL) {
         float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
         nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;

ArduPlane/Parameters.cpp

@@ -1113,6 +1113,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Bitmask: 12: Enable FBWB style loiter altitude control
     // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces
     // @Bitmask: 14: In AUTO - climb to next waypoint altitude immediately instead of linear climb
+    // @Bitmask: 15: Auto land at low alt
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
 
@@ -1274,6 +1275,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(precland, "PLND_", 35, ParametersG2, AC_PrecLand),
 #endif
 
+    // @Param: LIM_ROLL_AUTO
+    // @DisplayName: Roll limit for AUTO modes
+    // @Description: This is the limit for roll when under auto control. If zero then LIM_ROLL_CD is used
+    // @Range: 0 80
+    // @Increment: 1
+    // @User: Advanced
+    // @Units: Degrees
+    AP_GROUPINFO("LIM_ROLL_AUTO", 36, ParametersG2, lim_roll_auto, 0),
+
     AP_GROUPEND
 };
 

ArduPlane/Parameters.h

@@ -33,7 +33,7 @@ class Parameters {
     //////////////////////////////////////////////////////////////////
     // STOP!!! DO NOT CHANGE THIS VALUE UNTIL YOU FULLY UNDERSTAND THE
     // COMMENTS ABOVE. IF UNSURE, ASK ANOTHER DEVELOPER!!!
-    static const uint16_t k_format_version = 13;
+    static const uint16_t k_format_version = 120;
     //////////////////////////////////////////////////////////////////
 
 
@@ -569,6 +569,8 @@ class ParametersG2 {
     // min initial climb in RTL
     AP_Int16        rtl_climb_min;
 
+    AP_Float        lim_roll_auto;
+
     AP_Int8         man_expo_roll;
     AP_Int8         man_expo_pitch;
     AP_Int8         man_expo_rudder;

ArduPlane/Plane.h

@@ -336,7 +336,8 @@ class Plane : public AP_Vehicle {
 
     // last time we ran roll/pitch stabilization
     uint32_t last_stabilize_ms;
-
+    bool righting_mode;
+
     // Failsafe
     struct {
         // Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold
@@ -524,6 +525,18 @@ class Plane : public AP_Vehicle {
         // how much correction have we added for terrain data
         float terrain_correction;
 
+        // highest barometric altitude seen (for ALTITUDE_WAIT)
+        float highest_baro_alt;
+
+        // have we started an emergency landing?
+        bool started_landing;
+
+        bool emergency_land;
+        float land_alt_amsl = -1;
+
+        uint32_t started_3D_fix_ms;
+        uint32_t arming_time_ms;
+
         // last home altitude for detecting changes
         int32_t last_home_alt_cm;
     } auto_state;
@@ -959,6 +972,7 @@ class Plane : public AP_Vehicle {
     void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
+    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_vtol_land(const AP_Mission::Mission_Command& cmd);
@@ -1167,6 +1181,8 @@ class Plane : public AP_Vehicle {
     void update_soaring();
 #endif
 
+    bool in_auto_land(void);
+
     // RC_Channel.cpp
     bool emergency_landing;
 

ArduPlane/altitude.cpp

@@ -553,6 +553,11 @@ float Plane::mission_alt_offset(void)
  */
 float Plane::height_above_target(void)
 {
+    if (plane.auto_state.emergency_land &&
+        !is_equal(plane.auto_state.land_alt_amsl,-1.0f)) {
+        // use emergency landing alt for target
+        return current_loc.alt*0.01 - plane.auto_state.land_alt_amsl;
+    }
     float target_alt = next_WP_loc.alt*0.01;
     if (!next_WP_loc.relative_alt) {
         target_alt -= ahrs.get_home().alt*0.01f;
@@ -726,6 +731,7 @@ void Plane::rangefinder_height_update(void)
                 gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
             }
         }
+        rangefinder_state.prev_distance = rangefinder_state.last_distance;
         rangefinder_state.last_distance = distance;
     } else {
         rangefinder_state.in_range_count = 0;

ArduPlane/commands.cpp

@@ -9,7 +9,11 @@
  */
 void Plane::set_next_WP(const Location &loc)
 {
-    if (auto_state.next_wp_crosstrack) {
+    float bearing_deg = current_loc.get_bearing_to(loc) * 0.01;
+    float yaw_deg = degrees(ahrs.get_yaw());
+    float bearing_err = fabsf(bearing_deg - yaw_deg);
+
+    if (auto_state.next_wp_crosstrack && bearing_err < 25) {
         // copy the current WP into the OldWP slot
         prev_WP_loc = next_WP_loc;
         auto_state.crosstrack = true;
@@ -121,6 +125,25 @@ bool Plane::update_home()
     if (hal.util->was_watchdog_armed()) {
         return false;
     }
+    if (g2.home_reset_threshold == -2) {
+        // special case for init using GPS height and terrain data,
+        // allowing for arming while not on the ground
+        float terrain_amsl;
+        Location loc;
+        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
+            terrain.height_amsl(current_loc, terrain_amsl, false) &&
+            ahrs.get_location(loc) &&
+            !ahrs.home_is_locked()) {
+            barometer.update_calibration();
+            ahrs.resetHeightDatum();
+            loc.alt = terrain_amsl * 100;
+            if (!AP::ahrs().set_home(loc)) {
+                // silently fail
+            }
+            return true;
+        }
+        return false;
+    }
     if ((g2.home_reset_threshold == -1) ||
         ((g2.home_reset_threshold > 0) &&
          (fabsf(barometer.get_altitude()) > g2.home_reset_threshold))) {