AP_Scripting: added external AHRS scripting backend

Tools/autotest/ArduPlane_Tests/InertialLabsEAHRSScripting/ap1.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	50.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367333	149.163164	28.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366814	149.165878	28.000000	1
+5	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362947	149.165179	0.000000	1

Tools/autotest/arduplane.py

@@ -3168,10 +3168,11 @@ def TerrainLoiter(self):
         self.progress("Returning home")
         self.fly_home_land_and_disarm(240)
 
-    def fly_external_AHRS(self, sim, eahrs_type, mission):
+    def fly_external_AHRS(self, sim, eahrs_type, mission, extra_params=None, script=None):
         """Fly with external AHRS (VectorNav)"""
         self.customise_SITL_commandline(["--serial4=sim:%s" % sim])
 
+        self.context_push()
         self.set_parameters({
             "EAHRS_TYPE": eahrs_type,
             "SERIAL4_PROTOCOL": 36,
@@ -3180,7 +3181,20 @@ def fly_external_AHRS(self, sim, eahrs_type, mission):
             "AHRS_EKF_TYPE": 11,
             "INS_GYR_CAL": 1,
         })
+        if script is not None:
+            # optionally install a driver script
+            self.install_driver_script_context(script)
+            self.set_parameters({
+                "SCR_ENABLE" : 1,
+                "SCR_VM_I_COUNT" : 1000000,
+            })
         self.reboot_sitl()
+        if extra_params is not None:
+            # optional extra parameters
+            self.set_parameters(extra_params)
+            self.reboot_sitl()
+            # set again for ones that need to be after reboot
+            self.set_parameters(extra_params)
         self.delay_sim_time(5)
         self.progress("Running accelcal")
         self.run_cmd(
@@ -3192,6 +3206,7 @@ def fly_external_AHRS(self, sim, eahrs_type, mission):
         self.wait_ready_to_arm()
         self.arm_vehicle()
         self.fly_mission(mission)
+        self.context_pop()
 
     def wait_and_maintain_wind_estimate(
             self,
@@ -3291,7 +3306,18 @@ def MicroStrainEAHRS7(self):
 
     def InertialLabsEAHRS(self):
         '''Test InertialLabs EAHRS support'''
-        self.fly_external_AHRS("ILabs", 5, "ap1.txt")
+        self.fly_external_AHRS("ILabs", 5, "ap1.txt",
+                               extra_params={'SIM_SPEEDUP': 10})
+
+    def InertialLabsEAHRSScripting(self):
+        '''Test InertialLabs EAHRS support with scripting'''
+        self.fly_external_AHRS("ILabs", 10, "ap1.txt",
+                               extra_params={"ILABS_ENABLE": 1,
+                                             'SERIAL4_PROTOCOL': 28,
+                                             'SIM_SPEEDUP': 1,
+                                             'EAHRS_SENSORS': 13,
+                                             'ARSPD_TYPE': 16},
+                               script="EAHRS_InertialLabs.lua")
 
     def get_accelvec(self, m):
         return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81
@@ -5408,6 +5434,7 @@ def tests(self):
             self.MicroStrainEAHRS5,
             self.MicroStrainEAHRS7,
             self.InertialLabsEAHRS,
+            self.InertialLabsEAHRSScripting,
             self.Deadreckoning,
             self.DeadreckoningNoAirSpeed,
             self.EKFlaneswitch,

Tools/autotest/vehicle_test_suite.py

@@ -4580,6 +4580,12 @@ def install_applet_script_context(self, scriptname):
         self.install_applet_script(scriptname)
         self.context_get().installed_scripts.append(scriptname)
 
+    def install_driver_script_context(self, scriptname):
+        '''installs a driver script which will be removed when the context goes
+        away'''
+        self.install_driver_script(scriptname)
+        self.context_get().installed_scripts.append(scriptname)
+
     def rootdir(self):
         this_dir = os.path.dirname(__file__)
         return os.path.realpath(os.path.join(this_dir, "../.."))
@@ -8126,6 +8132,9 @@ def script_test_source_path(self, scriptname):
     def script_applet_source_path(self, scriptname):
         return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "applets", scriptname)
 
+    def script_drivers_source_path(self, scriptname):
+        return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "drivers", scriptname)
+
     def installed_script_path(self, scriptname):
         return os.path.join("scripts", os.path.basename(scriptname))
 
@@ -8166,6 +8175,10 @@ def install_applet_script(self, scriptname, install_name=None):
         source = self.script_applet_source_path(scriptname)
         self.install_script(source, scriptname, install_name=install_name)
 
+    def install_driver_script(self, scriptname, install_name=None):
+        source = self.script_drivers_source_path(scriptname)
+        self.install_script(source, scriptname, install_name=install_name)
+
     def remove_installed_script(self, scriptname):
         dest = self.installed_script_path(os.path.basename(scriptname))
         try:

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -672,6 +672,8 @@ void AP_Airspeed::read(uint8_t i)
 // read all airspeed sensors
 void AP_Airspeed::update()
 {
+    WITH_SEMAPHORE(sem);
+
     if (!lib_enabled()) {
         return;
     }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -225,8 +225,14 @@ class AP_Airspeed
     // get aggregate calibration state for the Airspeed library:
     CalibrationState get_calibration_state() const;
 
+    // allow threads to lock against update
+    HAL_Semaphore &get_semaphore(void) {
+        return sem;
+    }
+
 private:
     static AP_Airspeed *_singleton;
+    HAL_Semaphore sem;
 
     AP_Int8 _enable;
     bool lib_enabled() const;

libraries/AP_Baro/AP_Baro_Backend.cpp

@@ -99,3 +99,16 @@ bool AP_Baro_Backend::pressure_ok(float press)
     }
     return ret;
 }
+
+/*
+  set an instance as calibrated
+ */
+void AP_Baro_Backend::set_calibrated(uint8_t instance, float pressure)
+{
+    if (!_frontend.sensors[instance].calibrated) {
+        // ahrs external baro may not update at boot, so mark
+        // calibrated on first reading
+        _frontend.sensors[instance].ground_pressure.set_and_save(pressure);
+        _frontend.sensors[instance].calibrated = true;
+    }
+}

libraries/AP_Baro/AP_Baro_Backend.h

@@ -80,4 +80,9 @@ class AP_Baro_Backend
     void set_bus_id(uint8_t instance, uint32_t id) {
         _frontend.sensors[instance].bus_id.set(int32_t(id));
     }
+
+    /*
+      set an instance as calibrated
+    */
+    void set_calibrated(uint8_t instance, float pressure);
 };

libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp

@@ -15,6 +15,9 @@ void AP_Baro_ExternalAHRS::update(void)
     if (count) {
         WITH_SEMAPHORE(_sem);
         _copy_to_frontend(instance, sum_pressure/count, sum_temp/count);
+
+        set_calibrated(instance, sum_pressure/count);
+
         sum_pressure = sum_temp = 0;
         count = 0;
     }

libraries/AP_Compass/AP_Compass.cpp

@@ -1717,6 +1717,7 @@ Compass::read(void)
     if (!available()) {
         return false;
     }
+    WITH_SEMAPHORE(sem);
 
 #if HAL_LOGGING_ENABLED
     const bool old_healthy = healthy();

libraries/AP_Compass/AP_Compass.h

@@ -365,8 +365,14 @@ friend class AP_Compass_Backend;
     // get the first compass marked for use by COMPASSx_USE
     uint8_t get_first_usable(void) const { return _first_usable; }
 
+    // allow threads to lock against update
+    HAL_Semaphore &get_semaphore(void) {
+        return sem;
+    }
+
 private:
     static Compass *_singleton;
+    HAL_Semaphore sem;
 
     // Use Priority and StateIndex typesafe index types
     // to distinguish between two different type of indexing

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -26,6 +26,7 @@
 #include "AP_ExternalAHRS_MicroStrain5.h"
 #include "AP_ExternalAHRS_MicroStrain7.h"
 #include "AP_ExternalAHRS_InertialLabs.h"
+#include "AP_ExternalAHRS_Scripting.h"
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -129,6 +130,12 @@ void AP_ExternalAHRS::init(void)
         return;
 #endif
 
+#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED
+    case DevType::Scripting:
+        backend = new AP_ExternalAHRS_Scripting(this, state);
+        return;
+#endif
+
     }
 
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype));
@@ -278,7 +285,7 @@ bool AP_ExternalAHRS::get_accel(Vector3f &accel)
 void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const
 {
     if (backend) {
-        backend->send_status_report(link);
+        backend->send_EKF_status_report(link);
     }
 }
 
@@ -319,21 +326,26 @@ void AP_ExternalAHRS::update(void)
         // @Field: Lon: longitude
         // @Field: Alt: altitude AMSL
         // @Field: Flg: nav status flags
+        // @Field: VVar: velocity variance
+        // @Field: PVar: position variance
+        // @Field: HVar: height variance
 
         float roll, pitch, yaw;
         state.quat.to_euler(roll, pitch, yaw);
         nav_filter_status filterStatus {};
         get_filter_status(filterStatus);
 
-        AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg",
-                                    "sdddnnnDUm-",
-                                    "F000000GG0-",
-                                    "QffffffLLfI",
+        AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg,VVar,PVar,HVar",
+                                    "sdddnnnDUm-nmm",
+                                    "F000000GG0-000",
+                                    "QffffffLLfIfff",
                                     AP_HAL::micros64(),
                                     degrees(roll), degrees(pitch), degrees(yaw),
                                     state.velocity.x, state.velocity.y, state.velocity.z,
                                     state.location.lat, state.location.lng, state.location.alt*0.01,
-                                    filterStatus.value);
+                                    filterStatus.value,
+                                    state.velocity_variance, state.pos_horiz_variance,
+                                    state.pos_vert_variance);
     }
 #endif  // HAL_LOGGING_ENABLED
 }
@@ -347,6 +359,12 @@ const char* AP_ExternalAHRS::get_name() const
     return nullptr;
 }
 
+// handle input from scripting backend
+bool AP_ExternalAHRS::handle_scripting(const state_t &_state, nav_filter_status_flags_t &_filter_status)
+{
+    return backend->handle_scripting(_state, _filter_status);
+}
+
 namespace AP {
 
 AP_ExternalAHRS &externalAHRS()

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -51,6 +51,9 @@ class AP_ExternalAHRS {
 #endif
 #if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED
         InertialLabs = 5,
+#endif
+#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED
+        Scripting = 10,
 #endif
         // 3 reserved for AdNav
         // 4 reserved for CINS
@@ -100,6 +103,14 @@ class AP_ExternalAHRS {
         bool have_location;
         bool have_velocity;
 
+        // variances for EKF_STATUS_REPORT
+        float velocity_variance;
+        float pos_horiz_variance;
+        float pos_vert_variance;
+        float compass_variance;
+        float terrain_alt_variance;
+        float airspeed_variance;
+
         uint32_t last_location_update_us;
     } state;
 
@@ -122,6 +133,9 @@ class AP_ExternalAHRS {
     // update backend
     void update();
 
+    // handle input from scripting backend
+    bool handle_scripting(const state_t &state, nav_filter_status_flags_t &filter_status);
+
     /*
       structures passed to other subsystems
      */

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp

@@ -622,7 +622,7 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status)
     const uint32_t dt_limit_gps = 500;
     memset(&status, 0, sizeof(status));
     const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0;
-    status.flags.initalized = init_ok;
+    status.flags.initialized = init_ok;
     status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok;
     status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit);
     status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit);
@@ -637,59 +637,15 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status)
         (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 &&
         (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0;
     status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL);
-}
-
-// send an EKF_STATUS message to GCS
-void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const
-{
-    // prepare flags
-    uint16_t flags = 0;
-    nav_filter_status filterStatus;
-    get_filter_status(filterStatus);
-    if (filterStatus.flags.attitude) {
-        flags |= EKF_ATTITUDE;
-    }
-    if (filterStatus.flags.horiz_vel) {
-        flags |= EKF_VELOCITY_HORIZ;
-    }
-    if (filterStatus.flags.vert_vel) {
-        flags |= EKF_VELOCITY_VERT;
-    }
-    if (filterStatus.flags.horiz_pos_rel) {
-        flags |= EKF_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.horiz_pos_abs) {
-        flags |= EKF_POS_HORIZ_ABS;
-    }
-    if (filterStatus.flags.vert_pos) {
-        flags |= EKF_POS_VERT_ABS;
-    }
-    if (filterStatus.flags.terrain_alt) {
-        flags |= EKF_POS_VERT_AGL;
-    }
-    if (filterStatus.flags.const_pos_mode) {
-        flags |= EKF_CONST_POS_MODE;
-    }
-    if (filterStatus.flags.pred_horiz_pos_rel) {
-        flags |= EKF_PRED_POS_HORIZ_REL;
-    }
-    if (filterStatus.flags.pred_horiz_pos_abs) {
-        flags |= EKF_PRED_POS_HORIZ_ABS;
-    }
-    if (!filterStatus.flags.initalized) {
-        flags |= EKF_UNINITIALIZED;
-    }
 
-    // send message
     const float vel_gate = 5;
     const float pos_gate = 5;
     const float hgt_gate = 5;
     const float mag_var = 0;
-    mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
-                                       state2.kf_vel_covariance.length()/vel_gate,
-                                       state2.kf_pos_covariance.xy().length()/pos_gate,
-                                       state2.kf_pos_covariance.z/hgt_gate,
-                                       mag_var, 0, 0);
+    state.velocity_variance = state2.kf_vel_covariance.length()/vel_gate;
+    state.pos_horiz_variance = state2.kf_pos_covariance.xy().length()/pos_gate;
+    state.pos_vert_variance = state2.kf_pos_covariance.z/hgt_gate;
+    state.compass_variance = mag_var;
 }
 
 #endif  // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h

@@ -37,7 +37,6 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
     bool initialised(void) const override;
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
     void get_filter_status(nav_filter_status &status) const override;
-    void send_status_report(class GCS_MAVLINK &link) const override;
 
     // check for new data
     void update() override {