diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp
index b61bec4c9292a..9ae12d550541a 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed.cpp
@@ -53,6 +53,7 @@
#include "AP_Airspeed_NMEA.h"
#include "AP_Airspeed_MSP.h"
#include "AP_Airspeed_External.h"
+#include "AP_Airspeed_ND210.h"
#include "AP_Airspeed_SITL.h"
extern const AP_HAL::HAL &hal;
@@ -417,6 +418,11 @@ void AP_Airspeed::allocate()
case TYPE_I2C_ASP5033:
#if AP_AIRSPEED_ASP5033_ENABLED
sensor[i] = NEW_NOTHROW AP_Airspeed_ASP5033(*this, i);
+#endif
+ break;
+ case TYPE_I2C_ND210:
+#if AP_AIRSPEED_ND210_ENABLED
+ sensor[i] = new AP_Airspeed_ND210(*this, i);
#endif
break;
case TYPE_UAVCAN:
@@ -757,7 +763,7 @@ void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t
}
}
}
-#endif
+#endif
#if HAL_LOGGING_ENABLED
// @LoggerMessage: HYGR
diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h
index e4f9d760e01f5..185eb2eeab6c3 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.h
+++ b/libraries/AP_Airspeed/AP_Airspeed.h
@@ -192,6 +192,7 @@ class AP_Airspeed
TYPE_MSP=14,
TYPE_I2C_ASP5033=15,
TYPE_EXTERNAL=16,
+ TYPE_I2C_ND210=17,
TYPE_SITL=100,
};
@@ -216,7 +217,7 @@ class AP_Airspeed
#if AP_AIRSPEED_EXTERNAL_ENABLED
void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt);
#endif
-
+
enum class CalibrationState {
NOT_STARTED,
IN_PROGRESS,
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
index bdb4dded5c562..03ae9d2d23dc8 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
@@ -127,6 +127,7 @@ class AP_Airspeed_Backend {
ANALOG = 0x08,
NMEA = 0x09,
ASP5033 = 0x0A,
+ ND210 = 0x0B,
};
private:
diff --git a/libraries/AP_Airspeed/AP_Airspeed_ND210.cpp b/libraries/AP_Airspeed/AP_Airspeed_ND210.cpp
new file mode 100644
index 0000000000000..0ef6740da737b
--- /dev/null
+++ b/libraries/AP_Airspeed/AP_Airspeed_ND210.cpp
@@ -0,0 +1,302 @@
+/*
+ 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 .
+ */
+
+/*
+ backend driver for airspeed from a I2C ND210 sensor
+ */
+#include "AP_Airspeed_ND210.h"
+
+#if AP_AIRSPEED_ND210_ENABLED
+
+#include
+#include
+
+#include
+
+// default device address
+#define ND210_I2C_ADDR 0x28
+
+// used in pressure calculation. (.90*2^15)
+#define ND210_SCALE_PRESSURE_ND210 29491.2
+
+// 1 H20 = 249.08890833333 Pa
+#define H20_TO_PASCAL 249.089
+
+/*
+ * MODE_CONTROL_REGISTER
+ *
+ * PressureRange
+ * Bits 0-2 control the output pressure range.
+ * Pressure Range, 3 bits, in H20
+ * 000 0.25 -- gives a max airspeed of 11 m/s
+ * 001 0.25
+ * 010 0.5
+ * 011 1.0
+ * 100 2.0
+ * 101 4.0
+ * 110 5.0 -- gives a max airspeed of ~50 m/s
+ * 111 10.0 -- gives a max airspeed of ~75 m/s
+ * _selected_pressure_range should match (or drive) the value of PressureRange
+ *
+ * Watchdog Enable bit
+ * Bit 3 is the I/O Watchdog Enable bit. When set, the I/O watchdog is enabled.
+ * When enabled, the I/O watchdog will monitor the I/O activity.
+ * If I/O activity is not detected for the I/O Watchdog timeout time, the pressure sensor will reset itself.
+ * The I/O watchdog timeout time is determined by the currently active bandwidth setting.
+ *
+ * Bandwidth Limit Select, 3 bits
+ * Bits 4-6 control the BW Limit Filter.
+ * 000 1 Hz
+ * 001 2 Hz
+ * 010 5 Hz
+ * 011 10 Hz
+ * 100 20 Hz
+ * 101 50 Hz
+ * 110 100 Hz
+ * 111 200 Hz
+ *
+ * Bit 7 is the Notch Filter Enable bit. When enabled, the 50/60Hz notch filter is active *
+ *
+ * ModeControlRegister default value is 0xF6 (b1 111 0 110 ) Notch (1), 200Hz (b111), No Watchdog, PressureRange = 5 (b110)
+ * (NotchFilterEnable << 7) | (BandWidth << 6) | (WatchdogEnable << 4) | PressureRange;
+ */
+static constexpr uint8_t MODE_CONTROL_REGISTER = 0xF7; // 0xF6 is default
+
+/*
+ * The Rate Control Register controls the rate at which the DAV
+ * pin is asserted indicating new data is available. This register
+ * is primarily used to throttle down the actual data transfer
+ * rate (when using the DAV as the trigger to sample) since the
+ * general industrial requirement is less than the internal 444Hz update rate.
+ * The value of the Rate Control Register is
+ * the divisor of the 444 Hz internal data rate. Since a divisor of
+ * zero is not possible, a zero value will select the auto-select
+ * rate mode. In this mode, the rate is selected based on the
+ * selected bandwidth limit. The auto rate value is roughly 3x
+ * the corner frequency of the selected bandwidth limit in all
+ * auto selected rates (where possible).
+ * 00000000 = AutoSelect (based on BW)
+ * 00000001 = 444Hz
+ * 00000010 = 222Hz ...
+ */
+static constexpr uint8_t RATE_CONTROL_REGISTER = 0x00; // Auto Select aka (BW = 1Hz, so Rate will be 3Hz)
+
+extern const AP_HAL::HAL &hal;
+
+AP_Airspeed_ND210::AP_Airspeed_ND210(AP_Airspeed &_frontend, uint8_t _instance) :
+ AP_Airspeed_Backend(_frontend, _instance)
+{
+}
+
+/*
+ send a 16 bit command code
+ */
+bool AP_Airspeed_ND210::_send_configuration_command()
+{
+ uint8_t b[2] {MODE_CONTROL_REGISTER, RATE_CONTROL_REGISTER};
+ return _dev->transfer(b, 2, nullptr, 0);
+}
+
+// probe and initialise the sensor
+bool AP_Airspeed_ND210::init()
+{
+ _dev = hal.i2c_mgr->get_device(get_bus(), ND210_I2C_ADDR);
+ if (!_dev) {
+ GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210 no sensor found at 0x%02X", ND210_I2C_ADDR);
+ return false;
+ }
+ _dev->get_semaphore()->take_blocking();
+ // lots of retries during probe
+ _dev->set_retries(10);
+
+ // set Mode and Rate modes
+ if (!_send_configuration_command()) {
+ _dev->get_semaphore()->give();
+ GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210[%u]: Init Send Command Failed.", get_instance());
+ return false;
+ }
+
+ _dev->get_semaphore()->give();
+
+ switch(MODE_CONTROL_REGISTER & 0x07) {
+ case 0b000:
+ case 0b001:
+ _selected_pressure_range = 0.25;
+ break;
+ case 0b010:
+ _selected_pressure_range = 0.5;
+ break;
+ case 0b011:
+ _selected_pressure_range = 1.0;
+ break;
+ case 0b100:
+ _selected_pressure_range = 2.0;
+ break;
+ case 0b101:
+ _selected_pressure_range = 4.0;
+ break;
+ case 0b110:
+ _selected_pressure_range = 5.0;
+ break;
+ case 0b111:
+ _selected_pressure_range = 10.0;
+ break;
+ }
+
+ /*
+ this sensor uses zero offset and skips cal
+ */
+#if 0 // Airspeed seems more accurate without these.
+ set_use_zero_offset();
+ set_skip_cal();
+ set_offset(0);
+#endif
+
+ _dev->set_device_type(uint8_t(DevType::ND210));
+ set_bus_id(_dev->get_bus_id());
+
+ // drop to 2 retries for runtime
+ _dev->set_retries(2);
+
+ _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Airspeed_ND210::_timer, void));
+
+ _print_info();
+
+ return true;
+}
+
+void AP_Airspeed_ND210::_print_info()
+{
+#if HAL_GCS_ENABLED
+ _dev->get_semaphore()->take_blocking();
+
+ uint8_t val[22];
+ bool ret = _dev->transfer(nullptr, 0, val, sizeof(val));
+ if (ret == false) {
+ GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210 Init Read Failed");
+ _dev->get_semaphore()->give();
+ return;
+ }
+ _dev->get_semaphore()->give();
+
+ // Model, 5-12, 8 byte, ASCII, null terminated,
+ // Right reading ASCII with null terminator,
+ // eg 4EH,44H,32H,31H,30H,00H,xxH,xxH = ND210
+ char model[8];
+ memcpy(model, &val[4], sizeof(model));
+ // Serial Number, 13-16, 4 byte, Hex,
+ // Unique 4 byte serial for each part,
+ // eg 2FD627A4H
+ uint32_t serialNumber = ( (((uint32_t)val[12]) << 24) | (((uint32_t)val[13]) << 16) | ( ((uint32_t)val[14]) << 8 ) | (uint32_t)val[15] );
+ // Build Number, 17-22, 6 byte, ASCII, null terminated,
+ // Right reading ASCII with null termination,
+ // eg 30H,30H,30H,33H,43H,00H = 0003C
+ char buildNumber[6];
+ memcpy(buildNumber, &val[16], sizeof(buildNumber) );
+
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+ "ND210: Model=%s at address=0x%02x",
+ model,
+ ND210_I2C_ADDR
+ );
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+ "ND210: Serial#=0x%04X, Build#=%s",
+ (unsigned int) serialNumber,
+ buildNumber
+ );
+ GCS_SEND_TEXT(MAV_SEVERITY_INFO,
+ "ND210: _selected_pressure_range=%f",
+ _selected_pressure_range
+ );
+#endif
+}
+
+
+// read the values from the sensor. Called at 50Hz
+void AP_Airspeed_ND210::_timer()
+{
+ // read 4 bytes from the sensor
+ uint8_t val[4];
+ bool ret = _dev->transfer(nullptr, 0, val, sizeof(val));
+ uint32_t now = AP_HAL::millis();
+ if (ret == false) {
+ GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210 Read Failed");
+ if (now - _last_sample_time_ms > 200) {
+ // reset configuration
+ _send_configuration_command();
+ }
+ return;
+ }
+
+ uint16_t P = (((uint16_t)val[0]) << 8) | val[1];
+ float diff_pressure_h20 = (float(P) / ND210_SCALE_PRESSURE_ND210) * _selected_pressure_range;
+ // GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210 Raw Pressure = 0x%02X", P);
+
+ uint8_t temperatureInteger = val[2];
+ float temperatureFractional = val[3] / 256; // convert byte to fraction.
+ float temperatureCelsius = temperatureInteger + temperatureFractional;
+ // GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ND210 Temperature Celcius = %f", temperatureCelsius);
+
+ WITH_SEMAPHORE(sem);
+
+ _press_sum += (diff_pressure_h20 * H20_TO_PASCAL);
+ _temp_sum += temperatureCelsius;
+ _press_count++;
+ _temp_count++;
+ _last_sample_time_ms = now;
+}
+
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_ND210::get_differential_pressure(float &pressure)
+{
+ WITH_SEMAPHORE(sem);
+
+ if (AP_HAL::millis() - _last_sample_time_ms > 100) {
+ return false;
+ }
+
+ if (_press_count > 0) {
+ _press = _press_sum / _press_count;
+ _press_count = 0;
+ _press_sum = 0;
+ }
+
+ pressure = _press;
+
+ return true;
+}
+
+// return the current temperature in degrees C, if available
+bool AP_Airspeed_ND210::get_temperature(float &temperature)
+{
+ WITH_SEMAPHORE(sem);
+
+ if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
+ return false;
+ }
+
+ if (_temp_count > 0) {
+ _temp = _temp_sum / _temp_count;
+ _temp_count = 0;
+ _temp_sum = 0;
+ }
+
+ temperature = _temp;
+
+ return true;
+}
+
+#endif // AP_AIRSPEED_SDP3X_ENABLED
diff --git a/libraries/AP_Airspeed/AP_Airspeed_ND210.h b/libraries/AP_Airspeed/AP_Airspeed_ND210.h
new file mode 100644
index 0000000000000..ee527f12d4d68
--- /dev/null
+++ b/libraries/AP_Airspeed/AP_Airspeed_ND210.h
@@ -0,0 +1,75 @@
+/*
+ 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
+
+#ifndef AP_AIRSPEED_ND210_ENABLED
+#define AP_AIRSPEED_ND210_ENABLED AP_AIRSPEED_BACKEND_DEFAULT_ENABLED
+#endif
+
+#if AP_AIRSPEED_ND210_ENABLED
+
+/*
+ backend driver for airspeed from I2C
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+#include "AP_Airspeed_Backend.h"
+#include "AP_Airspeed.h"
+
+/*
+ * Thanks to PX4 for registers definitions
+ */
+
+class AP_Airspeed_ND210 : public AP_Airspeed_Backend
+{
+public:
+ AP_Airspeed_ND210(AP_Airspeed &frontend, uint8_t _instance);
+ ~AP_Airspeed_ND210(void) {}
+
+ // probe and initialise the sensor
+ bool init() override;
+
+ // return the current differential_pressure in Pascal
+ bool get_differential_pressure(float &pressure) override;
+
+ // return the current temperature in degrees C, if available
+ bool get_temperature(float &temperature) override;
+
+private:
+ void _timer();
+ bool _send_configuration_command();
+ void _print_info();
+
+ float _temp; // temperature in Celsius
+ float _press; // differential pressure in Pascal
+ uint16_t _temp_count;
+ uint16_t _press_count;
+ float _temp_sum;
+ float _press_sum; // pressure accumulator in Pascal
+ uint32_t _last_sample_time_ms;
+ float _selected_pressure_range;
+
+ AP_HAL::OwnPtr _dev;
+};
+
+
+#endif // AP_AIRSPEED_SDP3X_ENABLED
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
index 749fa9d612164..77b80098e4252 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp
@@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Airspeed type
// @Description: Type of airspeed sensor
- // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL
+ // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,17:NP210,100:SITL
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE),