From 4b02c8ef2cd59a614fcf983d7221cd15fe7c9cf3 Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:20:12 -0800 Subject: [PATCH 1/6] AP_AngleSensor: Add Angle Sensor Library This creates a generic angle sensor/absolute encoder library with initial support for the AS5048B Magnetic Rotary encoder. This driver follows the typical frontend/backend pattern and future support for additional encoders is expected. This is initially integrated with AP_WindVane, but could be used anywhere that absolute angle measurements are required. --- libraries/AP_AngleSensor/AP_AngleSensor.cpp | 177 ++++++++++++++++++ libraries/AP_AngleSensor/AP_AngleSensor.h | 111 +++++++++++ .../AP_AngleSensor/AP_AngleSensor_AS5048B.cpp | 117 ++++++++++++ .../AP_AngleSensor/AP_AngleSensor_AS5048B.h | 45 +++++ .../AP_AngleSensor/AP_AngleSensor_Backend.cpp | 38 ++++ .../AP_AngleSensor/AP_AngleSensor_Backend.h | 47 +++++ .../AP_AngleSensor/AP_AngleSensor_Params.cpp | 54 ++++++ .../AP_AngleSensor/AP_AngleSensor_Params.h | 29 +++ .../AP_AngleSensor/AP_AngleSensor_config.h | 13 ++ libraries/AP_AngleSensor/LogStructure.h | 33 ++++ .../AbsoluteEncoder_Test/AngleSensor_Test.cpp | 74 ++++++++ .../examples/AbsoluteEncoder_Test/wscript | 7 + 12 files changed, 745 insertions(+) create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor.cpp create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor.h create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.cpp create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.h create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_Backend.cpp create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_Backend.h create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_Params.cpp create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_Params.h create mode 100644 libraries/AP_AngleSensor/AP_AngleSensor_config.h create mode 100644 libraries/AP_AngleSensor/LogStructure.h create mode 100644 libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/AngleSensor_Test.cpp create mode 100644 libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/wscript diff --git a/libraries/AP_AngleSensor/AP_AngleSensor.cpp b/libraries/AP_AngleSensor/AP_AngleSensor.cpp new file mode 100644 index 0000000000000..25949eb34e920 --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor.cpp @@ -0,0 +1,177 @@ +/* + 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_AngleSensor.h" + +#if AP_ANGLESENSOR_ENABLED + +#include "AP_AngleSensor_AS5048B.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_AngleSensor::var_info[] = { + // @Group: _ + // @Path: AP_AngleSensor_Params.cpp + AP_SUBGROUPINFO(_params[0], "_", 0, AP_AngleSensor, AP_AngleSensor_Params), + + #if ANGLE_SENSOR_MAX_INSTANCES > 1 + // @Group: 2_ + // @Path: AP_AngleSensor_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 1, AP_AngleSensor, AP_AngleSensor_Params), + #endif + AP_GROUPEND +}; + +AP_AngleSensor::AP_AngleSensor(void) +{ + _singleton = this; + + AP_Param::setup_object_defaults(this, var_info); +} + +// initialise the AP_AngleSensor class. +void AP_AngleSensor::init(void) +{ + if (_num_instances != 0) { + // init called a 2nd time? + return; + } + for (uint8_t i=0; iupdate(); + } + } + Log_Write(); +} + +// log angle sensor information +void AP_AngleSensor::Log_Write() const +{ + // return immediately if no angle sensors are enabled + if (!enabled(0) && !enabled(1)) { + return; + } + + struct log_AngleSensor pkt = { + LOG_PACKET_HEADER_INIT(LOG_ANGLESENSOR_MSG), + time_us : AP_HAL::micros64(), + angle_0 : (float)get_angle_radians(0), + quality_0 : (uint8_t)get_signal_quality(0), + angle_1 : (float)get_angle_radians(1), + quality_1 : (uint8_t)get_signal_quality(1), + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +// check if an instance is healthy +bool AP_AngleSensor::healthy(uint8_t instance) const +{ + return enabled(instance) && get_signal_quality(instance) > 0; +} + +// check if an instance is activated +bool AP_AngleSensor::enabled(uint8_t instance) const +{ + if (instance >= _num_instances) { + return false; + } + // if no sensor type is selected, the sensor is not activated. + return _params[instance]._type != ANGLESENSOR_TYPE_NONE; +} + +uint8_t AP_AngleSensor::get_type(uint8_t instance) const +{ + if (instance >= _num_instances) { + return 0; + } + // if no sensor type is selected, the sensor is not activated. + return (uint8_t) _params[instance]._type; +} + +// //get the most recent angle measurement of the sensor (in radians) +float AP_AngleSensor::get_angle_radians(uint8_t instance) const +{ + + // for invalid instances return zero + if (instance >= ANGLE_SENSOR_MAX_INSTANCES) { + return 0; + } + + float raw_angle = state[instance].angle_radians; + float offset_rad = radians(_params[instance]._offset); + int8_t direction = _params[instance]._direction; + + return wrap_2PI(raw_angle*(float)direction + offset_rad); +} + +// get the signal quality for a sensor +uint8_t AP_AngleSensor::get_signal_quality(uint8_t instance) const +{ + // for invalid instances return zero + if (instance >= ANGLE_SENSOR_MAX_INSTANCES) { + return 0; + } + return state[instance].quality; +} + +// get the system time (in milliseconds) of the last update +uint32_t AP_AngleSensor::get_last_reading_ms(uint8_t instance) const +{ + // for invalid instances return zero + if (instance >= ANGLE_SENSOR_MAX_INSTANCES) { + return 0; + } + return state[instance].last_reading_ms; +} + +// singleton instance +AP_AngleSensor *AP_AngleSensor::_singleton; + +namespace AP { + +AP_AngleSensor *ANGLE_SENSOR() +{ + return AP_AngleSensor::get_singleton(); +} + +} + +#endif //AP_ANGLESENSOR_ENABLED + diff --git a/libraries/AP_AngleSensor/AP_AngleSensor.h b/libraries/AP_AngleSensor/AP_AngleSensor.h new file mode 100644 index 0000000000000..db2036893527e --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor.h @@ -0,0 +1,111 @@ +/* + 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_AngleSensor_config.h" + +#if AP_ANGLESENSOR_ENABLED + +#include +#include +#include +#include "AP_AngleSensor_Params.h" + +#define ANGLE_SENSOR_TYPE_NONE 0 + +class AP_AngleSensor_Backend; +class AP_AngleSensor_AS5048B; + +class AP_AngleSensor +{ +public: + friend class AP_AngleSensor_Backend; + friend class AP_AngleSensor_AS5048B; + + AP_AngleSensor(void); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_AngleSensor); + + // get singleton instance + static AP_AngleSensor *get_singleton() { + return _singleton; + } + + // AngleSensor driver types + enum AngleSensor_Type : uint8_t { + ANGLESENSOR_TYPE_NONE = ANGLE_SENSOR_TYPE_NONE, + ANGLESENSOR_TYPE_AS5048B = 1, + }; + + // The AngleSensor_State structure is filled in by the backend driver + struct AngleSensor_State { + uint8_t instance; // the instance number of this Angle Sensor + float angle_radians; // current angle measured by the sensor, in radians + uint8_t quality; // sensor quality as a percent + uint32_t last_reading_ms; // time of last reading + }; + + // detect and initialise any available angle sensor devices + void init(void); + + // update state of all sensors. Should be called from main loop + void update(void); + + // log data to logger + void Log_Write() const; + + // return the number of angle sensor sensor instances + uint8_t num_sensors(void) const { return _num_instances; } + + // return true if healthy + bool healthy(uint8_t instance) const; + + // return true if the instance is enabled + bool enabled(uint8_t instance) const; + + // get the type of sensor for a specific instance + uint8_t get_type(uint8_t instance) const; + + //get the most recent angle measurement of the sensor + float get_angle_radians(uint8_t instance) const; + + // get the signal quality for a sensor (0 = extremely poor quality, 100 = extremely good quality) + uint8_t get_signal_quality(uint8_t instance) const; + + // get the system time (in milliseconds) of the last update + uint32_t get_last_reading_ms(uint8_t instance) const; + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + /// parameters + AP_AngleSensor_Params _params[ANGLE_SENSOR_MAX_INSTANCES]; + + +private: + + AngleSensor_State state[ANGLE_SENSOR_MAX_INSTANCES]; + AP_AngleSensor_Backend *drivers[ANGLE_SENSOR_MAX_INSTANCES]; + uint8_t _num_instances; + + static AP_AngleSensor *_singleton; +}; + +namespace AP { + AP_AngleSensor *ANGLE_SENSOR(); +} + +#endif // AP_ANGLESENSOR_ENABLED diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.cpp b/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.cpp new file mode 100644 index 0000000000000..0d7588fdb958b --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.cpp @@ -0,0 +1,117 @@ +/* + 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_AngleSensor_AS5048B.h" + +#if AP_ANGLESENSOR_ENABLED + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AS5048B_POLL_RATE_HZ 20 + +#define AS5048B_DIAGNOSTIC_REG 0xFB // Magnitude, least significant byte +#define AS5048B_MAGMSB_REG 0xFC // Magnitude, most significant byte +#define AS5048B_MAGLSB_REG 0xFD // Magnitude, least significant byte +#define AS5048B_ANGLEMSB_REG 0xFE // Angle, most significant byte +#define AS5048B_ANGLELSB_REG 0xFF // Angle, least significant byte + +// We will read 5 registers, starting from the diagnostic register +#define AS5048B_DIAGNOSTIC_OFS 0 +#define AS5048B_MAGMSB_OFS (AS5048B_MAGMSB_REG - AS5048B_DIAGNOSTIC_REG) +#define AS5048B_MAGLSB_OFS (AS5048B_MAGLSB_REG - AS5048B_DIAGNOSTIC_REG) +#define AS5048B_ANGLEMSB_OFS (AS5048B_ANGLEMSB_REG - AS5048B_DIAGNOSTIC_REG) +#define AS5048B_ANGLELSB_OFS (AS5048B_ANGLELSB_REG - AS5048B_DIAGNOSTIC_REG) + +#define AS5048B_COUNTS 16384 + +AP_AngleSensor_AS5048B::AP_AngleSensor_AS5048B( + AP_AngleSensor &frontend, + uint8_t instance, + AP_AngleSensor::AngleSensor_State &state) : + AP_AngleSensor_Backend(frontend, instance, state) +{ + init(); +} + +bool AP_AngleSensor_AS5048B::init(void) +{ + uint8_t bus = _frontend._params[_state.instance]._bus; + uint8_t address = _frontend._params[_state.instance]._addr; + + _dev = hal.i2c_mgr->get_device(bus, address); + if (!_dev) { + hal.console->printf("AP_AngleSensor: Could not open I2C Device on Bus %u at Address 0x%02x \n", bus, address); + return false; + } + + hal.console->printf("AP_AngleSensor: Opened Device for Bus %u at Address 0x%02x \n", bus, address); + + // call timer() at 20Hz + _dev->register_periodic_callback(1E6/AS5048B_POLL_RATE_HZ , + FUNCTOR_BIND_MEMBER(&AP_AngleSensor_AS5048B::timer, void)); + + return true; +} + +void AP_AngleSensor_AS5048B::update(void) +{ + // nothing to do - its all done in the timer() +} + + +void AP_AngleSensor_AS5048B::timer(void) +{ + + uint8_t buf[5]; + + uint32_t time = AP_HAL::millis(); + _dev->get_semaphore()->take_blocking(); + bool success = _dev->read_registers(AS5048B_DIAGNOSTIC_REG, buf, sizeof(buf)); + _dev->get_semaphore()->give(); + + if (!success){ + copy_state_to_frontend(0,0,time); + return; + } + + bool data_invalid = buf[AS5048B_DIAGNOSTIC_OFS] & 0b10; // CORDIC Overflow bit + + if (data_invalid) { + copy_state_to_frontend(0,0,time); + return; + } + + uint8_t angle_msb = buf[AS5048B_ANGLEMSB_OFS]; + uint8_t angle_lsb = buf[AS5048B_ANGLELSB_OFS]; + uint16_t angle_raw = ((uint16_t) angle_msb << 6) | (angle_lsb & 0x3f); + float angle_radians = (angle_raw / (float) AS5048B_COUNTS) * M_2PI; + + uint8_t mag_msb = buf[AS5048B_MAGMSB_OFS]; + uint8_t mag_lsb = buf[AS5048B_MAGLSB_OFS]; + uint16_t mag_raw = ((uint16_t) mag_msb << 6) | (mag_lsb & 0x3f); + uint8_t quality = (100 * mag_raw)/ AS5048B_COUNTS ; + + copy_state_to_frontend(angle_radians,quality,time); + +} + + +#endif // AP_ANGLESENSOR_ENABLED diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.h b/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.h new file mode 100644 index 0000000000000..fa3e3302d7e44 --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_AS5048B.h @@ -0,0 +1,45 @@ +/* + 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_AngleSensor.h" + +#if AP_ANGLESENSOR_ENABLED + +#include "AP_AngleSensor.h" +#include "AP_AngleSensor_Backend.h" +#include + +class AP_AngleSensor_AS5048B : public AP_AngleSensor_Backend +{ +public: + // constructor + AP_AngleSensor_AS5048B(AP_AngleSensor &frontend, uint8_t instance, AP_AngleSensor::AngleSensor_State &state); + + // update state + void update(void) override; + + bool init(void); + +private: + + // pointer to I2C device + AP_HAL::OwnPtr _dev = nullptr; + + void timer(void); + +}; + +#endif // AP_ANGLESENSOR_ENABLED diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_Backend.cpp b/libraries/AP_AngleSensor/AP_AngleSensor_Backend.cpp new file mode 100644 index 0000000000000..65921f8b00c2e --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_Backend.cpp @@ -0,0 +1,38 @@ +/* + 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_AngleSensor_Backend.h" + +#if AP_ANGLESENSOR_ENABLED + +#include "AP_AngleSensor.h" + +// base class constructor. +AP_AngleSensor_Backend::AP_AngleSensor_Backend(AP_AngleSensor &frontend, uint8_t instance, AP_AngleSensor::AngleSensor_State &state) : + _frontend(frontend), + _state(state) +{ + state.instance = instance; +} + +// copy state to front end helper function +void AP_AngleSensor_Backend::copy_state_to_frontend(float angle_radians, uint8_t quality, uint32_t last_reading_ms) +{ + _state.angle_radians = angle_radians; + _state.quality = quality; + _state.last_reading_ms = last_reading_ms; +} + +#endif // AP_ANGLESENSOR_ENABLED \ No newline at end of file diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_Backend.h b/libraries/AP_AngleSensor/AP_AngleSensor_Backend.h new file mode 100644 index 0000000000000..9284ff6c256ff --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_Backend.h @@ -0,0 +1,47 @@ +/* + 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_AngleSensor.h" +#include + +#if AP_ANGLESENSOR_ENABLED + +class AP_AngleSensor_Backend +{ +public: + // constructor. This incorporates initialisation as well. + AP_AngleSensor_Backend(AP_AngleSensor &frontend, uint8_t instance, AP_AngleSensor::AngleSensor_State &state); + + // we declare a virtual destructor so that AngleSensor drivers can + // override with a custom destructor if need be + virtual ~AP_AngleSensor_Backend(void) {} + + // update the state structure. All backends must implement this. + virtual void update() = 0; + +protected: + + // copy state to front end helper function + void copy_state_to_frontend(float angle_radians, uint8_t quality, uint32_t last_reading_ms); + + AP_AngleSensor &_frontend; + AP_AngleSensor::AngleSensor_State &_state; + + // semaphore for access to shared frontend data + HAL_Semaphore _sem; +}; + +#endif // AP_ANGLESENSOR_ENABLED diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_Params.cpp b/libraries/AP_AngleSensor/AP_AngleSensor_Params.cpp new file mode 100644 index 0000000000000..a04aa89f144aa --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_Params.cpp @@ -0,0 +1,54 @@ +#include "AP_AngleSensor_config.h" + +#if AP_ANGLESENSOR_ENABLED + +#include "AP_AngleSensor.h" +#include "AP_AngleSensor_Params.h" + + +// table of user settable parameters +const AP_Param::GroupInfo AP_AngleSensor_Params::var_info[] = { + // @Param: TYPE + // @DisplayName: Angle Sensor type + // @Description: What type of Angle Sensor is connected + // @Values: 0:None,1:AS5048B + // @User: Standard + AP_GROUPINFO_FLAGS("TYPE", 0, AP_AngleSensor_Params, _type, ANGLE_SENSOR_TYPE_NONE, AP_PARAM_FLAG_ENABLE), + + // @Param: BUS + // @DisplayName: Angle Sensor Serial Bus Index + // @Description: Angle Sensor Serial Bus Index + // @Values: 1: I2C1, 2: I2C2, 3: I2C3 + // @User: Standard + AP_GROUPINFO("BUS", 4, AP_AngleSensor_Params, _bus, ANGLE_SENSOR_BUS_DEFAULT), + + // @Param: ADDR + // @DisplayName: Serial Bus Address + // @Description: Serial Bus Address + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ADDR", 5, AP_AngleSensor_Params, _addr, ANGLE_SENSOR_ADDR_DEFAULT), + + // @Param: OFFS + // @DisplayName: Zero Position Offset, in Degrees + // @Description: This offset is added to the measured angle + // @Units: deg + // @Increment: 0.001 + // @User: Standard + AP_GROUPINFO("OFFS", 6, AP_AngleSensor_Params, _offset, ANGLE_SENSOR_DEFAULT_OFFSET), + + // @Param: DIR + // @DisplayName: Axis Direction + // @Description: Axis Direction, set to -1 to reverse + // @Values: -1: Reversed, 1: Default + // @User: Standard + AP_GROUPINFO("DIR", 7, AP_AngleSensor_Params, _direction, ANGLE_SENSOR_DIRECTION_FORWARDS), + + AP_GROUPEND +}; + +AP_AngleSensor_Params::AP_AngleSensor_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} + +#endif //AP_ANGLESENSOR_ENABLED diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_Params.h b/libraries/AP_AngleSensor/AP_AngleSensor_Params.h new file mode 100644 index 0000000000000..705e41854b88c --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_Params.h @@ -0,0 +1,29 @@ +#pragma once + +#include "AP_AngleSensor_config.h" + +#if AP_ANGLESENSOR_ENABLED + +#include +#include "AP_AngleSensor_config.h" + + +class AP_AngleSensor_Params{ +public: + + AP_AngleSensor_Params(void); + + static const struct AP_Param::GroupInfo var_info[]; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_AngleSensor_Params); + + // parameters for each instance + AP_Int8 _type; + AP_Int8 _bus; + AP_Int8 _addr; + AP_Float _offset; + AP_Int8 _direction; +}; + +#endif // AP_ANGLESENSOR_ENABLED \ No newline at end of file diff --git a/libraries/AP_AngleSensor/AP_AngleSensor_config.h b/libraries/AP_AngleSensor/AP_AngleSensor_config.h new file mode 100644 index 0000000000000..507b1dc2d7516 --- /dev/null +++ b/libraries/AP_AngleSensor/AP_AngleSensor_config.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#ifndef AP_ANGLESENSOR_ENABLED +#define AP_ANGLESENSOR_ENABLED 0 //DISABLED by default +#endif + +#define ANGLE_SENSOR_MAX_INSTANCES 2 +#define ANGLE_SENSOR_BUS_DEFAULT 0 +#define ANGLE_SENSOR_ADDR_DEFAULT 0x40 +#define ANGLE_SENSOR_DEFAULT_OFFSET 0 +#define ANGLE_SENSOR_DIRECTION_FORWARDS 1 diff --git a/libraries/AP_AngleSensor/LogStructure.h b/libraries/AP_AngleSensor/LogStructure.h new file mode 100644 index 0000000000000..f57c63b77806e --- /dev/null +++ b/libraries/AP_AngleSensor/LogStructure.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include "AP_AngleSensor_config.h" + +#define LOG_IDS_FROM_ANGLESENSOR \ + LOG_ANGLESENSOR_MSG + +// @LoggerMessage: AENC +// @Description: Angle Sensor Status +// @Field: TimeUS: Time since system startup +// @Field: Angle0: Absolute angle of axis 0 +// @Field: Qual0: Measurement quality for axis 0 +// @Field: Angle1: Absolute angle of axis 1 +// @Field: Qual1: Measurement quality for axis 1 + + +struct PACKED log_AngleSensor { + LOG_PACKET_HEADER; + uint64_t time_us; + float angle_0; + uint8_t quality_0; + float angle_1; + uint8_t quality_1; +}; + +#if !AP_ANGLESENSOR_ENABLED +#define LOG_STRUCTURE_FROM_ANGLESENSOR +#else +#define LOG_STRUCTURE_FROM_ANGLESENSOR \ + { LOG_ANGLESENSOR_MSG, sizeof(log_AngleSensor), \ + "AENC", "Qfbfb", "TimeUS,Angle0,Qual0,Angle1,Qual1", "sr%r%", "F0-0-" , true }, +#endif diff --git a/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/AngleSensor_Test.cpp b/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/AngleSensor_Test.cpp new file mode 100644 index 0000000000000..764e96604df17 --- /dev/null +++ b/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/AngleSensor_Test.cpp @@ -0,0 +1,74 @@ +/* + * RangeFinder test code + */ + +#include +#include +#include +#include + +const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { + AP_GROUPEND +}; +GCS_Dummy _gcs; + +void setup(); +void loop(); + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +#if AP_ANGLESENSOR_ENABLED + static AP_AngleSensor angle_sensor_driver; +#endif + +void setup() +{ + #if AP_ANGLESENSOR_ENABLED + // print welcome message + hal.console->printf("Angle Sensor library test\n"); + + // setup for analog pin 13 + AP_Param::set_object_value(&angle_sensor_driver, angle_sensor_driver.var_info, "_TYPE", (uint8_t)AP_AngleSensor::AngleSensor_Type::ANGLESENSOR_TYPE_AS5048B); + AP_Param::set_object_value(&angle_sensor_driver, angle_sensor_driver.var_info, "_BUS", (uint8_t)0); + AP_Param::set_object_value(&angle_sensor_driver, angle_sensor_driver.var_info, "_OFFS", (float)-354.57); + AP_Param::set_object_value(&angle_sensor_driver, angle_sensor_driver.var_info, "_DIR", (int8_t)-1); + + // initialise sensor, delaying to make debug easier + hal.scheduler->delay(2000); + angle_sensor_driver.init(); + hal.console->printf("Angle Sensor: %d device(s) initialized\n", angle_sensor_driver.num_sensors()); + #endif +} + +void loop() +{ + #if AP_ANGLESENSOR_ENABLED + // Delay between reads + hal.scheduler->delay(100); + angle_sensor_driver.update(); + + bool had_data = false; + for (uint8_t i=0; iprintf("device_%u type %d not healthy\n", + i, + (int)angle_sensor_driver.get_type(i)); + continue; + } + hal.console->printf("device_%u type %d angle %f quality %d\n", + i, + (int)angle_sensor_driver.get_type(i), + (float)angle_sensor_driver.get_angle_radians(i)*180/M_PI, + (int)angle_sensor_driver.get_signal_quality(i)); + had_data = true; + } + if (!had_data) { + hal.console->printf("All: no data on any sensor\n"); + } + #else + hal.console->printf("AngleSensor not available\n"); + hal.scheduler->delay(1000); + #endif + +} +AP_HAL_MAIN(); diff --git a/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/wscript b/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/wscript new file mode 100644 index 0000000000000..719ec151ba9d4 --- /dev/null +++ b/libraries/AP_AngleSensor/examples/AbsoluteEncoder_Test/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_example( + use='ap', + ) From ea0fa122a009fe6e80a406fe3fae6ca3dcbb8bd8 Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:21:53 -0800 Subject: [PATCH 2/6] AP_Logger: Add AngleSensor Log --- libraries/AP_Logger/LogStructure.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 1259c6417cd75..9fb86eff426a1 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -143,6 +143,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format // It is packed on ChibiOS to save flash space; however, this causes problems @@ -1296,6 +1297,7 @@ LOG_STRUCTURE_FROM_FENCE \ LOG_STRUCTURE_FROM_VISUALODOM \ { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \ "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" , true }, \ +LOG_STRUCTURE_FROM_ANGLESENSOR \ { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \ "WENC", "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1", "sm-m-", "F0-0-" , true }, \ { LOG_ADSB_MSG, sizeof(log_ADSB), \ @@ -1412,6 +1414,7 @@ enum LogMessages : uint8_t { LOG_RCOUT2_MSG, LOG_RCOUT3_MSG, LOG_IDS_FROM_FENCE, + LOG_IDS_FROM_ANGLESENSOR, _LOG_LAST_MSG_ }; From acdd40989a554d3f60ca2eb1c37a276dc1ee8dc5 Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:22:43 -0800 Subject: [PATCH 3/6] Tools: add AP_AngleSensor Build --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 3 files changed, 3 insertions(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 75c9975b826bd..a5f37ca8f574e 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -116,6 +116,7 @@ 'AP_OpenDroneID', 'AP_CheckFirmware', 'AP_ExternalControl', + 'AP_AngleSensor', ] def get_legacy_defines(sketch_name, bld): diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 7dc8f69cfe6a8..c01044fa38422 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -298,6 +298,7 @@ def __init__(self, Feature('Sensors', 'BEACON', 'AP_BEACON_ENABLED', 'Enable Beacon', 0, None), Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None), Feature('Sensors', 'IMU_ON_UART', 'AP_SERIALMANAGER_IMUOUT_ENABLED', 'Enable sending raw IMU data on a serial port', 0, None), # NOQA: E501 + Feature('Sensors', 'ANGLESENSOR', 'AP_ANGLESENSOR_ENABLED', 'Enable Angle Sensor Sensors', 0, None), Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight Gyro FFT calculations', 0, None), Feature('Other', 'DISPLAY', 'HAL_DISPLAY_ENABLED', 'Enable I2C Displays', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index fa2b0378710eb..cf06506f69671 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -237,6 +237,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_TUNING_ENABLED', 'AP_Tuning::check_input'), ('AP_DRONECAN_SERIAL_ENABLED', 'AP_DroneCAN_Serial::update'), ('AP_SERIALMANAGER_IMUOUT_ENABLED', 'AP_InertialSensor::send_uart_data'), + ('AP_ANGLESENSOR_ENABLED', 'AP_AngleSensor::AP_AngleSensor'), ] def progress(self, msg): From 23b64dabfe88b02e085e54544ce2220afa84d5cf Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:23:23 -0800 Subject: [PATCH 4/6] AP_Vehicle: add AngleSensor support --- libraries/AP_Vehicle/AP_Vehicle.cpp | 13 +++++++++++++ libraries/AP_Vehicle/AP_Vehicle.h | 5 +++++ 2 files changed, 18 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 9908cf767d60f..0c1a8f9f9b7d6 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -251,6 +251,12 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { // @Path: ../Filter/AP_Filter.cpp AP_SUBGROUPINFO(filters, "FILT", 26, AP_Vehicle, AP_Filters), #endif + +#if AP_ANGLESENSOR_ENABLED + // @Group: AENC + // @Path: ../AP_AngleSensor/AP_AngleSensor.cpp + AP_SUBGROUPINFO(anglesensor, "ANG", 27, AP_Vehicle, AP_AngleSensor), +#endif AP_GROUPEND }; @@ -433,6 +439,10 @@ void AP_Vehicle::setup() filters.init(); #endif +#if AP_ANGLESENSOR_ENABLED + anglesensor.init(); +#endif + #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED for (uint8_t i = 0; i #include #include +#include class AP_DDS_Client; @@ -409,6 +410,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_TemperatureSensor temperature_sensor; #endif +#if AP_ANGLESENSOR_ENABLED + AP_AngleSensor anglesensor; +#endif + static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Scheduler::Task scheduler_tasks[]; From 6f02de6682338bbbc2913813880c0c939b7e2613 Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:23:45 -0800 Subject: [PATCH 5/6] AP_WindVane: Add Support for AngleSensor Backend This adds a backend for AP_Windvane using the new AP_AngleSensor driver. The "DIR_PIN" param is overloaded to select the absolute encoder backend. --- libraries/AP_WindVane/AP_WindVane.cpp | 13 ++++-- libraries/AP_WindVane/AP_WindVane.h | 10 +++-- .../AP_WindVane/AP_WindVane_AngleSensor.cpp | 41 +++++++++++++++++++ .../AP_WindVane/AP_WindVane_AngleSensor.h | 36 ++++++++++++++++ libraries/AP_WindVane/AP_WindVane_config.h | 8 ++++ 5 files changed, 102 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_WindVane/AP_WindVane_AngleSensor.cpp create mode 100644 libraries/AP_WindVane/AP_WindVane_AngleSensor.h diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index 3b3cb71835242..4cfce16bae15a 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -26,6 +26,7 @@ #include "AP_WindVane_RPM.h" #include "AP_WindVane_SITL.h" #include "AP_WindVane_NMEA.h" +#include "AP_WindVane_AngleSensor.h" #include #include @@ -40,7 +41,7 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Param: TYPE // @DisplayName: Wind Vane Type // @Description: Wind Vane type - // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,10:SITL true,11:SITL apparent + // @Values: 0:None,1:Heading when armed,2:RC input offset heading when armed,3:Analog,4:NMEA,5:AngleSensor,10:SITL true,11:SITL apparent // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE), @@ -49,8 +50,8 @@ const AP_Param::GroupInfo AP_WindVane::var_info[] = { // @Param: DIR_PIN // @DisplayName: Wind vane analog voltage pin for direction - // @Description: Analog input pin to read as wind vane direction - // @Values: 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS + // @Description: Analog input pin to read as wind vane direction. If the Wind Vane type is Angle Sensor, this parameter selects the Angle Sensor Instance. + // @Values: 0: AENC1, 1:AENC2, 11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS // @User: Standard AP_GROUPINFO("DIR_PIN", 3, AP_WindVane, _dir_analog_pin, WINDVANE_DEFAULT_PIN), @@ -228,9 +229,15 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager) _direction_driver = new AP_WindVane_NMEA(*this); _direction_driver->init(serial_manager); break; +#endif +#if AP_WINDVANE_ANGLESENSOR_ENABLED + case WindVaneType::WINDVANE_ANGLESENSOR: + _direction_driver = new AP_WindVane_AngleSensor(*this); + break; #endif } + // wind speed switch (_speed_sensor_type) { case Speed_type::WINDSPEED_NONE: diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index 148f072e867a5..dd22b61913789 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -45,6 +45,7 @@ class AP_WindVane friend class AP_WindVane_Airspeed; friend class AP_WindVane_RPM; friend class AP_WindVane_NMEA; + friend class AP_WindVane_AngleSensor; public: AP_WindVane(); @@ -176,9 +177,12 @@ class AP_WindVane #if AP_WINDVANE_NMEA_ENABLED WINDVANE_NMEA = 4, #endif -#if AP_WINDVANE_SIM_ENABLED - WINDVANE_SITL_TRUE = 10, - WINDVANE_SITL_APPARENT = 11, +#if AP_WINDVANE_ANGLESENSOR_ENABLED + WINDVANE_ANGLESENSOR = 5, +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + WINDVANE_SITL_TRUE = 10, + WINDVANE_SITL_APPARENT = 11, #endif }; diff --git a/libraries/AP_WindVane/AP_WindVane_AngleSensor.cpp b/libraries/AP_WindVane/AP_WindVane_AngleSensor.cpp new file mode 100644 index 0000000000000..64fa61784755e --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_AngleSensor.cpp @@ -0,0 +1,41 @@ +/* + 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_WindVane_config.h" + +#if AP_WINDVANE_ANGLESENSOR_ENABLED + +#include "AP_WindVane_AngleSensor.h" + +// constructor +AP_WindVane_AngleSensor::AP_WindVane_AngleSensor(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ + _encoder_instance = _frontend._dir_analog_pin; +} + +void AP_WindVane_AngleSensor::update_direction() +{ + _encoder_instance = _frontend._dir_analog_pin; // Allow Runtime parameter update + + const AP_AngleSensor* angle_sensor_driver = AP_AngleSensor::get_singleton(); + if (angle_sensor_driver != nullptr) { + if (angle_sensor_driver->healthy(_encoder_instance)) { + _frontend._direction_apparent_raw = angle_sensor_driver->get_angle_radians(_encoder_instance); + } + } +} + +#endif //AP_WINDVANE_ANGLESENSOR_ENABLED \ No newline at end of file diff --git a/libraries/AP_WindVane/AP_WindVane_AngleSensor.h b/libraries/AP_WindVane/AP_WindVane_AngleSensor.h new file mode 100644 index 0000000000000..47753b5d5ff24 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_AngleSensor.h @@ -0,0 +1,36 @@ +/* + 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 + +#if AP_WINDVANE_ANGLESENSOR_ENABLED + +#include "AP_WindVane_Backend.h" + +#include + +class AP_WindVane_AngleSensor : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_AngleSensor(AP_WindVane &frontend); + + // update state + void update_direction() override; + +private: + uint8_t _encoder_instance = 0; +}; + +#endif //AP_WINDVANE_ANGLESENSOR_ENABLED \ No newline at end of file diff --git a/libraries/AP_WindVane/AP_WindVane_config.h b/libraries/AP_WindVane/AP_WindVane_config.h index 2c1e0f46c8382..c9c4bd4913074 100644 --- a/libraries/AP_WindVane/AP_WindVane_config.h +++ b/libraries/AP_WindVane/AP_WindVane_config.h @@ -4,6 +4,7 @@ #include #include +#include #ifndef AP_WINDVANE_ENABLED #define AP_WINDVANE_ENABLED 1 @@ -37,6 +38,13 @@ #define AP_WINDVANE_RPM_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_RPM_ENABLED #endif +#ifndef AP_WINDVANE_ANGLESENSOR_ENABLED +#define AP_WINDVANE_ANGLESENSOR_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_ANGLESENSOR_ENABLED +#endif + #ifndef AP_WINDVANE_SIM_ENABLED #define AP_WINDVANE_SIM_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED #endif + + + From 375e7694417983a2fed8778424d933595fc57031 Mon Sep 17 00:00:00 2001 From: David Ingraham Date: Fri, 29 Dec 2023 21:25:01 -0800 Subject: [PATCH 6/6] Rover: Enable AngleSensor by default if windvane is enabled --- Rover/config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Rover/config.h b/Rover/config.h index 7a280e88f3497..0cc3e842754c7 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -66,6 +66,11 @@ # define MODE_FOLLOW_ENABLED AP_FOLLOW_ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Angle Sensor Driver - useful for wind vanes +#ifndef ANGLESENSOR_ENABLED + #define ANGLESENSOR_ENABLED ENABLED +#endif ////////////////////////////////////////////////////////////////////////////// // Developer Items