Add NUSense Fan Controller functionality

NUSense/Core/Inc/fan_controller.h

@@ -0,0 +1,83 @@
+#ifndef FAN_CONTROLLER_H
+#define FAN_CONTROLLER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "main.h"
+#include <stdint.h>
+#include <stdbool.h>
+
+// Fan configuration
+#define PULSES_PER_REVOLUTION 4 // Fan tachometer pulse count that corresponds to one revolution
+#define FAN_RPM_WARNING 100 // RPM
+
+// Fan Controller I2C Address
+#define FAN_CONTROLLER_ADDRESS 0xA0
+#define DEFAULT_FAN_SPEED 0xFF // full steam ahead
+
+// Registers
+#define REG_CONTROL1  0x00
+#define REG_CONTROL2  0x01
+#define REG_CONTROL3  0x02
+#define REG_FAN1COUNT 0x52 // MSB of 2 byte fan controller tachometer count for fan 1
+#define REG_FAN2COUNT 0x54 // MSB of 2 byte fan controller tachometer count for fan 2
+#define REG_PWMR      0x50 // Direct Duty-Cycle Control Register
+#define REG_STATUS    0x5A // Status Register
+
+// Functions
+
+/// @brief Initializes the fan controller with the following settings:
+///         - Manual mode enabled (Direct Fan Control enabled)
+///         - Both tachometers enabled
+///         - Manual PWM set to full speed (0xFF)
+void fan_controller_init();
+
+/// @brief Reads a value from a fan controller register.
+/// @param reg The register address to read from.
+/// @return The value read from the register.
+uint8_t read_fan_register(uint8_t reg);
+
+/// @brief Writes a value to a fan controller register.
+/// @param reg The register address to write to.
+/// @param value The value to write.
+/// @return The HAL status of the write operation.
+uint8_t write_fan_register(uint8_t reg, uint8_t value);
+
+/// @brief Checks the fan warning states for the specified fan.
+/// @param fan_id The ID of the fan to check (0 for Fan 1 (J401 on NUSense), 1 for Fan 2 (J402 on NUSense)) 
+/// @return The warning state of the specified fan. Returns true if there is a warning, false if there is no warning.
+bool fan_warning_state(uint8_t fan_id);
+
+/// @brief Sets the PWM frequency of the fan. The frequency is determined by bits 3 and 4 of Control Register 1.
+/// @param freq The desired frequency setting (0b00 for 33Hz, 0b01 for 150Hz, 0b10 for 1500Hz, 0b11 for 25kHz)
+void set_fan_pwm_freq(uint8_t freq);
+
+/// @brief Sets the fan mode between manual and automatic.
+/// @param mode true for manual mode (Direct Fan Control enabled), false for automatic mode (Direct Fan Control disabled)
+void set_fan_mode(bool mode);
+
+/// @brief Sets the fan spin-up mode.
+/// @param enabled true to enable spin-up mode, false to disable
+void set_fan_spin_up(bool enabled);
+
+/// @brief Sets the manual PWM value for the fan.
+/// @param pwm_value
+void set_fan_manual_pwm(uint8_t pwm_value);
+
+/// @brief reads the fan speed from the controller
+/// @param tachometer 0 for Fan 1, 1 for Fan 2
+/// @return The speed in RPM.
+uint16_t read_fan_speed(uint8_t tachometer);
+
+/// @brief Enables or disables the tachometer for the specified fan.
+/// @param tachometer  0 for Tachometer 1, 1 for Tachometer 2
+/// @param enabled true to enable, false to disable
+void set_fan_tachometer_enabled(uint8_t tachometer, bool enabled);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // FAN_CONTROLLER_H

NUSense/Core/Inc/i2c.h

@@ -0,0 +1,45 @@
+#ifndef I2C_H
+#define I2C_H
+
+#include "main.h"
+#include "stm32h7xx_hal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define I2C_SDA_Pin GPIO_PIN_9
+#define I2C_SDA_GPIO_Port GPIOC
+#define I2C_SCL_Pin GPIO_PIN_8
+#define I2C_SCL_GPIO_Port GPIOA
+
+extern I2C_HandleTypeDef hi2c3;
+
+/**
+ * @brief I2C3 Initialisation Function
+ * @param None
+ * @retval None
+ */
+void MX_I2C3_Init(void);
+
+/**
+ * @brief I2C MSP Initialisation
+ * This function configures the hardware resources used by I2C3
+ * @param hi2c: I2C handle pointer
+ * @retval None
+ */
+void HAL_I2C_MspInit(I2C_HandleTypeDef *hi2c);
+
+/**
+ * @brief GPIO Initialisation Function
+ * @param None
+ * @retval None
+ */
+void MX_GPIO_Init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // I2C_H
+

NUSense/Core/Src/fan_controller.c

@@ -0,0 +1,70 @@
+#include "fan_controller.h"
+#include <stdbool.h>
+extern I2C_HandleTypeDef hi2c3;
+
+uint8_t read_fan_register(uint8_t reg) {
+    uint8_t value = 0;
+    HAL_I2C_Mem_Read(&hi2c3, FAN_CONTROLLER_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, &value, 1, HAL_MAX_DELAY);
+    return value;
+}
+
+uint8_t write_fan_register(uint8_t reg, uint8_t value) {
+    return HAL_I2C_Mem_Write(&hi2c3, FAN_CONTROLLER_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, &value, 1, HAL_MAX_DELAY);
+}
+
+bool fan_warning_state(uint8_t fan_id) {
+    // Warning state is true if fan speed is less than or equal to the defined warning threshold
+    return read_fan_speed(fan_id) <= FAN_RPM_WARNING;
+}
+
+void set_fan_pwm_freq(uint8_t freq) {
+    uint8_t control1 = read_fan_register(REG_CONTROL1);
+    control1 &= ~(0b11 << 3);       // clear bits 3 and 4
+    control1 |= (freq & 0b11) << 3; // set bits 3 and 4 to the desired frequency
+    write_fan_register(REG_CONTROL1, control1);
+}
+
+void set_fan_spin_up(bool enabled) {
+    uint8_t control2 = read_fan_register(REG_CONTROL2);
+    if (enabled) {
+        control2 |= (1 << 1); // set bit 1 to enable Spin-Up mode
+    }
+    else {
+        control2 &= ~(1 << 1); // clear bit 1 to disable Spin-Up mode
+    }
+    write_fan_register(REG_CONTROL2, control2);
+}
+
+void set_fan_tachometer_enabled(uint8_t tachometer, bool enabled) {
+    uint8_t control3 = read_fan_register(REG_CONTROL3);
+    if (enabled) {  
+        control3 |= (1 << tachometer);  
+    } 
+    else {  
+        control3 &= ~(1 << tachometer);  
+    }
+    write_fan_register(REG_CONTROL3, control3);
+}
+
+void set_fan_manual_pwm(uint8_t pwm_value) {
+    write_fan_register(REG_PWMR, pwm_value);
+}
+
+uint16_t read_fan_speed(uint8_t tachometer) {
+	uint8_t fan_register = (tachometer == 0 ? REG_FAN1COUNT : REG_FAN2COUNT);
+    uint8_t msb = read_fan_register(fan_register);
+	uint8_t lsb = read_fan_register(fan_register + 1);
+	uint16_t fan_count = (msb << 8) | lsb;
+	return 60 * 100000 / fan_count / PULSES_PER_REVOLUTION;
+}
+
+void fan_controller_init() {
+    set_fan_mode(true);
+
+    // Enable both tachometers
+    set_fan_tachometer_enabled(0, true);
+    set_fan_tachometer_enabled(1, true);
+
+    // Set Direct Duty-Cycle Control Register to full blast.
+    set_fan_manual_pwm(DEFAULT_FAN_SPEED);
+}

NUSense/Core/Src/i2c.c

@@ -0,0 +1,70 @@
+#include "i2c.h"
+
+I2C_HandleTypeDef hi2c3;
+
+void MX_I2C3_Init(void) {
+    hi2c3.Instance = I2C3;
+    hi2c3.Init.Timing = 0x00702681; // 400kHz I2C Fast Mode
+    hi2c3.Init.OwnAddress1 = 0;
+    hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c3.Init.OwnAddress2 = 0;
+    hi2c3.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+    hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c3) != HAL_OK) {
+        Error_Handler();
+    }
+    if (HAL_I2CEx_ConfigAnalogFilter(&hi2c3, I2C_ANALOGFILTER_ENABLE) != HAL_OK) {
+        Error_Handler();
+    }
+    if (HAL_I2CEx_ConfigDigitalFilter(&hi2c3, 0) != HAL_OK) {
+        Error_Handler();
+    }
+}
+
+void HAL_I2C_MspInit(I2C_HandleTypeDef *hi2c)
+{
+    GPIO_InitTypeDef GPIO_InitStruct = {0};
+    if (hi2c->Instance == I2C3) {
+        /* Peripheral clock enable */
+        __HAL_RCC_GPIOC_CLK_ENABLE();
+        __HAL_RCC_GPIOA_CLK_ENABLE();
+        __HAL_RCC_I2C3_CLK_ENABLE();
+
+        /**I2C3 GPIO Configuration
+        PC9     ------> I2C3_SDA
+        PA8     ------> I2C3_SCL
+        */
+        GPIO_InitStruct.Pin = I2C_SDA_Pin;
+        GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
+        HAL_GPIO_Init(I2C_SDA_GPIO_Port, &GPIO_InitStruct);
+
+        GPIO_InitStruct.Pin = I2C_SCL_Pin;
+        GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
+        HAL_GPIO_Init(I2C_SCL_GPIO_Port, &GPIO_InitStruct);
+    }
+}
+
+static void MX_GPIO_Init(void) {
+    /* USER CODE BEGIN MX_GPIO_Init_1 */
+
+    /* USER CODE END MX_GPIO_Init_1 */
+
+    /* GPIO Ports Clock Enable */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+
+    /* I2C GPIO configuration is handled in HAL_I2C_MspInit */
+
+    /* USER CODE BEGIN MX_GPIO_Init_2 */
+
+    /* USER CODE END MX_GPIO_Init_2 */
+}
+

NUSense/Core/Src/main.cpp

@@ -19,7 +19,9 @@
 #include "main.h"
 
 #include "dma.h"
+#include "fan_controller.h"
 #include "gpio.h"
+#include "i2c.h"
 #include "spi.h"
 #include "tim.h"
 #include "usart.h"
@@ -52,7 +54,7 @@ int main(void) {
     // order here given that it also has another strange habit of ignoring main.cpp and creating and
     // overwriting main.c instead despite CubeIDE knowing that this is a C++ project! Oh CubeMX ...
 
-    /* Initialize all configured peripherals */
+    /* Initialise all configured peripherals */
     MX_DMA_Init();
     MX_GPIO_Init();
     MX_USB_DEVICE_Init();
@@ -65,6 +67,10 @@ int main(void) {
     MX_USART6_UART_Init();
     MX_TIM1_Init();
     MX_TIM4_Init();
+    MX_I2C3_Init();
+
+    /* Initialise fan controller after I2C3 is ready */
+    fan_controller_init();
 
 #ifdef FIRST_BUZZ
     // Confirm that the programme is running.

NUSense/Core/Src/nusense/NUSenseIO.hpp

@@ -21,6 +21,7 @@
 #include "ChainManager.hpp"
 #include "NUgus.hpp"
 #include "ServoState.hpp"
+#include "fan_controller.h"
 #include "imu.h"
 
 namespace nusense {

NUSense/Core/Src/nusense/NUSenseIO/send_nusense_data.cpp

@@ -31,6 +31,10 @@ namespace nusense {
         nusense_msg.buttons.left   = mode_button.filter();
         nusense_msg.buttons.middle = start_button.filter();
 
+        nusense_msg.has_fan_warnings          = true;
+        nusense_msg.fan_warnings.fan1_warning = fan_warning_state(0) ? true : false;
+        nusense_msg.fan_warnings.fan2_warning = fan_warning_state(1) ? true : false;
+
         if (nusense_msg.buttons.left) {
             tx_led.pulse(1, false, device::Pulser::LOW);
         }

NUSense/Core/Src/usb/protobuf/NUSenseData.pb.c

@@ -21,6 +21,9 @@ PB_BIND(message_platform_IMU_fvec3, message_platform_IMU_fvec3, AUTO)
 PB_BIND(message_platform_Buttons, message_platform_Buttons, AUTO)
 
 
+PB_BIND(message_platform_FanWarning, message_platform_FanWarning, AUTO)
+
+
 PB_BIND(message_platform_NUSense, message_platform_NUSense, 2)
 
 
@@ -49,4 +52,3 @@ PB_BIND(message_platform_ServoIDStates_ServoIDState, message_platform_ServoIDSta
  */
 PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES)
 #endif
-