Merge branch 'ArduPilot:master' into impl/mavlink_highres_imu

ArduPilot · May 8, 2024 · a2e7e10 · a2e7e10
2 parents 9f0d755 + bcf0733
commit a2e7e10
Show file tree

Hide file tree

Showing 9 changed files with 37 additions and 5 deletions.
diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp
@@ -8,7 +8,7 @@ bool ModeRTL::_enter()
     }
 
     // initialise waypoint navigation library
-    g2.wp_nav.init(MAX(0, g2.rtl_speed));
+    g2.wp_nav.init(MAX(0.0f, g2.rtl_speed));
 
     // set target to the closest rally point or home
 #if HAL_RALLY_ENABLED

diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt
@@ -296,6 +296,8 @@ AP_HW_RadiolinkPIX6                  1410
 
 AP_HW_JHEMCU-H743HD                  1411
 
+AP_HW_LongbowF405                    1422
+
 AP_HW_SakuraRC-H743                  2714
 
 # IDs 5000-5099 reserved for Carbonix

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
@@ -208,7 +208,7 @@ void AP_Periph_FW::init()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#if (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) && (HAL_USE_I2C == TRUE)
     const bool pins_enabled = ChibiOS::I2CBus::check_select_pins(0x01);
     if (pins_enabled) {
         ChibiOS::I2CBus::set_bus_to_floating(0);

diff --git a/Tools/AP_Periph/rangefinder.cpp b/Tools/AP_Periph/rangefinder.cpp
@@ -39,6 +39,7 @@ void AP_Periph_FW::can_rangefinder_update(void)
         // limit to max rate
         return;
     }
+    last_rangefinder_update_ms = now;
 
     // update all rangefinder instances
     rangefinder.update();
@@ -114,7 +115,6 @@ void AP_Periph_FW::can_rangefinder_update(void)
                         &buffer[0],
                         total_size);
 
-        last_rangefinder_update_ms = now;
     }
 }
 

diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
@@ -525,6 +525,27 @@ def DriveRTL(self, timeout=120):
         self.disarm_vehicle()
         self.progress("RTL Mission OK (%fm)" % home_distance)
 
+    def RTL_SPEED(self, timeout=120):
+        '''Test RTL_SPEED is honoured'''
+
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 300, 0, 0),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 0),
+        ])
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        self.change_mode('AUTO')
+        self.wait_current_waypoint(2, timeout=120)
+        for speed in 1, 5.5, 1.5, 7.5:
+            self.set_parameter("RTL_SPEED", speed)
+            self.change_mode('RTL')
+            self.wait_groundspeed(speed-0.1, speed+0.1, minimum_duration=10)
+            self.change_mode('HOLD')
+        self.do_RTL()
+        self.disarm_vehicle()
+
     def AC_Avoidance(self):
         '''Test AC Avoidance switch'''
         self.context_push()
@@ -6814,6 +6835,7 @@ def tests(self):
             self.MAV_CMD_BATTERY_RESET,
             self.NetworkingWebServer,
             self.NetworkingWebServerPPP,
+            self.RTL_SPEED,
         ])
         return ret
 

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef.dat
@@ -104,7 +104,7 @@ env ROMFS_UNCOMPRESSED True
 # PWM, WS2812 LED
 PA3 TIM2_CH4 TIM2 PWM(1)
 
-DMA_NOSHARE USART2*
+DMA_NOSHARE USART2* SPI1*
 
 define CAN_APP_NODE_NAME "in.sierraaerospace.TrueNavPro-G4"
 

diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp
@@ -69,14 +69,17 @@ void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
                 * it's sage to update speed
                 */
             _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+            high_speed_set = true;
             debug("Set higher SPI-frequency");
         } else {
             _dev->set_speed(AP_HAL::Device::SPEED_LOW);
+            high_speed_set = false;
             debug("Set lower SPI-frequency");
         }
         break;
     default:
         _dev->set_speed(AP_HAL::Device::SPEED_LOW);
+        high_speed_set = false;
         debug("Set lower SPI-frequency");
         debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
         break;

diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.h b/libraries/AP_HAL_Linux/SPIUARTDriver.h
@@ -11,6 +11,9 @@ class SPIUARTDriver : public UARTDriver {
     SPIUARTDriver();
     void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
     void _timer_tick(void) override;
+    uint32_t get_baud_rate() const override {
+        return high_speed_set ? 4000000U : 1000000U;
+    }
 
 protected:
     int _write_fd(const uint8_t *buf, uint16_t n) override;
@@ -23,6 +26,8 @@ class SPIUARTDriver : public UARTDriver {
     uint32_t _last_update_timestamp;
 
     bool _external;
+
+    bool high_speed_set;
 };
 
 }
diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h
@@ -56,7 +56,7 @@ class UARTDriver : public AP_HAL::UARTDriver {
 
     uint32_t bw_in_bytes_per_second() const override;
 
-    uint32_t get_baud_rate() const override { return _baudrate; }
+    virtual uint32_t get_baud_rate() const override { return _baudrate; }
 
 private:
     AP_HAL::OwnPtr<SerialDevice> _device;