Add airsensor implementation

Signed-off-by: Ryan Friedman <[email protected]>

Tools/ardupilotwaf/ardupilotwaf.py

@@ -26,6 +26,7 @@
 ]
 
 COMMON_VEHICLE_DEPENDENT_LIBRARIES = [
+    'AP_AirSensor',
     'AP_Airspeed',
     'AP_AccelCal',
     'AP_ADC',

libraries/AP_AirSensor/AP_AirSensor.cpp

@@ -0,0 +1,105 @@
+/*
+   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/>.
+ */
+
+#include "AP_AirSensor.h"
+
+#if AP_AIRSENSOR_ENABLED
+
+#include <GCS_MAVLink/GCS.h>
+#include "AP_AirSensor_Backend.h"
+#include "AP_AirSensor_Scripting.h"
+
+// table of user settable parameters
+// const AP_Param::GroupInfo AP_AirSensor::var_info[] = {
+// };
+
+AP_AirSensor::AP_AirSensor()
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (_singleton != nullptr) {
+        AP_HAL::panic("AP_AirSensor must be singleton");
+    }
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    _singleton = this;
+}
+
+void AP_AirSensor::init()
+{
+  // TODO add param for enabling,
+  allocate();
+}
+
+void AP_AirSensor::update()
+{
+    // TODO
+}
+
+void AP_AirSensor::allocate()
+{
+    for (size_t i = 0; i < AP_AIR_SENSOR_MAX_SENSORS; i++) {
+        // TODO this overrides to use scripting for now.
+        params[i].type.set((int8_t)AP_AirSensor::Type::SCRIPTING);
+        switch ((AP_AirSensor::Type)params[i].type.get()) {
+            case AP_AirSensor::Type::NONE:
+                // nothing to do
+                break;
+            case AP_AirSensor::Type::SCRIPTING:
+#if AP_AIRSENSOR_SCRIPTING_ENABLED
+            sensors[i] = NEW_NOTHROW AP_AirSensor_Scripting(*this, state[i]); // TODO pass in params[i]
+#endif
+                break;
+            default:
+                break;
+                // TODO
+        }
+
+        if (sensors[i] && !sensors[i]->init()) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AirSensor %zu init failed", i + 1);
+            delete sensors[i];
+            sensor[i] = nullptr;
+        }
+        if (sensors[i] != nullptr) {
+            _num_sensors = i+1;
+        }
+    }
+}
+
+// return air sensor backend for Lua scripting
+AP_AirSensor_Backend *AP_AirSensor::get_backend(uint8_t id) const
+{
+    if (!valid_instance(id)) {
+        return nullptr;
+    }
+    return drivers[id];
+}
+
+// return true if the given instance exists
+bool AP_AirSensor::valid_instance(uint8_t i) const
+{
+    if (i >= AP_AIR_SENSOR_MAX_SENSORS) {
+        return false;
+    }
+
+    if (drivers[i] == nullptr) {
+        return false;
+    }
+    return true;
+    // return (Type)params[i].type.get() != Type::None; // TODO
+}
+
+AP_AirSensor *AP_AirSensor::_singleton;
+
+
+#endif // AP_AIRSENSOR_ENABLED

libraries/AP_AirSensor/AP_AirSensor.h

@@ -0,0 +1,100 @@
+/*
+   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/>.
+ */
+#pragma once
+
+#include "AP_AirSensor_config.h"
+
+#if AP_AIRSENSOR_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <inttypes.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+
+class AP_AirSensor_Backend;
+
+class AP_AirSensor_Params {
+public:
+    // Constructor
+    AP_AirSensor_Params(void);
+    AP_Int8  type; // TODO forcing scripting for now as default type.
+    // static const struct AP_Param::GroupInfo var_info[];
+};
+
+/// @class AP_AirSensor
+/// Air data sensing class that provides a common interface for 1D AP_Airspeed probes
+/// 2D WindVane sensors, and other 1D-3D capable air data sensor interfaces.
+class AP_AirSensor
+{
+public:
+    friend class AP_AirSensor_Backend;  
+    // static const struct AP_Param::GroupInfo var_info[];
+
+    AP_AirSensor();
+
+    // return the number of air sensor backends
+    uint8_t num_sensors() const { return _num_sensors; }
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_AirSensor);
+
+    enum class Type {
+        NONE,
+        SCRIPTING
+    };
+
+    enum class Status {
+        NotConnected = 0,
+        NoData,
+        Good
+    };
+
+    // The State structure is filled in by the backend driver
+    struct State {
+        uint8_t instance; // the instance number of this proximity sensor
+        Status status; // sensor status
+
+        // const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists)
+    };
+
+    // detect and initialise any available air sensors
+    void init();
+    // update state of all air sensors. Should be called at high rate from main loop
+    void update();
+    static AP_AirSensor *get_singleton(void) { return _singleton; };
+
+    // return backend object for Lua scripting
+    AP_AirSensor_Backend *get_backend(uint8_t id) const;
+
+    // Returns status of first good sensor. If no good sensor found, returns status of last instance sensor 
+    Status get_status() const;
+private:
+
+    void allocate();
+
+    // return true if the given instance exists
+    bool valid_instance(uint8_t i) const;
+
+    static AP_AirSensor *_singleton;
+    State state[AP_AIR_SENSOR_MAX_SENSORS];
+    // TODO params
+
+
+    AP_AirSensor_Params params[AP_AIR_SENSOR_MAX_SENSORS];
+    AP_AirSensor_Backend *sensors[AP_AIR_SENSOR_MAX_SENSORS];
+    uint8_t _num_sensors;
+};
+
+#endif // AP_AIRSENSOR_ENABLED

libraries/AP_AirSensor/AP_AirSensor_Backend.cpp

@@ -0,0 +1,35 @@
+/*
+   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/>.
+ */
+
+#include "AP_AirSensor_Backend.h"
+
+#if AP_AIRSENSOR_ENABLED
+
+AP_AirSensor_Backend::AP_AirSensor_Backend(AP_AirSensor& frontend, AP_AirSensor::State& state) :
+    _frontend(frontend),
+    _state(state)
+{
+    // _backend_type = (AP_Proximity::Type )_params.type.get(); // TODO
+    _backend_type = AP_AirSensor::Type::SCRIPTING;
+
+}
+
+void AP_AirSensor_Backend::set_status(AP_AirSensor::Status status)
+{
+    _state.status = status;
+}
+
+
+#endif // AP_AIRSENSOR_ENABLED

libraries/AP_AirSensor/AP_AirSensor_Backend.h

@@ -0,0 +1,52 @@
+/*
+   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/>.
+ */
+#pragma once
+
+#include "AP_AirSensor.h"
+
+#include "AP_AirSensor_config.h"
+#include "AP_Math/vector3.h"
+
+class AP_AirSensor_Backend {
+public:
+    AP_AirSensor_Backend() = delete;
+	AP_AirSensor_Backend(AP_AirSensor &frontend, AP_AirSensor::State &state);
+    virtual bool init() = 0;
+    virtual ~AP_AirSensor_Backend() {}
+    virtual void update() = 0;
+    // get measured wind vector
+    virtual bool get_wind(Vector3f& wind_uvw) const { return false; }
+
+#if AP_SCRIPTING_ENABLED
+    // this is in body frame
+    virtual bool handle_script_uvw_msg(float wind_u, float wind_v, float wind_w) { return false; }
+    virtual bool handle_script_3d_msg(const Vector3f &wind_uvw) { return false; }
+#endif
+
+    // return the type of sensor
+    AP_AirSensor::Type type() const {
+        // return (AP_Proximity::Type)params.type.get(); // TODO use the right type
+        return  AP_AirSensor::Type::SCRIPTING;   
+    }
+
+protected:
+    // set status and update valid_count
+    void set_status(AP_AirSensor::Status status);
+
+private:
+    AP_AirSensor::Type _backend_type;
+    AP_AirSensor &_frontend;
+    AP_AirSensor::State &_state;   // reference to this instances state
+};

libraries/AP_AirSensor/AP_AirSensor_Params.cpp

@@ -0,0 +1,30 @@
+/*
+   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/>.
+ */
+
+#include "AP_AirSensor.h"
+
+#if AP_AIRSENSOR_ENABLED
+
+// table of user settable parameters
+// const AP_Param::GroupInfo AP_AirSensor_Params::var_info[] = {
+// };
+
+AP_AirSensor_Params::AP_AirSensor_Params(void)
+{
+    // AP_Param::setup_object_defaults(this, var_info);
+
+}
+
+#endif // AP_AIRSENSOR_ENABLED

libraries/AP_AirSensor/AP_AirSensor_Scripting.cpp

@@ -0,0 +1,60 @@
+/*
+   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/>.
+ */
+
+#include "AP_AirSensor_config.h"
+
+#if AP_AIRSENSOR_SCRIPTING_ENABLED
+
+#include "AP_AirSensor_Scripting.h"
+
+// Require 10Hz, with 1.5x FOS.
+static constexpr uint32_t TIMEOUT_MS = 100 * 1.5;
+
+void AP_AirSensor_Scripting::update() {
+
+    // TODO investigate why scripting checks 0 first.
+    if (AP_HAL::millis() - _last_update_ms < 100 * 1.5) {
+        // set_status(AP_Proximity::Status::NoData);
+    } else {
+        // set_status(AP_Proximity::Status::Good);
+    }
+}
+
+bool AP_AirSensor_Scripting::get_wind(Vector3f& wind_uvw) const {
+    if ((_last_update_ms != 0) && (AP_HAL::millis() - _last_update_ms <= TIMEOUT_MS)) {
+        wind_uvw = _wind_uvw;
+        return true;
+    }
+    return false;
+}
+
+#if AP_SCRIPTING_ENABLED
+bool AP_AirSensor_Scripting::handle_script_uvw_msg(float wind_u, float wind_v, float wind_w) {
+    _last_update_ms = AP_HAL::millis();
+    _wind_uvw = {
+        wind_u,
+        wind_v,
+        wind_w
+    };
+    return true;
+}
+bool AP_AirSensor_Scripting::handle_script_3d_msg(const Vector3f &wind_uvw) {
+    _last_update_ms = AP_HAL::millis();
+    _wind_uvw = wind_uvw;
+    return true;
+}
+#endif
+
+#endif // AP_AIRSENSOR_SCRIPTING_ENABLED