Merge branch 'master' into gpsid_detect

ArduCopter/mode_guided.cpp

@@ -456,6 +456,13 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         return true;
     }
 
+    // set position target and zero velocity and acceleration
+    Vector3f pos_target_f;
+    bool terrain_alt;
+    if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
+        return false;
+    }
+
     // if configured to use position controller for position control
     // ensure we are in position control mode
     if (guided_mode != SubMode::Pos) {
@@ -465,13 +472,6 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
     // set yaw state
     set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
-    // set position target and zero velocity and acceleration
-    Vector3f pos_target_f;
-    bool terrain_alt;
-    if (!wp_nav->get_vector_NEU(dest_loc, pos_target_f, terrain_alt)) {
-        return false;
-    }
-
     // initialise terrain following if needed
     if (terrain_alt) {
         // get current alt above terrain

Tools/AP_Bootloader/board_types.txt

@@ -316,6 +316,15 @@ AP_HW_AIRVOLUTE_DCS2                 5200
 AP_HW_AOCODA-RC-H743DUAL             5210
 AP_HW_AOCODA-RC-F405V3               5211
 
+# IDs 5220-5239 reserved for UAV-DEV GmbH
+AP_HW_UAV-DEV-HAT-H7                 5220
+AP_HW_UAV-DEV-NucPilot-H7            5221
+AP_HW_UAV-DEV-M10S-L4                5222
+AP_HW_UAV-DEV-F9P-G4                 5223
+AP_HW_UAV-DEV-UM982-G4               5224
+AP_HW_UAV-DEV-M20D-G4                5225
+AP_HW_UAV-DEV-Sensorboard-G4         5226
+
 #IDs 5301-5399 reserved for Sierra Aerospace
 AP_HW_Sierra-TrueNavPro-G4           5301
 AP_HW_Sierra-TrueNavIC               5302

Tools/autotest/arducopter.py

@@ -4599,10 +4599,7 @@ def fly_guided_stop(self,
                     m.climb < climb_tolerance):
                 break
 
-    def fly_guided_move_global_relative_alt(self, lat, lon, alt):
-        startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
-                                       blocking=True)
-
+    def send_set_position_target_global_int(self, lat, lon, alt):
         self.mav.mav.set_position_target_global_int_send(
             0, # timestamp
             1, # target system_id
@@ -4622,6 +4619,12 @@ def fly_guided_move_global_relative_alt(self, lat, lon, alt):
             0, # yawrate
         )
 
+    def fly_guided_move_global_relative_alt(self, lat, lon, alt):
+        startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
+                                       blocking=True)
+
+        self.send_set_position_target_global_int(lat, lon, alt)
+
         tstart = self.get_sim_time()
         while True:
             if self.get_sim_time_cached() - tstart > 200:
@@ -11345,6 +11348,35 @@ def check_altitude(mav, m):
         self.context_pop()
         self.reboot_sitl(force=True)
 
+    def GuidedForceArm(self):
+        '''ensure Guided acts appropriately when force-armed'''
+        self.set_parameters({
+            "EK3_SRC2_VELXY": 5,
+            "SIM_GPS_DISABLE": 1,
+        })
+        self.load_default_params_file("copter-optflow.parm")
+        self.reboot_sitl()
+        self.delay_sim_time(30)
+        self.change_mode('GUIDED')
+        self.arm_vehicle(force=True)
+        self.takeoff(20, mode='GUIDED')
+        location = self.offset_location_ne(self.sim_location(), metres_north=0, metres_east=-300)
+        self.progress("Ensure we don't move for 10 seconds")
+        tstart = self.get_sim_time()
+        startpos = self.sim_location_int()
+        while True:
+            now = self.get_sim_time_cached()
+            if now - tstart > 10:
+                break
+            self.send_set_position_target_global_int(int(location.lat*1e7), int(location.lng*1e7), 10)
+            dist = self.get_distance_int(startpos, self.sim_location_int())
+            if dist > 10:
+                raise NotAchievedException("Wandered too far from start position")
+            self.delay_sim_time(1)
+
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def tests2b(self):  # this block currently around 9.5mins here
         '''return list of all tests'''
         ret = ([
@@ -11431,6 +11463,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.AutoRTL,
             self.EK3_OGN_HGT_MASK,
             self.FarOrigin,
+            self.GuidedForceArm,
         ])
         return ret
 

libraries/AP_Compass/AP_Compass_IST8308.cpp

@@ -200,10 +200,6 @@ void AP_Compass_IST8308::timer()
         return;
     }
 
-    if (stat & STAT1_VAL_DOR) {
-        printf("IST8308: data overrun\n");
-    }
-
     if (!_dev->read_registers(DATAX_L_REG, (uint8_t *) &buffer,
                               sizeof(buffer))) {
         return;

libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp

@@ -5,6 +5,7 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_RPM/AP_RPM.h>
+#include <AP_RCTelemetry/AP_RCTelemetry.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -44,47 +45,15 @@ void AP_Frsky_Backend::loop(void)
     }
 }
 
-/*
- * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
- * for FrSky D and SPort protocols
- */
-float AP_Frsky_Backend::get_vspeed_ms(void)
-{
-
-    {
-        // release semaphore as soon as possible
-        AP_AHRS &_ahrs = AP::ahrs();
-        Vector3f v;
-        WITH_SEMAPHORE(_ahrs.get_semaphore());
-        if (_ahrs.get_velocity_NED(v)) {
-            return -v.z;
-        }
-    }
-
-    auto &_baro = AP::baro();
-    WITH_SEMAPHORE(_baro.get_semaphore());
-    return _baro.get_climb_rate();
-}
-
 /*
  * prepare altitude between vehicle and home location data
  * for FrSky D and SPort protocols
  */
 void AP_Frsky_Backend::calc_nav_alt(void)
 {
-    _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s
-
-    Location loc;
-    float current_height = 0;
-
-    AP_AHRS &_ahrs = AP::ahrs();
-    WITH_SEMAPHORE(_ahrs.get_semaphore());
-    if (_ahrs.get_location(loc)) {
-        if (!loc.get_alt_m(Location::AltFrame::ABSOLUTE, current_height)) {
-            // ignore this error
-        }
-    }
+    _SPort_data.vario_vspd = (int32_t)(AP_RCTelemetry::get_vspeed_ms()*100); //convert to cm/s
 
+    float current_height = AP_RCTelemetry::get_nav_alt_m();
     _SPort_data.alt_nav_meters = float_to_uint16(current_height);
     _SPort_data.alt_nav_cm = float_to_uint16((current_height - _SPort_data.alt_nav_meters) * 100);
 }

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -646,7 +646,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void)
  */
 uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
 {
-    float vspd = get_vspeed_ms();
+    float vspd = AP_RCTelemetry::get_vspeed_ms();
     // vertical velocity in dm/s
     uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1);
     float airspeed_m;       // m/s

libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro/hwdef.dat

@@ -98,9 +98,8 @@ define GPS_MAX_RECEIVERS 1
 define GPS_MAX_INSTANCES 1
 define HAL_COMPASS_MAX_SENSORS 1
 define HAL_PERIPH_GPS_PORT_DEFAULT 0
-
-# minimal GPS drivers to reduce flash usage
-include ../include/minimal_GPS.inc
+define AP_GPS_BACKEND_DEFAULT_ENABLED 0
+define AP_GPS_UBLOX_ENABLED 1
 
 # GPS PPS
 PA15 GPS_PPS_IN INPUT

libraries/AP_Mount/AP_Mount.cpp

@@ -62,6 +62,9 @@ void AP_Mount::init()
     // primary is reset to the first instantiated mount
     bool primary_set = false;
 
+    // keep track of number of serial instances for initialisation
+    uint8_t serial_instance = 0;
+
     // create each instance
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         switch (get_mount_type(instance)) {
@@ -98,8 +101,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_STORM32SERIAL_ENABLED
         // check for SToRM32 mounts using serial protocol
         case Type::SToRM32_serial:
-            _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif
 
@@ -122,8 +126,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_SIYI_ENABLED
         // check for Siyi gimbal
         case Type::Siyi:
-            _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif // HAL_MOUNT_SIYI_ENABLED
 
@@ -146,8 +151,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_VIEWPRO_ENABLED
         // check for Xacti gimbal
         case Type::Viewpro:
-            _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif // HAL_MOUNT_VIEWPRO_ENABLED
         }
@@ -1015,7 +1021,10 @@ void AP_Mount::convert_params()
     // convert MNT_TYPE to MNT1_TYPE
     int8_t mnt_type = 0;
     IGNORE_RETURN(AP_Param::get_param_by_index(this, 19, AP_PARAM_INT8, &mnt_type));
-    if (mnt_type > 0) {
+    if (mnt_type == 0) {
+        // if the mount was not previously set, no need to perform the upgrade logic
+        return;
+    } else if (mnt_type > 0) {
         int8_t stab_roll = 0;
         int8_t stab_pitch = 0;
         IGNORE_RETURN(AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &stab_roll));
@@ -1025,8 +1034,9 @@ void AP_Mount::convert_params()
             // conversion is still done even if HAL_MOUNT_SERVO_ENABLED is false
             mnt_type = 7;  // (int8_t)Type::BrushlessPWM;
         }
+        // if the mount was previously set, then we need to save the upgraded mount type
+        _params[0].type.set_and_save(mnt_type);
     }
-    _params[0].type.set_and_save(mnt_type);
 
     // convert MNT_JSTICK_SPD to MNT1_RC_RATE
     int8_t jstick_spd = 0;

libraries/AP_Mount/AP_Mount_Backend_Serial.cpp

@@ -0,0 +1,24 @@
+#include "AP_Mount_Backend_Serial.h"
+
+#if HAL_MOUNT_ENABLED
+#include <AP_SerialManager/AP_SerialManager.h>
+
+// Default init function for every mount
+void AP_Mount_Backend_Serial::init()
+{
+    const AP_SerialManager& serial_manager = AP::serialmanager();
+
+    // search for serial port.  hild classes should check that uart is not nullptr
+    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, _serial_instance);
+    if (_uart == nullptr) {
+        return;
+    }
+
+    // initialised successfully if uart is found
+    _initialised = true;
+
+    // call the parent class init
+    AP_Mount_Backend::init();
+}
+
+#endif // HAL_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_Backend_Serial.h

@@ -0,0 +1,48 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  Mount driver backend class for serial drivers
+  Mounts using custom serial protocols should be derived from this class
+ */
+#pragma once
+
+#include "AP_Mount_config.h"
+
+#if HAL_MOUNT_ENABLED
+
+#include "AP_Mount_Backend.h"
+
+class AP_Mount_Backend_Serial : public AP_Mount_Backend
+{
+public:
+    // Constructor
+    AP_Mount_Backend_Serial(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance, uint8_t serial_instance) :
+        AP_Mount_Backend(frontend, params, instance),
+        _serial_instance(serial_instance)
+    {}
+
+    // perform any required initialisation for this instance
+    void init() override;
+
+protected:
+
+    // internal variables
+    AP_HAL::UARTDriver *_uart;      // uart connected to gimbal
+    uint8_t _serial_instance;       // this instance's serial instance number
+    bool _initialised;              // true if uart has been initialised
+};
+
+#endif // HAL_MOUNT_ENABLED