diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index c8634444135eca..d4dc3eb6dda0a2 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -26,6 +26,7 @@ ] COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ + 'AP_AirSensor', 'AP_Airspeed', 'AP_AccelCal', 'AP_ADC', diff --git a/libraries/AP_AirSensor/AP_AirSensor.cpp b/libraries/AP_AirSensor/AP_AirSensor.cpp new file mode 100644 index 00000000000000..0d08506015345a --- /dev/null +++ b/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 . + */ + +#include "AP_AirSensor.h" + +#if AP_AIRSENSOR_ENABLED + +#include +#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 diff --git a/libraries/AP_AirSensor/AP_AirSensor.h b/libraries/AP_AirSensor/AP_AirSensor.h new file mode 100644 index 00000000000000..873dbcf681b72d --- /dev/null +++ b/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 . + */ +#pragma once + +#include "AP_AirSensor_config.h" + +#if AP_AIRSENSOR_ENABLED + +#include +#include +#include +#include + +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 diff --git a/libraries/AP_AirSensor/AP_AirSensor_Backend.cpp b/libraries/AP_AirSensor/AP_AirSensor_Backend.cpp new file mode 100644 index 00000000000000..3e2556d6b7ccb1 --- /dev/null +++ b/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 . + */ + +#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 diff --git a/libraries/AP_AirSensor/AP_AirSensor_Backend.h b/libraries/AP_AirSensor/AP_AirSensor_Backend.h new file mode 100644 index 00000000000000..891a37d8ec4710 --- /dev/null +++ b/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 . + */ +#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 +}; diff --git a/libraries/AP_AirSensor/AP_AirSensor_Params.cpp b/libraries/AP_AirSensor/AP_AirSensor_Params.cpp new file mode 100644 index 00000000000000..4c6cbf1596d2a0 --- /dev/null +++ b/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 . + */ + +#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 \ No newline at end of file diff --git a/libraries/AP_AirSensor/AP_AirSensor_Scripting.cpp b/libraries/AP_AirSensor/AP_AirSensor_Scripting.cpp new file mode 100644 index 00000000000000..aa3f8e1e8b1341 --- /dev/null +++ b/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 . + */ + +#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 diff --git a/libraries/AP_AirSensor/AP_AirSensor_Scripting.h b/libraries/AP_AirSensor/AP_AirSensor_Scripting.h new file mode 100644 index 00000000000000..74d09ed640c9cb --- /dev/null +++ b/libraries/AP_AirSensor/AP_AirSensor_Scripting.h @@ -0,0 +1,42 @@ +/* + 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 . + */ +#pragma once + +#include "AP_AirSensor_config.h" + +#if AP_AIRSENSOR_SCRIPTING_ENABLED + +#include "AP_AirSensor_Backend.h" +#include "AP_Math/vector3.h" + +class AP_AirSensor_Scripting: public AP_AirSensor_Backend { +public: + bool init() override { return true; } + void update() override; + + // TODO add a getter like "bool get_wind(Vector3f& wind_uvw) const" - see AP_Proximity_Scripting get_upward_distance + bool get_wind(Vector3f& wind_uvw) const override; + +#if AP_SCRIPTING_ENABLED + // this is in body frame + bool handle_script_uvw_msg(float wind_u, float wind_v, float wind_w) override; + bool handle_script_3d_msg(const Vector3f &wind_uvw) override; +#endif +private: + uint32_t _last_update_ms; + Vector3f _wind_uvw; +}; + +#endif // AP_AIRSENSOR_SCRIPTING_ENABLED diff --git a/libraries/AP_AirSensor/AP_AirSensor_config.h b/libraries/AP_AirSensor/AP_AirSensor_config.h new file mode 100644 index 00000000000000..78c5cd75346a1e --- /dev/null +++ b/libraries/AP_AirSensor/AP_AirSensor_config.h @@ -0,0 +1,26 @@ +#pragma once + +#include +#include +#include +#include + +#ifndef AP_AIRSENSOR_ENABLED +#define AP_AIRSENSOR_ENABLED 1 +#endif + +#ifndef AP_AIRSENSOR_AIRSPEED_ENABLED +#define AP_AIRSENSOR_AIRSPEED_ENABLED (AP_AIRSENSOR_ENABLED && AP_AIRSPEED_ENABLED) +#endif + +#ifndef AP_AIRSENSOR_WINDVANE_ENABLED +#define AP_AIRSENSOR_WINDVANE_ENABLED (AP_AIRSENSOR_ENABLED && AP_WINDVANE_ENABLED) +#endif + +#ifndef AP_AIRSENSOR_SCRIPTING_ENABLED +#define AP_AIRSENSOR_SCRIPTING_ENABLED (AP_AIRSENSOR_ENABLED && AP_SCRIPTING_ENABLED) +#endif + +#ifndef AP_AIR_SENSOR_MAX_SENSORS +#define AP_AIR_SENSOR_MAX_SENSORS 1 +#endif \ No newline at end of file diff --git a/libraries/AP_AirSensor/README.md b/libraries/AP_AirSensor/README.md index d92c516498a68e..2dcb521a0a4baf 100644 --- a/libraries/AP_AirSensor/README.md +++ b/libraries/AP_AirSensor/README.md @@ -125,4 +125,15 @@ classDiagram AP_AirSensor *-- Type : "enum" AP_AirSensor *-- State : "state" AP_AirSensor *-- "AIR_SENSOR_MAX_SENSORS" AP_AirSensorBackend -``` \ No newline at end of file +``` + +# Scripting + +The scripting support is intended to allow for quick adoption of new air sensors. +This is similar to the following drivers: +* AP_Camera_Scripting +* AP_EFI_Scripting +* AP_Mount_Scripting +* AP_Proximity_Scripting +* AP_BattMonitor_Scripting +* AP_MotorsMatrix_6DoF_Scripting \ No newline at end of file diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 856097a9be470c..fe6fc4d351d417 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -1070,3 +1070,16 @@ userdata AP_Servo_Telem::TelemetryData field motor_temperature_cdeg int16_t'skip userdata AP_Servo_Telem::TelemetryData field pcb_temperature_cdeg int16_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::PCB_TEMP userdata AP_Servo_Telem::TelemetryData field status_flags uint8_t'skip_check read valid_mask present_types AP_Servo_Telem::TelemetryData::Types::STATUS userdata AP_Servo_Telem::TelemetryData field last_update_ms uint32_t'skip_check read + +include AP_AirSensor/AP_AirSensor.h +include AP_AirSensor/AP_AirSensor_Backend.h + +singleton AP_AirSensor depends AP_AIRSENSOR_ENABLED == 1 +ap_object AP_AirSensor_Backend depends AP_AIRSENSOR_ENABLED == 1 +ap_object AP_AirSensor_Backend method handle_script_3d_msg boolean Vector3f +ap_object AP_AirSensor_Backend method type uint8_t + +singleton AP_AirSensor rename airsensor +-- singleton AP_AirSensor method get_status uint8_t +singleton AP_AirSensor method num_sensors uint8_t +singleton AP_AirSensor method get_backend AP_AirSensor_Backend uint8_t'skip_check diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b4f002d52e6fab..d0e86a5c91ff74 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -284,6 +284,12 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(serial_manager, "SERIAL", 31, AP_Vehicle, AP_SerialManager), #endif +// #if AP_AIRSENSOR_ENABLED +// // @Group: ARSPD +// // @Path: ../AP_AirSensor/AP_AirSensor.cpp +// AP_SUBGROUPINFO(airsensor, "ASNS", 32, AP_Vehicle, AP_AirSensor), +// #endif + AP_GROUPEND }; @@ -434,6 +440,17 @@ void AP_Vehicle::setup() #endif #endif // AP_AIRSPEED_ENABLED +#if AP_AIRSENSOR_ENABLED + airsensor.init(); + // if (airsensor.enabled()) { + // airspeed.calibrate(true); + // } +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + // else { + // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No air sensor"); + // } +#endif +#endif // AP_AIRSENSOR_ENABLED #if AP_SRV_CHANNELS_ENABLED AP::srv().init(); @@ -672,6 +689,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if AP_ARMING_ENABLED SCHED_TASK(update_arming, 1, 50, 253), #endif +#if AP_AIRSENSOR_ENABLED + SCHED_TASK_CLASS(AP_AirSensor, &vehicle.airsensor, update, 10, 100, 254), +#endif }; void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index e1ec483203e256..c9192d4e086ac8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -27,6 +27,7 @@ #include #include +#include #include #include #include // board configuration library @@ -453,6 +454,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_Airspeed airspeed; #endif +#if AP_AIRSENSOR_ENABLED + AP_AirSensor airsensor; +#endif + #if AP_STATS_ENABLED // vehicle statistics AP_Stats stats;