Skip to content

Commit

Permalink
AP_WindVane: Add Support for AbsoluteEncoder Backend
Browse files Browse the repository at this point in the history
This adds a backend for AP_Windvane using the new AP_AbsoluteEncoder driver. The "DIR_PIN" param is overloaded to select the absolute encoder backend.
  • Loading branch information
DavidIngraham committed Dec 12, 2023
1 parent 8fdba98 commit ca68fd9
Show file tree
Hide file tree
Showing 5 changed files with 102 additions and 6 deletions.
13 changes: 10 additions & 3 deletions libraries/AP_WindVane/AP_WindVane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
Expand All @@ -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),
Expand All @@ -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),

Expand Down Expand Up @@ -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:
Expand Down
10 changes: 7 additions & 3 deletions libraries/AP_WindVane/AP_WindVane.h
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
};

Expand Down
41 changes: 41 additions & 0 deletions libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.cpp
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>.
*/

#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
36 changes: 36 additions & 0 deletions libraries/AP_WindVane/AP_WindVane_AbsoluteEncoder.h
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once

#if AP_WINDVANE_ABSOLUTEENCODER_ENABLED

#include "AP_WindVane_Backend.h"

#include <AP_AbsoluteEncoder/AP_AbsoluteEncoder.h>

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
8 changes: 8 additions & 0 deletions libraries/AP_WindVane/AP_WindVane_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include <AP_Airspeed/AP_Airspeed_config.h>
#include <AP_RPM/AP_RPM_config.h>
#include <AP_AbsoluteEncoder/AP_AbsoluteEncoder_config.h>

#ifndef AP_WINDVANE_ENABLED
#define AP_WINDVANE_ENABLED 1
Expand Down Expand Up @@ -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



0 comments on commit ca68fd9

Please sign in to comment.