Merge branch 'ArduPilot:master' into stellar-h7v2

ArduCopter/Copter.cpp

@@ -458,7 +458,7 @@ AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, c
                 return nullptr;
             }
 
-            // Registration sucsessful, notify the GCS that it should re-request the avalable modes
+            // Registration successful, notify the GCS that it should re-request the available modes
             gcs().available_modes_changed();
 
             return &mode_guided_custom[i]->state;

ArduCopter/GCS_MAVLink_Copter.cpp

@@ -1785,7 +1785,7 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
 
 #if MODE_AUTO_ENABLED
     // Auto RTL is odd
-    // Have to deal with is separately becase its number and name can change depending on if were in it or not
+    // Have to deal with is separately because its number and name can change depending on if were in it or not
     if (index_zero == 0) {
         mode_number = (uint8_t)Mode::Number::AUTO_RTL;
         name = "AUTO RTL";

ArduCopter/Parameters.cpp

@@ -1133,7 +1133,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 
     // @Param: TKOFF_SLEW_TIME
     // @DisplayName: Slew time of throttle during take-off
-    // @Description: Time to slew the throttle from minimum to maximum while checking for a succsessful takeoff.
+    // @Description: Time to slew the throttle from minimum to maximum while checking for a successful takeoff.
     // @Units: s
     // @Range: 0.25 5.0
     // @User: Standard

ArduCopter/mode.h

@@ -1758,7 +1758,7 @@ class ModeSystemId : public Mode {
     float time_const_freq;      // Time at constant frequency before chirp starts
     int8_t log_subsample;       // Subsample multiple for logging.
     Vector2f target_vel;        // target velocity for position controller modes
-    Vector2f target_pos;       // target positon
+    Vector2f target_pos;       // target position
     Vector2f input_vel_last;    // last cycle input velocity
     // System ID states
     enum class SystemIDModeState {

ArduCopter/mode_poshold.cpp

@@ -495,7 +495,7 @@ void ModePosHold::run()
 // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
 void ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
 {
-    // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
+    // if raw input is large or reversing the vehicle's lean angle immediately set the filtered angle to the new raw angle
     if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
         lean_angle_filtered = lean_angle_raw;
     } else {

ArduPlane/Log.cpp

@@ -210,6 +210,7 @@ struct PACKED log_Status {
     bool is_still;
     uint8_t stage;
     bool impact;
+    bool throttle_supressed;
 };
 
 void Plane::Log_Write_Status()
@@ -225,6 +226,7 @@ void Plane::Log_Write_Status()
         ,is_still    : AP::ins().is_still()
         ,stage       : static_cast<uint8_t>(flight_stage)
         ,impact      : crash_state.impact_detected
+        ,throttle_supressed : throttle_suppressed
         };
 
     logger.WriteBlock(&pkt, sizeof(pkt));
@@ -382,8 +384,9 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: Still: True when vehicle is not moving in any axis
 // @Field: Stage: Current stage of the flight
 // @Field: Hit: True if impact is detected
+// @Field: Sup: True if throttle is suppressed
     { LOG_STATUS_MSG, sizeof(log_Status),
-      "STAT", "QBfBBBBBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit", "s--------", "F--------" , true },
+      "STAT", "QBfBBBBBBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit,Sup", "s---------", "F---------" , true },
 
 // @LoggerMessage: QTUN
 // @Description: QuadPlane vertical tuning message

ArduPlane/tiltrotor.cpp

@@ -119,7 +119,7 @@ void Tiltrotor::setup()
 
     // check if there are any permanent VTOL motors
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
-        if (motors->is_motor_enabled(i) && ((tilt_mask & (1U<<1)) == 0)) {
+        if (motors->is_motor_enabled(i) && !is_motor_tilting(i)) {
             // enabled motor not set in tilt mask
             _have_vtol_motor = true;
             break;

Tools/AP_Bootloader/board_types.txt

@@ -301,6 +301,7 @@ AP_HW_FlywooH743Pro                  1181
 AP_HW_CBU_StampH743_LC               1182
 AP_HW_NarinH7                        1183
 AP_HW_BrahmaF4                       1184
+AP_HW_X-MAV-AP-F405Mini              1185
 
 AP_HW_JFB200                         1200
 

Tools/AP_Periph/AP_Periph.cpp

@@ -265,7 +265,7 @@ void AP_Periph_FW::init()
     }
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
     pwm_hardpoint_init();
 #endif
 

Tools/AP_Periph/AP_Periph.h

@@ -48,7 +48,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 
 #if AP_PERIPH_RELAY_ENABLED
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
     #error "Relay and PWM_HARDPOINT both use hardpoint message"
 #endif
 #include <AP_Relay/AP_Relay.h>
@@ -308,7 +308,7 @@ class AP_Periph_FW {
     AP_Proximity proximity;
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
     void pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);
     void pwm_hardpoint_init();
     void pwm_hardpoint_update();

Tools/AP_Periph/Parameters.cpp

@@ -375,7 +375,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(adsb_port, "ADSB_PORT", HAL_PERIPH_ADSB_PORT_DEFAULT),
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
     // @Param: HARDPOINT_ID
     // @DisplayName: Hardpoint ID
     // @Description: Hardpoint ID

Tools/AP_Periph/Parameters.h

@@ -144,7 +144,7 @@ class Parameters {
     AP_Int8 adsb_port;
 #endif
 
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
     AP_Int16 hardpoint_id;
     AP_Int8 hardpoint_rate;
 #endif

Tools/AP_Periph/can.cpp

@@ -1945,7 +1945,7 @@ void AP_Periph_FW::can_update()
     #ifdef HAL_GPIO_PIN_SAFE_BUTTON
         can_safety_button_update();
     #endif
-    #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+    #if AP_PERIPH_PWM_HARDPOINT_ENABLED
         pwm_hardpoint_update();
     #endif
     #if AP_PERIPH_HOBBYWING_ESC_ENABLED

Tools/AP_Periph/hardpoint.cpp

@@ -1,6 +1,6 @@
 #include "AP_Periph.h"
 
-#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#if AP_PERIPH_PWM_HARDPOINT_ENABLED
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -63,4 +63,4 @@ void AP_Periph_FW::pwm_hardpoint_update()
     }
 }
 
-#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT
+#endif // AP_PERIPH_PWM_HARDPOINT_ENABLED

Tools/Replay/Replay.cpp

@@ -196,6 +196,54 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
     }
 }
 
+static const LogStructure EKF2_log_structures[] = {
+    { LOG_FORMAT_UNITS_MSG, sizeof(log_Format_Units), \
+      "FMTU", "QBNN",      "TimeUS,FmtType,UnitIds,MultIds","s---", "F---" },   \
+    LOG_STRUCTURE_FROM_NAVEKF2                  \
+};
+
+/*
+  write format and units structure format to the log for a LogStructure
+ */
+void Replay::Write_Format(const struct LogStructure &s)
+{
+    struct log_Format pkt {};
+
+    pkt.head1 = HEAD_BYTE1;
+    pkt.head2 = HEAD_BYTE2;
+    pkt.msgid = LOG_FORMAT_MSG;
+    pkt.type = s.msg_type;
+    pkt.length = s.msg_len;
+    strncpy_noterm(pkt.name, s.name, sizeof(pkt.name));
+    strncpy_noterm(pkt.format, s.format, sizeof(pkt.format));
+    strncpy_noterm(pkt.labels, s.labels, sizeof(pkt.labels));
+
+    AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
+
+    struct log_Format_Units pkt2 {};
+    pkt2.head1 = HEAD_BYTE1;
+    pkt2.head2 = HEAD_BYTE2;
+    pkt2.msgid = LOG_FORMAT_UNITS_MSG;
+    pkt2.time_us = AP_HAL::micros64();
+    pkt2.format_type = s.msg_type;
+    strncpy_noterm(pkt2.units, s.units, sizeof(pkt2.units));
+    strncpy_noterm(pkt2.multipliers, s.multipliers, sizeof(pkt2.multipliers));
+
+    AP::logger().WriteCriticalBlock(&pkt2, sizeof(pkt2));
+}
+
+/*
+  write all EKF2 formats out at the start. This allows --force-ekf2 to
+  work even when EKF2 was not compiled into the original replay log
+  firmware
+ */
+void Replay::write_EKF_formats(void)
+{
+    for (const auto &f : EKF2_log_structures) {
+        Write_Format(f);
+    }
+}
+
 void Replay::setup()
 {
     ::printf("Starting\n");
@@ -239,6 +287,10 @@ void Replay::setup()
         ::printf("open(%s): %m\n", filename);
         exit(1);
     }
+
+    if (replay_force_ekf2) {
+        write_EKF_formats();
+    }
 }
 
 void Replay::loop()

Tools/Replay/Replay.h

@@ -107,4 +107,7 @@ class Replay : public AP_HAL::HAL::Callbacks {
     bool parse_param_line(char *line, char **vname, float &value);
     void load_param_file(const char *filename);
     void usage();
+
+    void Write_Format(const struct LogStructure &s);
+    void write_EKF_formats(void);
 };

Tools/ardupilotwaf/boards.py

@@ -982,6 +982,7 @@ def configure_env(self, cfg, env):
             AP_PERIPH_RCIN_ENABLED = 0,
             AP_PERIPH_NETWORKING_ENABLED = 0,
             AP_PERIPH_NOTIFY_ENABLED = 0,
+            AP_PERIPH_PWM_HARDPOINT_ENABLED = 0,
         )
 
 class sitl_periph_gps(sitl_periph):
@@ -1016,6 +1017,7 @@ def configure_env(self, cfg, env):
             AP_PERIPH_HOBBYWING_ESC_ENABLED = 0,
             AP_PERIPH_NETWORKING_ENABLED = 0,
             AP_PERIPH_NOTIFY_ENABLED = 0,
+            AP_PERIPH_PWM_HARDPOINT_ENABLED =0,
         )
 
 class sitl_periph_battmon(sitl_periph):
@@ -1050,6 +1052,7 @@ def configure_env(self, cfg, env):
             AP_PERIPH_HOBBYWING_ESC_ENABLED = 0,
             AP_PERIPH_NETWORKING_ENABLED = 0,
             AP_PERIPH_NOTIFY_ENABLED = 0,
+            AP_PERIPH_PWM_HARDPOINT_ENABLED = 0,
         )
 
 class esp32(Board):