diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index 3b3cb718352426..b9610a0e5389bb 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_AbsoluteEncoder.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:AbsoluteEncoder,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 Absolute Encoder, this parameter selects the Absolute Encoder 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_ABSOLUTEENCODER_ENABLED + case WindVaneType::WINDVANE_ABSOLUTEENCODER: + _direction_driver = new AP_WindVane_AbsoluteEncoder(*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 148f072e867a53..1dfd55e62f2daf 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_AbsoluteEncoder; 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_ABSOLUTEENCODER_ENABLED + WINDVANE_ABSOLUTEENCODER = 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_AbsoluteEncoder.cpp b/libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.cpp new file mode 100644 index 00000000000000..5dc0415ec60735 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.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_ABSOLUTEENCODER_ENABLED + +#include "AP_WindVane_AbsoluteEncoder.h" + +// constructor +AP_WindVane_AbsoluteEncoder::AP_WindVane_AbsoluteEncoder(AP_WindVane &frontend) : + AP_WindVane_Backend(frontend) +{ + _encoder_instance = _frontend._dir_analog_pin; +} + +void AP_WindVane_AbsoluteEncoder::update_direction() +{ + _encoder_instance = _frontend._dir_analog_pin; // Allow Runtime parameter update + + const AP_AbsoluteEncoder* encoder_driver = AP_AbsoluteEncoder::get_singleton(); + if (encoder_driver != nullptr) { + if (encoder_driver->healthy(_encoder_instance)) { + _frontend._direction_apparent_raw = encoder_driver->get_angle_radians(_encoder_instance); + } + } +} + +#endif //AP_WINDVANE_ABSOLUTEENCODER_ENABLED \ No newline at end of file diff --git a/libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.h b/libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.h new file mode 100644 index 00000000000000..c7ba0541de5834 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.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_ABSOLUTEENCODER_ENABLED + +#include "AP_WindVane_Backend.h" + +#include + +class AP_WindVane_AbsoluteEncoder : public AP_WindVane_Backend +{ +public: + // constructor + AP_WindVane_AbsoluteEncoder(AP_WindVane &frontend); + + // update state + void update_direction() override; + +private: + uint8_t _encoder_instance = 0; +}; + +#endif //AP_WINDVANE_ABSOLUTEENCODER_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 2c1e0f46c83826..73a5c5d0f0c642 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_ABSOLUTEENCODER_ENABLED +#define AP_WINDVANE_ABSOLUTEENCODER_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_ABSOLUTEENCODER_ENABLED +#endif + #ifndef AP_WINDVANE_SIM_ENABLED #define AP_WINDVANE_SIM_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED #endif + + +