Development status Feb 9 before San Diego regatta

libraries/AP_WindVane/AP_WindVane.cpp

@@ -22,6 +22,7 @@
 #include "AP_WindVane_RPM.h"
 #include "AP_WindVane_SITL.h"
 #include "AP_WindVane_NMEA.h"
+#include "AP_WindVane_AS5600.h"
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -36,7 +37,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:AS5600,10:SITL true,11:SITL apparent
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_WindVane, _direction_type, 0, AP_PARAM_FLAG_ENABLE),
@@ -197,6 +198,8 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
         return;
     }
 
+    gcs().send_text(MAV_SEVERITY_ALERT, "WindVane: got _direction type %u", (unsigned int) _direction_type);
+
     // wind direction
     switch (_direction_type) {
         case WindVaneType::WINDVANE_NONE:
@@ -219,6 +222,10 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager)
             _direction_driver = new AP_WindVane_NMEA(*this);
             _direction_driver->init(serial_manager);
             break;
+        case WindVaneType::WINDVANE_AS5600:
+            _direction_driver = new AP_WindVane_AS5600(*this);
+            _direction_driver->init();
+            break;
     }
 
     // wind speed

libraries/AP_WindVane/AP_WindVane.h

@@ -41,6 +41,7 @@ class AP_WindVane
     friend class AP_WindVane_Airspeed;
     friend class AP_WindVane_RPM;
     friend class AP_WindVane_NMEA;
+    friend class AP_WindVane_AS5600;
 
 public:
     AP_WindVane();
@@ -166,6 +167,7 @@ class AP_WindVane
         WINDVANE_PWM_PIN        = 2,
         WINDVANE_ANALOG_PIN     = 3,
         WINDVANE_NMEA           = 4,
+        WINDVANE_AS5600        = 5,
         WINDVANE_SITL_TRUE      = 10,
         WINDVANE_SITL_APPARENT  = 11,
     };

libraries/AP_WindVane/AP_WindVane_AS5600.cpp

@@ -0,0 +1,136 @@
+/*
+   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_HAL/AP_HAL.h>
+#include "AP_WindVane_AS5600.h"
+#include <AP_HAL/utility/OwnPtr.h>
+#include <GCS_MAVLink/GCS.h>
+
+#define I2C_BUS 1
+
+#define RE_ADDRESS          0x36
+#define RE_RAW_ANGLE        0x0C
+#define AS5600_CONF         0x07
+
+//  CONFIGURATION BIT MASKS - byte level
+#define AS5600_CONF_POWER_MODE      0x03;
+
+extern const AP_HAL::HAL& hal;
+
+// init - performs any required initialization for this instance
+void AP_WindVane_AS5600::init()
+{   
+
+    // Defaults to absurdly high latency low power mode
+    _dev = hal.i2c_mgr->get_device(I2C_BUS, RE_ADDRESS); //, 400000U, false, 105);
+    if (!_dev) {
+        gcs().send_text(MAV_SEVERITY_INFO, "No AP_WindVane_AS5600 found.");
+        return;
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5600: Found on bus %u address 0x%02x",
+                        I2C_BUS, RE_ADDRESS);
+
+    _dev->set_retries(2);
+
+    setPowerMode(0x00);
+
+    calibrate();
+
+
+}
+
+// https://github.com/RobTillaart/AS5600/blob/master/AS5600.cpp
+bool AP_WindVane_AS5600::setPowerMode(uint8_t powerMode)
+{
+  if (powerMode > 3) return false;
+
+  _dev->set_retries(10);
+
+  WITH_SEMAPHORE(_dev->get_semaphore());
+
+  uint8_t value;
+  if (not _dev->read_registers((AS5600_CONF + 1), &value, 1))
+  {
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5600: Could not get CONF value to set power.");
+    _dev->set_retries(2);
+    return false;
+  }
+  gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5600: Got CONF value %02x",
+                        value);
+
+  value &= ~AS5600_CONF_POWER_MODE;
+  value |= powerMode;
+
+  bool written = _dev->write_register((AS5600_CONF + 1), value, true);
+  if (written)
+  {
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5600: Set CONF value %02x",
+                        value);
+  }
+  else
+  {
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5600: Could not set CONF value %02x",
+                        value);
+    _dev->set_retries(2);
+    return false;
+  }
+
+  _dev->set_retries(2);
+
+  return true;
+}
+
+uint16_t AP_WindVane_AS5600::readRawAngle()
+{
+
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    // Read two-byte angle (12-bit precision) and apply offset
+    uint16_t value;
+    if (!_dev->read_registers(RE_RAW_ANGLE, (uint8_t *) &value, 2)) {
+        gcs().send_text(MAV_SEVERITY_INFO,"AP_WindVane_AS5600: Reading the angle register failed.");
+        return 0x0FFF;
+    }
+
+    // gcs().send_text(MAV_SEVERITY_INFO,"AP_WindVane_AS5600: Got angle bytes %x.", value);
+
+    // Swap the bytes. Extract the first byte and shift it right. Extract the second byte 
+    // and shift it left. Bitwise OR the them together and you're set!
+    value = ((value & 0xff) << 8) | ((value & 0xff00) >> 8);
+
+    // Due to twelve-bit precision, we make sure we have a twelve-bit angle.
+    value &=  0x0FFF;
+
+    // gcs().send_text(MAV_SEVERITY_INFO,"AP_WindVane_AS5600: Interpreting angle %u.", value);
+
+    return value;
+}
+
+void AP_WindVane_AS5600::calibrate()
+{
+    _raw_angle_offset = readRawAngle();
+}
+
+void AP_WindVane_AS5600::update_direction()
+{
+
+    _wind_dir_rad = (float) (readRawAngle() + _raw_angle_offset) * 2.0 * M_PI / 4096.0;
+
+    _frontend._direction_apparent_raw = wrap_PI(_wind_dir_rad);
+
+    // gcs().send_text(MAV_SEVERITY_INFO, "Setting wind direction %f", _frontend._direction_apparent_raw);
+
+}

libraries/AP_WindVane/AP_WindVane_AS5600.h

@@ -0,0 +1,49 @@
+/*
+   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
+
+#include "AP_WindVane_Backend.h"
+#include <AP_HAL/I2CDevice.h>
+
+class AP_WindVane_AS5600 : public AP_WindVane_Backend
+{
+
+public:
+    // constructor
+    using AP_WindVane_Backend::AP_WindVane_Backend;
+    ~AP_WindVane_AS5600(void) {}
+
+    // initialization
+    void init() override;
+
+    // update state
+    void update_direction() override;
+
+    void calibrate() override;
+
+private:
+
+    uint16_t readRawAngle();
+
+    bool setPowerMode(uint8_t powerMode);
+
+    uint16_t _raw_angle_offset = 0;
+
+    // latest values read in
+    float _wind_dir_rad;
+
+    // pointer to I2C device
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev = nullptr;
+};

libraries/AP_WindVane/AP_WindVane_Backend.h

@@ -28,6 +28,7 @@ class AP_WindVane_Backend
 
     // initialization
     virtual void init(const AP_SerialManager& serial_manager) {};
+    virtual void init() {};
 
     // update the state structure
     virtual void update_speed() {};

libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp

@@ -41,7 +41,8 @@ void AP_WindVane_ModernDevice::update_speed()
             // pin invalid, don't have health monitoring to report yet
             return;
         }
-        analog_voltage = _temp_analog_source->voltage_average();
+        analog_voltage = _temp_analog_source->voltage_average();\
+        // gcs().send_text(MAV_SEVERITY_INFO, "Temp Voltage (Pin %u): %.2f", (uint8_t) _frontend._speed_sensor_temp_pin.get(), (float) analog_voltage);
         temp_ambient = (analog_voltage - 0.4f) / 0.0195f; // deg C
         // constrain to reasonable range to avoid deviating from calibration too much and potential divide by zero
         temp_ambient = constrain_float(temp_ambient, 10.0f, 40.0f);
@@ -56,12 +57,17 @@ void AP_WindVane_ModernDevice::update_speed()
     // apply voltage offset and make sure not negative
     // by default the voltage offset is the number provide by the manufacturer
     analog_voltage = _current_analog_voltage - _frontend._speed_sensor_voltage_offset;
+    // gcs().send_text(MAV_SEVERITY_INFO, "Speed sensor voltage offset: %.2f", (float) _frontend._speed_sensor_voltage_offset);
+
+    // gcs().send_text(MAV_SEVERITY_INFO, "Speed Voltage (Pin %u): %.2f", _frontend._speed_sensor_speed_pin.get(), analog_voltage);
     if (is_negative(analog_voltage)) {
         analog_voltage = 0.0f;
     }
 
     // simplified equation from data sheet, converted from mph to m/s
     _frontend._speed_apparent_raw  = 24.254896f * powf((analog_voltage / powf(temp_ambient, 0.115157f)), 3.009364f);
+
+    // gcs().send_text(MAV_SEVERITY_INFO, "Set speed to: %.2f", _frontend._speed_apparent_raw);
 }
 
 void AP_WindVane_ModernDevice::calibrate()

scratch/AP_WindVane_AS5048B.cpp

@@ -0,0 +1,121 @@
+/*
+   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_HAL/AP_HAL.h>
+#include "AP_WindVane_AS5048B.h"
+#include <AP_HAL/utility/OwnPtr.h>
+#include <GCS_MAVLink/GCS.h>
+
+#define I2C_BUS 0
+
+#define RE_ADDRESS          0x40
+#define RE_ZEROMSB_REG      0x16  
+// Zero, most significant byte
+#define RE_ZEROLSB_REG      0x17  
+// Zero, least significant byte
+#define RE_MAGNMSB_REG      0xFC  
+// Magnitude, most significant byte
+#define RE_MAGNLSB_REG      0xFD  
+// Magnitude, least significant byte
+#define RE_ANGLEMSB_REG     0xFE  
+// Angle, most significant byte
+#define RE_ANGLELSB_REG     0xFF  
+// Angle, least significant byte
+
+extern const AP_HAL::HAL& hal;
+
+// init - performs any required initialization for this instance
+void AP_WindVane_AS5048B::init()
+{   
+
+    _dev = hal.i2c_mgr->get_device(I2C_BUS, RE_ADDRESS);
+    if (!_dev) {
+        gcs().send_text(MAV_SEVERITY_INFO, "No AP_WindVane_AS5048B found.");
+        return;
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_WindVane_AS5048B: Found on bus %u address 0x%02x",
+                        I2C_BUS, RE_ADDRESS);
+
+    if(!_dev->get_semaphore()->take(10)){
+        return;
+    }
+    calibrate();
+    _dev->get_semaphore()->give();
+
+}
+
+void AP_WindVane_AS5048B::calibrate()
+{
+    _dev->set_retries(10);
+    // set initial position to zero by setting the zero register to zero,
+    // reading the measured angle, and setting the register to this angle.
+    int ret = _dev->write_register(RE_ZEROMSB_REG, 0x00);
+    if (!ret) {
+        return;
+    }
+    ret = _dev->write_register(RE_ZEROLSB_REG, 0x00);
+    if (!ret) {
+        return;
+    }
+    uint8_t msb = 0;
+    ret = _dev->read_registers(RE_ANGLEMSB_REG, &msb, 1);
+    if (!ret) {
+        return;
+    }
+    uint8_t lsb = 0;
+    ret = _dev->read_registers(RE_ANGLELSB_REG, &lsb, 1);
+    if (!ret) {
+        return;
+    }
+    ret = _dev->write_register(RE_ZEROMSB_REG, msb);
+    if (!ret) {
+        return;
+    }
+    ret = _dev->write_register(RE_ZEROLSB_REG, lsb);
+    if (!ret) {
+        return;
+    }
+    _dev->set_retries(2);
+}
+
+void AP_WindVane_AS5048B::update_direction()
+{
+    if(!_dev->get_semaphore()->take(2)){
+        return;
+    }
+
+    uint8_t msb = 0;
+    int ret = _dev->read_registers(RE_ANGLEMSB_REG, &msb, 1);
+    if (!ret) {
+        return;
+    }
+    uint8_t lsb = 0;
+    ret = _dev->read_registers(RE_ANGLELSB_REG, &lsb, 1);
+    if (!ret) {
+        return;
+    }
+
+    // bit shifting, see https://ams.com/documents/20143/36005/AS5048_DS000298_4-00.pdf p. 25
+    uint16_t angle = ((uint16_t) msb << 6) | (lsb & 0x3f);
+    _wind_dir_deg = (float) angle * 360.0 / 16384.0;
+
+    // user may not have AS5048B selected for direction, I think.
+    if (_frontend._direction_type.get() == _frontend.WindVaneType::WINDVANE_AS5048B) {
+        _frontend._direction_apparent_raw = wrap_PI(radians(_wind_dir_deg));
+    }
+
+    _dev->get_semaphore()->give();
+}