Merge branch 'master' into plane-fixNudge

AntennaTracker/GCS_Mavlink.cpp

@@ -161,12 +161,6 @@ void GCS_MAVLINK_Tracker::send_pid_tuning()
     }
 }
 
-bool GCS_MAVLINK_Tracker::handle_guided_request(AP_Mission::Mission_Command&)
-{
-    // do nothing
-    return false;
-}
-
 /*
   default stream rates to 1Hz
  */

AntennaTracker/GCS_Mavlink.h

@@ -43,7 +43,6 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK
     void handle_message_manual_control(const mavlink_message_t &msg);
     void handle_message_global_position_int(const mavlink_message_t &msg);
     void handle_message_scaled_pressure(const mavlink_message_t &msg);
-    bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_set_attitude_target(const mavlink_message_t &msg);
 
     void send_global_position_int() override;

AntennaTracker/system.cpp

@@ -5,10 +5,6 @@ static const StorageAccess wp_storage(StorageManager::StorageMission);
 
 void Tracker::init_ardupilot()
 {
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    can_mgr.init();
-#endif
-
     // initialise notify
     notify.init();
     AP_Notify::flags.pre_arm_check = true;

ArduCopter/system.cpp

@@ -15,10 +15,6 @@ static void failsafe_check_static()
 
 void Copter::init_ardupilot()
 {
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    can_mgr.init();
-#endif
-
     // init cargo gripper
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();

ArduPlane/mode.cpp

@@ -259,6 +259,21 @@ void Mode::output_rudder_and_steering(float val)
     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val);
 }
 
+// Output pilot throttle, this is used in stabilized modes without auto throttle control
+// Direct mapping if THR_PASS_STAB is set
+// Otherwise apply curve for trim correction if configured
+void Mode::output_pilot_throttle()
+{
+    if (plane.g.throttle_passthru_stabilize) {
+        // THR_PASS_STAB set, direct mapping
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
+        return;
+    }
+
+    // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true));
+}
+
 // true if throttle min/max limits should be applied
 bool Mode::use_throttle_limits() const
 {

ArduPlane/mode.h

@@ -155,6 +155,9 @@ class Mode
     // Helper to output to both k_rudder and k_steering servo functions
     void output_rudder_and_steering(float val);
 
+    // Output pilot throttle, this is used in stabilized modes without auto throttle control
+    void output_pilot_throttle();
+
 #if HAL_QUADPLANE_ENABLED
     // References for convenience, used by QModes
     AC_PosControl*& pos_control;
@@ -262,6 +265,8 @@ class ModeAutoTune : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    void run() override;
+
 protected:
 
     bool _enter() override;
@@ -493,6 +498,8 @@ class ModeFBWA : public Mode
 
     bool mode_allows_autotuning() const override { return true; }
 
+    void run() override;
+
 };
 
 class ModeFBWB : public Mode

ArduPlane/mode_acro.cpp

@@ -26,6 +26,8 @@ void ModeAcro::update()
 
 void ModeAcro::run()
 {
+    output_pilot_throttle();
+
     if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 &&
         plane.yawController.rate_control_enabled()) {
         // we can do 3D acro locking

ArduPlane/mode_autotune.cpp

@@ -14,3 +14,10 @@ void ModeAutoTune::update()
     plane.mode_fbwa.update();
 }
 
+void ModeAutoTune::run()
+{
+    // Run base class function and then output throttle
+    Mode::run();
+
+    output_pilot_throttle();
+}

ArduPlane/mode_fbwa.cpp

@@ -35,3 +35,11 @@ void ModeFBWA::update()
         }
     }
 }
+
+void ModeFBWA::run()
+{
+    // Run base class function and then output throttle
+    Mode::run();
+
+    output_pilot_throttle();
+}

ArduPlane/mode_stabilize.cpp

@@ -13,4 +13,6 @@ void ModeStabilize::run()
     plane.stabilize_pitch();
     stabilize_stick_mixing_direct();
     plane.stabilize_yaw();
+
+    output_pilot_throttle();
 }

ArduPlane/mode_training.cpp

@@ -66,4 +66,6 @@ void ModeTraining::run()
     // Always manual rudder control
     output_rudder_and_steering(plane.rudder_in_expo(false));
 
+    output_pilot_throttle();
+
 }

ArduPlane/servos.cpp

@@ -585,23 +585,8 @@ void Plane::set_throttle(void)
 #if AP_SCRIPTING_ENABLED
     if (nav_scripting_active()) {
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
-    } else
-#endif
-           if (control_mode == &mode_stabilize ||
-               control_mode == &mode_training ||
-               control_mode == &mode_acro ||
-               control_mode == &mode_fbwa ||
-               control_mode == &mode_autotune) {
-        // a manual throttle mode
-        if (g.throttle_passthru_stabilize) {
-            // manual pass through of throttle while in FBWA or
-            // STABILIZE mode with THR_PASS_STAB set
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
-        } else {
-            // get throttle, but adjust center to output TRIM_THROTTLE if flight option set
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_adjusted_throttle_input(true));
-        }
     }
+#endif
 
     if (control_mode->use_battery_compensation()) {
         // Apply voltage compensation to throttle output from flight mode

ArduPlane/system.cpp

@@ -19,10 +19,6 @@ void Plane::init_ardupilot()
 
     ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
 
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    can_mgr.init();
-#endif
-
     rollController.convert_pid();
     pitchController.convert_pid();
 

ArduSub/system.cpp

@@ -14,10 +14,6 @@ static void failsafe_check_static()
 
 void Sub::init_ardupilot()
 {
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    can_mgr.init();
-#endif
-
     // init cargo gripper
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();

Blimp/GCS_Mavlink.cpp

@@ -407,15 +407,6 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 };
 
-bool GCS_MAVLINK_Blimp::handle_guided_request(AP_Mission::Mission_Command &cmd)
-{
-    // #if MODE_AUTO_ENABLED == ENABLED
-    //     // return blimp.mode_auto.do_guided(cmd);
-    // #else
-    return false;
-    // #endif
-}
-
 void GCS_MAVLINK_Blimp::packetReceived(const mavlink_status_t &status,
                                        const mavlink_message_t &msg)
 {

Blimp/GCS_Mavlink.h

@@ -53,7 +53,6 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK
 private:
 
     void handle_message(const mavlink_message_t &msg) override;
-    bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     bool try_send_message(enum ap_message id) override;
 
     void packetReceived(const mavlink_status_t &status,

Rover/system.cpp

@@ -14,10 +14,6 @@ static void failsafe_check_static()
 
 void Rover::init_ardupilot()
 {
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    can_mgr.init();
-#endif
-
     // init gripper
 #if AP_GRIPPER_ENABLED
     g2.gripper.init();

Tools/AP_Bootloader/board_types.txt

@@ -278,7 +278,7 @@ AP_HW_VIMDRONES_PERIPH               1407
 
 AP_HW_PIXHAWK6X_PERIPH               1408
 AP_HW_CUBERED_PERIPH                 1409
-
+AP_HW_RadiolinkPIX6                  1410
 
 # IDs 5000-5099 reserved for Carbonix
 # IDs 5100-5199 reserved for SYPAQ Systems

Tools/ardupilotwaf/ardupilotwaf.py

@@ -238,6 +238,7 @@ def ap_get_all_libraries(bld):
             continue
         libraries.append(name)
     libraries.extend(['AP_HAL', 'AP_HAL_Empty'])
+    libraries.append('AP_PiccoloCAN/piccolo_protocol')
     return libraries
 
 @conf

Tools/autotest/logger_metadata/enum_parse.py

@@ -92,6 +92,10 @@ def match_enum_line(self, line):
             raise ValueError("Failed to match (%s)" % line)
 
     def enumerations_from_file(self, source_file):
+        def debug(x):
+            pass
+        # if source_file == "/home/pbarker/rc/ardupilot/libraries/AP_HAL/AnalogIn.h":
+        #     debug = print
         state_outside = "outside"
         state_inside = "inside"
 
@@ -103,6 +107,7 @@ def enumerations_from_file(self, source_file):
             in_class = None
             while True:
                 line = f.readline()
+                #  debug(f"{state} line: {line}")
                 if line == "":
                     break
                 line = line.rstrip()
@@ -113,7 +118,7 @@ def enumerations_from_file(self, source_file):
                     if re.match("class .*;", line) is not None:
                         # forward-declaration of a class
                         continue
-                    m = re.match("class *(\w+)", line)
+                    m = re.match("class *([:\w]+)", line)
                     if m is not None:
                         in_class = m.group(1)
                         continue
@@ -125,6 +130,7 @@ def enumerations_from_file(self, source_file):
                     if m is not None:
                         # all one one line!  Thanks!
                         enum_name = m.group(2)
+                        debug("ol: %s: %s" % (source_file, enum_name))
                         entries_string = m.group(3)
                         entry_names = [x.strip() for x in entries_string.split(",")]
                         count = 0
@@ -139,7 +145,7 @@ def enumerations_from_file(self, source_file):
                     m = re.match(".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{", line)
                     if m is not None:
                         enum_name = m.group(2)
-                        # print("%s: %s" % (source_file, enum_name))
+                        debug("%s: %s" % (source_file, enum_name))
                         entries = []
                         last_value = None
                         state = state_inside
@@ -172,7 +178,7 @@ def enumerations_from_file(self, source_file):
                     if name is None:
                         skip_enumeration = True
                         continue
-                    # print(" name=(%s) value=(%s) comment=(%s)\n" % (name, value, comment))
+                    debug(" name=(%s) value=(%s) comment=(%s)\n" % (name, value, comment))
                     if value is None:
                         if last_value is None:
                             value = 0
@@ -190,6 +196,7 @@ def enumerations_from_file(self, source_file):
     class Enumeration(object):
 
         def __init__(self, name, entries):
+            # print("creating enum %s" % name)
             self.name = name
             self.entries = entries
 
@@ -246,4 +253,4 @@ def run(self):
         sys.exit(1)
 
     s.run()
-    print("Enumerations: %s" % s.enumerations)
+#    print("Enumerations: %s" % s.enumerations)

Tools/autotest/logger_metadata/parse.py

@@ -325,15 +325,20 @@ def search_messagedef_start(self,line,prevmessagedef=""):
 
     def parse_file(self, filepath):
         with open(filepath) as f:
-            #            print("Opened (%s)" % filepath)
+#            print("Opened (%s)" % filepath)
             lines = f.readlines()
             f.close()
+        def debug(x):
+            pass
+#        if filepath == "/home/pbarker/rc/ardupilot/libraries/AP_HAL/AnalogIn.h":
+#            debug = print
         state_outside = "outside"
         state_inside = "inside"
         messagedef = ""
         state = state_outside
         docco = None
         for line in lines:
+            debug(f"{state}: {line}")
             if messagedef:
                 messagedef = messagedef + line
                 if "}" in line or ";" in line:

Tools/scripts/build_options.py

@@ -160,6 +160,8 @@ def __init__(self,
     Feature('Compass', 'QMC5883L', 'AP_COMPASS_QMC5883L_ENABLED', 'Enable QMC5883L compasses', 1, None),
     Feature('Compass', 'RM3100', 'AP_COMPASS_RM3100_ENABLED', 'Enable RM3100 compasses', 1, None),
     Feature('Compass', 'DRONECAN_COMPASS', 'AP_COMPASS_DRONECAN_ENABLED', 'Enable DroneCAN compasses', 0, None),
+    Feature('Compass', 'FixedYawCal', 'AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'Enable Fixed-Yaw Compass Calibration', 1, None),  # noqa
+    Feature('Compass', 'CompassLearn', 'COMPASS_LEARN_ENABLED', 'Enable In-Flight Compass Learning', 1, "FixedYawCal"),
 
     Feature('Gimbal', 'MOUNT', 'HAL_MOUNT_ENABLED', 'Enable Mount', 0, None),
     Feature('Gimbal', 'ALEXMOS', 'HAL_MOUNT_ALEXMOS_ENABLED', 'Enable Alexmos Gimbal', 0, "MOUNT"),

Tools/scripts/extract_features.py

@@ -242,6 +242,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'),
             ('HAL_BUTTON_ENABLED', 'AP_Button::update'),
             ('HAL_LOGGING_ENABLED', 'AP_Logger::Init'),
+            ('AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'AP_Compass::mag_cal_fixed_yaw'),
+            ('COMPASS_LEARN_ENABLED', 'CompassLearn::update'),
         ]
 
     def progress(self, msg):

libraries/AP_Arming/AP_Arming.cpp

@@ -1244,6 +1244,7 @@ bool AP_Arming::can_checks(bool report)
                 case AP_CAN::Protocol::TOFSenseP:
                 case AP_CAN::Protocol::NanoRadar_NRA24:
                 case AP_CAN::Protocol::Benewake:
+                case AP_CAN::Protocol::MR72:
                 {
                     for (uint8_t j = i; j; j--) {
                         if (AP::can().get_driver_type(i) == AP::can().get_driver_type(j-1)) {

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -1,4 +1,9 @@
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_ENABLED
+
 #include "AP_BattMonitor.h"
+
 #include "AP_BattMonitor_Analog.h"
 #include "AP_BattMonitor_SMBus.h"
 #include "AP_BattMonitor_SMBus_Solo.h"
@@ -1117,3 +1122,5 @@ AP_BattMonitor &battery()
 }
 
 };
+
+#endif  // AP_BATTERY_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -1,12 +1,15 @@
 #pragma once
 
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_ENABLED
+
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include "AP_BattMonitor_Params.h"
-#include "AP_BattMonitor_config.h"
 
 // maximum number of battery monitors
 #ifndef AP_BATT_MONITOR_MAX_INSTANCES
@@ -315,3 +318,5 @@ class AP_BattMonitor
 namespace AP {
     AP_BattMonitor &battery();
 };
+
+#endif  // AP_BATTERY_ENABLED