AP_InertialSensor: New fallback type = NONE, which successfully does nothing like an INS, but without needing a real one, like esp32 dev boards.

Good for boards that u want to boot even when they dont have an IMU attached.
Author: Charles Villard <charlesvillard10@gmail.com>
Author: Buzz <davidbuzz@gmail.com>

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -27,6 +27,7 @@
 #include "AP_InertialSensor_ADIS1647x.h"
 #include "AP_InertialSensor_ExternalAHRS.h"
 #include "AP_InertialSensor_Invensensev3.h"
+#include "AP_InertialSensor_NONE.h"
 
 /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
  * Output is on the debug console. */
@@ -1087,7 +1088,15 @@ AP_InertialSensor::detect_backends(void)
 #endif
 
     if (_backend_count == 0) {
+
+        // no real INS backends avail, lets use an empty substitute to boot ok and get to mavlink
+        #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
+        ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A));
+        #else
+        hal.console->printf("INS: unable to initialise driver\n");
+        GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver");
         AP_BoardConfig::config_error("INS: unable to initialise driver");
+        #endif
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp

@@ -0,0 +1,327 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AP_InertialSensor_NONE.h"
+#include <SITL/SITL.h>
+#include <stdio.h>
+#include <GCS_MAVLink/GCS.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
+
+
+float rand_float(void)
+{
+    return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
+}
+
+const extern AP_HAL::HAL& hal;
+
+AP_InertialSensor_NONE::AP_InertialSensor_NONE(AP_InertialSensor &imu, const uint16_t sample_rates[]) :
+    AP_InertialSensor_Backend(imu),
+    gyro_sample_hz(sample_rates[0]),
+    accel_sample_hz(sample_rates[1])
+{
+}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_NONE::detect(AP_InertialSensor &_imu, const uint16_t sample_rates[])
+{
+    AP_InertialSensor_NONE *sensor = new AP_InertialSensor_NONE(_imu, sample_rates);
+    if (sensor == nullptr) {
+
+        return nullptr;
+    }
+    if (!sensor->init_sensor()) {
+        delete sensor;
+
+        return nullptr;
+    }
+    return sensor;
+}
+
+
+bool AP_InertialSensor_NONE::init_sensor(void)
+{
+  
+    return true;
+}
+
+
+
+void AP_InertialSensor_NONE::accumulate()
+{
+    // nothing to do
+}
+
+
+// calculate a noisy noise component
+static float calculate_noise(float noise, float noise_variation) {
+    return noise * (1.0f + noise_variation * rand_float());
+}
+
+/*
+  generate an accelerometer sample
+ */
+void AP_InertialSensor_NONE::generate_accel()
+{
+    Vector3f accel_accum;
+    uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
+    for (uint8_t j = 0; j < nsamples; j++) {
+
+        // add accel bias and noise
+        //Vector3f accel_bias = Vector3f{0.01,0.01,0.01}; 
+        float xAccel = 0.01;
+        float yAccel = 0.01;
+        float zAccel = 0.01;
+
+        // minimum noise levels are 2 bits, but averaged over many
+        // samples, giving around 0.01 m/s/s
+        float accel_noise = 0.01f;
+        float noise_variation = 0.05f;
+        // this smears the individual motor peaks somewhat emulating physical motors
+        //float freq_variation = 0.12f;
+        // add in sensor noise
+        xAccel += accel_noise * rand_float();
+        yAccel += accel_noise * rand_float();
+        zAccel += accel_noise * rand_float();
+
+        bool motors_on = 1; 
+
+        // on a real 180mm copter gyro noise varies between 0.8-4 m/s/s for throttle 0.2-0.8
+        // giving a accel noise variation of 5.33 m/s/s over the full throttle range
+        if (motors_on) {
+            // add extra noise when the motors are on
+            accel_noise = 0;
+        }
+
+        // VIB_FREQ is a static vibration applied to each axis
+        const Vector3f &vibe_freq =  Vector3f{0.01,0.01,0.01};
+
+        if (vibe_freq.is_zero()) {
+            // no rpm noise, so add in background noise if any
+            xAccel += accel_noise * rand_float();
+            yAccel += accel_noise * rand_float();
+            zAccel += accel_noise * rand_float();
+        }
+
+        if (!vibe_freq.is_zero() && motors_on) {
+            xAccel += sinf(accel_time * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation);
+            yAccel += sinf(accel_time * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation);
+            zAccel += sinf(accel_time * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation);
+            accel_time += 1.0f / (accel_sample_hz * nsamples);
+        }
+
+        // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
+        if ( motors_on) {
+            for (uint8_t i = 0; i < 4; i++) {
+                float &phase = accel_motor_phase[i];
+                float motor_freq = 50;
+                float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz * nsamples);
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+                xAccel += sinf(phase) * calculate_noise(accel_noise * 0.01, noise_variation);
+                yAccel += sinf(phase) * calculate_noise(accel_noise *0.01, noise_variation);
+                zAccel += sinf(phase) * calculate_noise(accel_noise * 0.01, noise_variation);
+            }
+        }
+
+        // correct for the acceleration due to the IMU position offset and angular acceleration
+        // correct for the centripetal acceleration
+        // only apply corrections to first accelerometer
+        Vector3f pos_offset =  Vector3f{0.01,0.01,0.01};
+        if (!pos_offset.is_zero()) {
+            // calculate sensed acceleration due to lever arm effect
+            // Note: the % operator has been overloaded to provide a cross product
+            Vector3f angular_accel = Vector3f(radians(0.01), radians(0.01), radians(0.01));
+            Vector3f lever_arm_accel = angular_accel % pos_offset;
+
+            // calculate sensed acceleration due to centripetal acceleration
+            Vector3f angular_rate = Vector3f(radians(0.01), radians(0.01), radians(0.01));
+            Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
+
+            // apply corrections
+            xAccel += lever_arm_accel.x + centripetal_accel.x;
+            yAccel += lever_arm_accel.y + centripetal_accel.y;
+            zAccel += lever_arm_accel.z + centripetal_accel.z;
+        }
+
+        if (fabsf(xAccel) > 1.0e-6f) {
+            xAccel = 0.01;
+            yAccel = 0.01;
+            zAccel = 0.01;
+        }
+
+        Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
+
+        _notify_new_accel_sensor_rate_sample(accel_instance, accel);
+
+        accel_accum += accel;
+    }
+
+    accel_accum /= nsamples;
+    _rotate_and_correct_accel(accel_instance, accel_accum);
+    _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64());
+
+    _publish_temperature(accel_instance, 23);
+}
+
+/*
+  generate a gyro sample
+ */
+void AP_InertialSensor_NONE::generate_gyro()
+{
+    Vector3f gyro_accum;
+    uint8_t nsamples = enable_fast_sampling(gyro_instance) ? 8 : 1;
+
+    for (uint8_t j = 0; j < nsamples; j++) {
+        float p = radians(0.01) + gyro_drift();
+        float q = radians(0.01) + gyro_drift();
+        float r = radians(0.01) + gyro_drift();
+
+        // minimum gyro noise is less than 1 bit
+        float gyro_noise = ToRad(0.04f);
+        float noise_variation = 0.05f;
+        // this smears the individual motor peaks somewhat emulating physical motors
+        float freq_variation = 0.12f;
+        // add in sensor noise
+        p += gyro_noise * rand_float();
+        q += gyro_noise * rand_float();
+        r += gyro_noise * rand_float();
+
+        bool motors_on = 1;
+        // on a real 180mm copter gyro noise varies between 0.2-0.4 rad/s for throttle 0.2-0.8
+        // giving a gyro noise variation of 0.33 rad/s or 20deg/s over the full throttle range
+        if (motors_on) {
+            // add extra noise when the motors are on
+            gyro_noise = ToRad(0.01) * 0.01;
+        }
+
+        // VIB_FREQ is a static vibration applied to each axis
+        const Vector3f &vibe_freq = Vector3f{0.01,0.01,0.01};
+
+        if ( vibe_freq.is_zero() ) {
+            // no rpm noise, so add in background noise if any
+            p += gyro_noise * rand_float();
+            q += gyro_noise * rand_float();
+            r += gyro_noise * rand_float();
+        }
+
+        if (!vibe_freq.is_zero() && motors_on) {
+            p += sinf(gyro_time * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation);
+            q += sinf(gyro_time * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation);
+            r += sinf(gyro_time * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation);
+            gyro_time += 1.0f / (gyro_sample_hz * nsamples);
+        }
+
+        // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
+        if ( motors_on) {
+            for (uint8_t i = 0; i < 4; i++) {
+                float motor_freq = calculate_noise(0.01 / 60.0f, freq_variation);
+                float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
+                float &phase = gyro_motor_phase[i];
+                phase += phase_incr;
+                if (phase_incr > M_PI) {
+                    phase -= 2 * M_PI;
+                }
+                else if (phase_incr < -M_PI) {
+                    phase += 2 * M_PI;
+                }
+                p += sinf(phase) * calculate_noise(gyro_noise * 0.01, noise_variation);
+                q += sinf(phase) * calculate_noise(gyro_noise * 0.01, noise_variation);
+                r += sinf(phase) * calculate_noise(gyro_noise * 0.01, noise_variation);
+            }
+        }
+
+        Vector3f gyro = Vector3f(p, q, r);
+
+        // add in gyro scaling
+        Vector3f scale = Vector3f{0.01,0.01,0.01};
+        gyro.x *= (1 + scale.x * 0.01f);
+        gyro.y *= (1 + scale.y * 0.01f);
+        gyro.z *= (1 + scale.z * 0.01f);
+
+        gyro_accum += gyro;
+        _notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
+    }
+    gyro_accum /= nsamples;
+    _rotate_and_correct_gyro(gyro_instance, gyro_accum);
+    _notify_new_gyro_raw_sample(gyro_instance, gyro_accum, AP_HAL::micros64());
+}
+
+void AP_InertialSensor_NONE::timer_update(void)
+{
+
+    uint64_t now = AP_HAL::micros64();
+
+    static uint64_t last_msg_sent = 0;
+    if (now > last_msg_sent + 2000000) { //2sec= 2000ms = 2000000us
+        //gcs().send_text(MAV_SEVERITY_WARNING, "NO IMU FOUND");
+        hal.console->printf("INS: NO IMU FOUND\n");
+        last_msg_sent = now;
+    }
+    if (now >= next_accel_sample) {
+        if (((1U << accel_instance) ) == 0) {
+            generate_accel();
+            if (next_accel_sample == 0) {
+                next_accel_sample = now + 1000000UL / accel_sample_hz;
+            } else {
+                while (now >= next_accel_sample) {
+                    next_accel_sample += 1000000UL / accel_sample_hz;
+                }
+            }
+        }
+    }
+    if (now >= next_gyro_sample) {
+        if (((1U << gyro_instance) ) == 0) {
+            generate_gyro();
+            if (next_gyro_sample == 0) {
+                next_gyro_sample = now + 1000000UL / gyro_sample_hz;
+            } else {
+                while (now >= next_gyro_sample) {
+                    next_gyro_sample += 1000000UL / gyro_sample_hz;
+                }
+            }
+        }
+    }
+}
+
+float AP_InertialSensor_NONE::gyro_drift(void)
+{
+ 
+    double period  = 0.01 * 2;
+    double minutes = fmod(AP_HAL::micros64() / 60.0e6, period);
+    if (minutes < period/2) {
+        return minutes * ToRad(0.01);
+    }
+    return (period - minutes) * ToRad(0.01);
+}
+
+
+bool AP_InertialSensor_NONE::update(void) 
+{
+    update_accel(accel_instance);
+    update_gyro(gyro_instance);
+    return true;
+}
+
+uint8_t AP_InertialSensor_NONE::bus_id = 0;
+
+void AP_InertialSensor_NONE::start()
+{
+    if (!_imu.register_gyro(gyro_instance, gyro_sample_hz,
+                            AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL)) ||
+        !_imu.register_accel(accel_instance, accel_sample_hz,
+                             AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL))) {
+        return;
+    }
+    bus_id++;
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_NONE::timer_update, void));
+
+}
+
+#endif // HAL_BOARD_NONE

libraries/AP_InertialSensor/AP_InertialSensor_NONE.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <SITL/SITL.h>
+#include <GCS_MAVLink/GCS.h>
+
+/*
+ This is a 'mock' implementation of an INS that does nothing and gives a level HUD, but does it successfully.   
+ Its useful for boards that don't have any form of IMU accel/gyro etc connected just yet, but where u want to boot-up "successfully" anyway, 
+ such as the ESP32, to allow wifi connectivity to startup, and alow mavlink to start streaming anyway.
+ Its a rip-off of _SITL with all the sitl stuff removed or replaced with constants.
+*/
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+// simulated sensor rates in Hz. This matches a pixhawk1
+const uint16_t INS_NONE_SENSOR_A[] = { 1000, 1000 };
+
+
+class AP_InertialSensor_NONE : public AP_InertialSensor_Backend
+{
+public:
+    AP_InertialSensor_NONE(AP_InertialSensor &imu, const uint16_t sample_rates[]);
+
+    /* update accel and gyro state */
+    bool update() override;
+    void start() override;
+    void accumulate() override;
+    // detect the sensor
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, const uint16_t sample_rates[]);
+
+private:
+    bool init_sensor(void);
+    void timer_update();
+    float gyro_drift(void);
+    void generate_accel();
+    void generate_gyro();
+    //float rand_float(void);
+
+
+    //SITL::SITL *sitl;
+
+    const uint16_t gyro_sample_hz;
+    const uint16_t accel_sample_hz;
+
+    uint8_t gyro_instance;
+    uint8_t accel_instance;
+    uint64_t next_gyro_sample;
+    uint64_t next_accel_sample;
+    float gyro_time;
+    float accel_time;
+    float gyro_motor_phase[12];
+    float accel_motor_phase[12];
+
+    static uint8_t bus_id;
+};
+#endif // CONFIG_HAL_BOARD