위치 센서 (AS5600)

AS5600의 현재 각도, 속도 연산

static PositionSensor_t sensor; static 연산에 주의할 것. static으로 선언되어서 main()에서 PositionSensor_t sensor를 불러 올 수 없음. PositionSensor_t *PositionSensor_GetHandle(void) 함수를 main()에서 이용해야 함. 

 

• static → internal linkage (only visible inside its own .c file)
• extern → external linkage (visible to other files)

 

 

position_sensor.c

 

#include "position_sensor.h"

#include <stdlib.h>

#include <math.h>

 

#define CPR_12BIT 4096.0f

#define TWO_PI (2.0f * M_PI)

 

static PositionSensor_t sensor;

 

// ------------------------------------------------------------

// Initialization

// ------------------------------------------------------------

int PositionSensor_Init(void)

{

sensor.as5600 = AS5600_New();

sensor.as5600->i2cHandle = &hi2c1;

 

 

if (AS5600_Init(sensor.as5600) != HAL_OK)

return 0;

 

uint16_t init_angle;

AS5600_GetAngle(sensor.as5600, &init_angle);

 

sensor.last_angle_raw = init_angle;

sensor.last_timestamp = __HAL_TIM_GET_COUNTER(&htim6);

 

sensor.angle_rad = 0.0f;

sensor.angle_prev_rad = 0.0f;

sensor.full_rotation_offset = 0.0f;

 

return 1;

}

 

// ------------------------------------------------------------

// Update angle + velocity

// ------------------------------------------------------------

void PositionSensor_Update(void)

{

uint16_t angle_raw;

AS5600_GetAngle(sensor.as5600, &angle_raw);

 

 

// timestamp handling

uint16_t now = __HAL_TIM_GET_COUNTER(&htim6);

 

float dt_us = (now >= sensor.last_timestamp)

? (now - sensor.last_timestamp)

: (0xFFFF - sensor.last_timestamp + now);

 

float dt_s = dt_us * 1e-6f;

 

// compute delta angle (raw 0–4095)

float d_raw = (float)angle_raw - (float)sensor.last_angle_raw;

 

// detect wrap-around (±0.8*CPR)

if (fabsf(d_raw) > (0.8f * CPR_12BIT))

sensor.full_rotation_offset += (d_raw > 0) ? -TWO_PI : TWO_PI;

 

sensor.last_angle_raw = angle_raw;

 

// absolute angle (multi-turn)

float angle_now = sensor.full_rotation_offset +

((float)angle_raw / CPR_12BIT) * TWO_PI;

 

sensor.angle_rad = angle_now;

sensor.angle_deg = angle_now * (180.0f / M_PI);

 

// velocity calc

sensor.velocity_rad = (sensor.angle_rad - sensor.angle_prev_rad) / dt_s;

sensor.velocity_deg = (sensor.angle_deg - sensor.angle_prev_deg) / dt_s;

 

sensor.angle_prev_rad = sensor.angle_rad;

sensor.angle_prev_deg = sensor.angle_deg;

 

sensor.last_timestamp = now;

 

}

 

// ------------------------------------------------------------

// Getters

// ------------------------------------------------------------

float PositionSensor_GetRadians(void)

{

return sensor.angle_rad;

}

 

float PositionSensor_GetRadiansMultiturn(void)

{

return sensor.angle_rad;

}

 

float PositionSensor_GetDegrees(void)

{

return sensor.angle_deg;

}

 

float PositionSensor_GetDegreesMultiturn(void)

{

return sensor.angle_deg;

}

 

float PositionSensor_GetVelocityRad(void)

{

return sensor.velocity_rad;

}

 

float PositionSensor_GetVelocityDeg(void)

{

return sensor.velocity_deg;

}

 

PositionSensor_t *PositionSensor_GetHandle(void)

{

return &sensor;

}

 

 

position_sensor.h

 

#ifndef POSITION_SENSOR_H

#define POSITION_SENSOR_H

 

#include "stm32g4xx_hal.h"

#include "as5600.h"

 

typedef struct

{

AS5600_TypeDef *as5600; //as5600 : Pointer to AS5600 low-level driver structure.

 

uint16_t last_angle_raw; // Previous raw 12-bit angle reading (0–4095).

uint16_t last_timestamp; // Timestamp (timer ticks) of previous update.

 

float angle_rad; // Current multi-turn angle in radians.

float angle_deg; // Current multi-turn angle in degrees.

 

float angle_prev_rad; // Previous angle in radians (used for velocity).

float angle_prev_deg; // Previous angle in degrees.

 

float velocity_rad; // Angular velocity in radians per second.

float velocity_deg; // Angular velocity in degrees per second.

 

float full_rotation_offset; // Number of added/subtracted 2π rotations to track multi-turn angle when wrapping occurs.

} PositionSensor_t;

 

 

extern I2C_HandleTypeDef hi2c1;

extern TIM_HandleTypeDef htim6; // us timer

 

int PositionSensor_Init(void);

void PositionSensor_Update(void);

 

// Simple getters for safe read-only API, Simple getters are safe

float PositionSensor_GetRadians(void);

float PositionSensor_GetRadiansMultiturn(void);

float PositionSensor_GetDegrees(void);

float PositionSensor_GetDegreesMultiturn(void);

float PositionSensor_GetVelocityRad(void);

float PositionSensor_GetVelocityDeg(void);

 

// Pointer getter for advanced code (FOC loop, debugging)

// Pointer is powerful, but external code can modify pointer values.

// Pointer is faster than calling simple getters function

PositionSensor_t* PositionSensor_GetHandle(void);

 

#endif