Merge branch 'ArduPilot:master' into master

ArduPlane/Plane.h

@@ -1159,7 +1159,7 @@ class Plane : public AP_Vehicle {
     void servos_twin_engine_mix();
     void force_flare();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
-    void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func);
+    void throttle_slew_limit();
     bool suppress_throttle(void);
     void update_throttle_hover();
     void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,

ArduPlane/servos.cpp

@@ -22,7 +22,7 @@
 /*****************************************
 * Throttle slew limit
 *****************************************/
-void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
+void Plane::throttle_slew_limit()
 {
 #if HAL_QUADPLANE_ENABLED
     const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode());
@@ -32,7 +32,9 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
 
     if (!do_throttle_slew) {
         // only do throttle slew limiting in modes where throttle control is automatic
-        SRV_Channels::set_slew_rate(func, 0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, 0.0, 100, G_Dt);
         return;
     }
 
@@ -55,7 +57,9 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
         slewrate = g.takeoff_throttle_slewrate;
     }
 #endif
-    SRV_Channels::set_slew_rate(func, slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, slewrate, 100, G_Dt);
 }
 
 /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
@@ -793,8 +797,6 @@ void Plane::servos_twin_engine_mix(void)
     } else {
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left);
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right);
-        throttle_slew_limit(SRV_Channel::k_throttleLeft);
-        throttle_slew_limit(SRV_Channel::k_throttleRight);
     }
 }
 
@@ -913,7 +915,7 @@ void Plane::set_servos(void)
     airbrake_update();
 
     // slew rate limit throttle
-    throttle_slew_limit(SRV_Channel::k_throttle);
+    throttle_slew_limit();
 
     int8_t min_throttle = 0;
 #if AP_ICENGINE_ENABLED

ArduPlane/tailsitter.cpp

@@ -289,9 +289,6 @@ void Tailsitter::output(void)
         return;
     }
 
-    float tilt_left = 0.0f;
-    float tilt_right = 0.0f;
-
     // throttle 0 to 1
     float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
 
@@ -341,6 +338,10 @@ void Tailsitter::output(void)
             // set AP_MotorsMatrix throttles for forward flight
             motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt);
 
+            // No tilt output unless forward gain is set
+            float tilt_left = 0.0;
+            float tilt_right = 0.0;
+
             // in forward flight: set motor tilt servos and throttles using FW controller
             if (vectored_forward_gain > 0) {
                 // remove scaling from surface speed scaling and apply throttle scaling
@@ -398,8 +399,11 @@ void Tailsitter::output(void)
             }
 
             // output tilt motors
-            tilt_left = 0.0f;
-            tilt_right = 0.0f;
+
+            // No output unless hover gain is set
+            float tilt_left = 0.0;
+            float tilt_right = 0.0;
+
             if (vectored_hover_gain > 0) {
                 const float hover_throttle = motors->get_throttle_hover();
                 const float output_throttle = motors->get_throttle();
@@ -438,8 +442,10 @@ void Tailsitter::output(void)
         tailsitter_motors->set_min_throttle(0.0);
     }
 
-    tilt_left = 0.0f;
-    tilt_right = 0.0f;
+    // No tilt output unless hover gain is set
+    float tilt_left = 0.0;
+    float tilt_right = 0.0;
+
     if (vectored_hover_gain > 0) {
         // thrust vectoring VTOL modes
         tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);

Tools/AP_Bootloader/board_types.txt

@@ -405,6 +405,12 @@ AP_HW_DroneBuild_G2                  5701
 
 # IDs 6000-6099 reserved for SpektreWorks
 
+# IDs 6100-6109 reserved for MFE
+AP_HW_MFE_PDB_CAN                            6100
+AP_HW_MFE_POS3_CAN                          6101
+AP_HW_MFE_RTK_CAN                             6102
+AP_HW_MFE_AirSpeed_CAN                     6103
+
 # IDs 6600-6699 reserved for Eagle Eye Drones
 
 # IDs 6900-6909 reserved for Easy Aerial

Tools/AP_Periph/AP_Periph.cpp

@@ -627,6 +627,7 @@ void AP_Periph_FW::prepare_reboot()
  */
 void AP_Periph_FW::reboot(bool hold_in_bootloader)
 {
+    prepare_reboot();
     hal.scheduler->reboot(hold_in_bootloader);
 }
 

Tools/GIT_Test/GIT_Success.txt

@@ -205,3 +205,4 @@ Naveen Kumar Kilaparthi
 Amr Attia
 Alessandro Martini
 Eren Mert YİĞİT
+Prashant Powar

Tools/ardupilotwaf/boards.py

@@ -696,6 +696,12 @@ def configure_env(self, cfg, env):
         cfg.define('AP_NOTIFY_LP5562_BUS', 2)
         cfg.define('AP_NOTIFY_LP5562_ADDR', 0x30)
 
+        # turn on fencepoint and rallypoint protocols so they're still tested:
+        env.CXXFLAGS.extend([
+            '-DAP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED=HAL_GCS_ENABLED&&HAL_RALLY_ENABLED',
+            '-DAC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT=HAL_GCS_ENABLED&&AP_FENCE_ENABLED'
+        ])
+
         try:
             env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0')
         except ValueError:

Tools/autotest/arducopter.py

@@ -9111,8 +9111,6 @@ def GPSBlending(self):
         '''Test GPS Blending'''
         '''ensure we get dataflash log messages for blended instance'''
 
-        self.context_push()
-
         # configure:
         self.set_parameters({
             "WP_YAW_BEHAVIOR": 0,  # do not yaw
@@ -9165,8 +9163,8 @@ def GPSBlending(self):
                         raise NotAchievedException("Blended diverged")
                 current_ts = None
 
-        self.context_pop()
-        self.reboot_sitl()
+        if len(measurements) != 3:
+            raise NotAchievedException("Did not see three GPS measurements!")
 
     def GPSWeightedBlending(self):
         '''Test GPS Weighted Blending'''

libraries/AC_Fence/AC_Fence_config.h

@@ -17,5 +17,5 @@
 // ArduPilot 4.7 stops compiling them in
 // ArduPilot 4.8 removes the code entirely
 #ifndef AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
-#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT HAL_GCS_ENABLED && AP_FENCE_ENABLED
+#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT 0
 #endif

libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp

@@ -13,21 +13,15 @@ extern const AP_HAL::HAL& hal;
 AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[];
 HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry;
 
-void AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("airspeed_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, driver_index) != nullptr)
 #if AP_AIRSPEED_HYGROMETER_ENABLE
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("hygrometer_sub");
-    }
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, driver_index) != nullptr)
 #endif
+    ;
 }
 
 AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)

libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h

@@ -26,7 +26,7 @@ class AP_Airspeed_DroneCAN : public AP_Airspeed_Backend {
     bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) override;
 #endif
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     static AP_Airspeed_Backend* probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid);
 

libraries/AP_Arming/AP_Arming.cpp

@@ -582,11 +582,11 @@ bool AP_Arming::compass_checks(bool report)
                              (double)MAX(fabsf(diff_mgauss.x), (double)fabsf(diff_mgauss.y)), (int)magfield_error_threshold);
                 return false;
             }
-            if (fabsf(diff_mgauss.x) > magfield_error_threshold*2.0) {
+            if (fabsf(diff_mgauss.z) > magfield_error_threshold*2.0) {
                 check_failed(ARMING_CHECK_COMPASS, report, "Check mag field (z diff:%.0f>%d)",
                              (double)fabsf(diff_mgauss.z), (int)magfield_error_threshold*2);
                 return false;
-            }           
+            }
         }
 #endif  // AP_AHRS_ENABLED
     }

libraries/AP_Baro/AP_Baro_DroneCAN.cpp

@@ -21,18 +21,13 @@ AP_Baro_DroneCAN::AP_Baro_DroneCAN(AP_Baro &baro) :
     AP_Baro_Backend(baro)
 {}
 
-void AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("pressure_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("temperature_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr)
+    ;
 }
 
 AP_Baro_Backend* AP_Baro_DroneCAN::probe(AP_Baro &baro)

libraries/AP_Baro/AP_Baro_DroneCAN.h

@@ -15,7 +15,7 @@ class AP_Baro_DroneCAN : public AP_Baro_Backend {
 
     void update() override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_Baro_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new);
     static AP_Baro_Backend* probe(AP_Baro &baro);
 

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp

@@ -42,23 +42,14 @@ AP_BattMonitor_DroneCAN::AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMon
     _state.healthy = false;
 }
 
-void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("battinfo_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("battinfo_aux_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mppt_stream_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, driver_index) != nullptr)
+    ;
 }
 
 /*

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h

@@ -47,7 +47,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend
     // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
     uint32_t get_mavlink_fault_bitmask() const override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_BattMonitor_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id);
     static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg);
     static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg);

libraries/AP_CANManager/AP_CANDriver.h

@@ -40,5 +40,5 @@ class AP_CANDriver
     virtual bool add_11bit_driver(CANSensor *sensor) { return false; }
 
     // handler for outgoing frames for auxillary drivers
-    virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; }
+    virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { return false; }
 };

libraries/AP_CANManager/AP_CANSensor.cpp

@@ -135,7 +135,7 @@ bool CANSensor::add_interface(AP_HAL::CANIface* can_iface)
     return true;
 }
 
-bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
 {
     if (!_initialized) {
         debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized for write_frame");
@@ -171,12 +171,12 @@ void CANSensor::loop()
 #endif
 
     while (true) {
-        uint64_t timeout = AP_HAL::micros64() + LOOP_INTERVAL_US;
+        uint64_t deadline_us = AP_HAL::micros64() + LOOP_INTERVAL_US;
 
         // wait to receive frame
         bool read_select = true;
         bool write_select = false;
-        bool ret = _can_iface->select(read_select, write_select, nullptr, timeout);
+        bool ret = _can_iface->select(read_select, write_select, nullptr, deadline_us);
         if (ret && read_select) {
             uint64_t time;
             AP_HAL::CANIface::CanIOFlags flags {};

libraries/AP_CANManager/AP_CANSensor.h

@@ -43,7 +43,7 @@ class CANSensor : public AP_CANDriver {
     virtual void handle_frame(AP_HAL::CANFrame &frame) = 0;
 
     // handler for outgoing frames
-    bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
+    bool write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us);
 
 #ifdef HAL_BUILD_AP_PERIPH
     static void set_periph(const uint8_t i, const AP_CAN::Protocol protocol, AP_HAL::CANIface* iface) {

libraries/AP_Common/Location.cpp

@@ -9,12 +9,6 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Terrain/AP_Terrain.h>
 
-/// constructors
-Location::Location()
-{
-    zero();
-}
-
 const Location definitely_zero{};
 bool Location::is_zero(void) const
 {

libraries/AP_Common/Location.h

@@ -28,7 +28,7 @@ class Location
     };
 
     /// constructors
-    Location();
+    Location() { zero(); }
     Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame);
     Location(const Vector3f &ekf_offset_neu, AltFrame frame);
     Location(const Vector3d &ekf_offset_neu, AltFrame frame);

libraries/AP_Compass/AP_Compass_DroneCAN.cpp

@@ -37,24 +37,16 @@ AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devi
 {
 }
 
-void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag2_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, driver_index) != nullptr)
 #if AP_COMPASS_DRONECAN_HIRES_ENABLED
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag3_sub");
-    }
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, driver_index) != nullptr)
 #endif
+    ;
 }
 
 AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index)

libraries/AP_Compass/AP_Compass_DroneCAN.h

@@ -14,7 +14,7 @@ class AP_Compass_DroneCAN : public AP_Compass_Backend {
 
     void        read(void) override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_Compass_Backend* probe(uint8_t index);
     static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
     static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);