openpilot v0.9.6 release

date: 2024-02-21T23:02:42
master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5

body/board/drivers/angle_sensor.h

@@ -0,0 +1,69 @@
+// Default addresses for AS5048B
+#define AS5048_ADDRESS_LEFT 0x40
+#define AS5048_ADDRESS_RIGHT 0x41
+#define UNKNOWN_IMU 0x68
+
+#define AS5048B_PROG_REG 0x03
+#define AS5048B_ADDR_REG 0x15
+#define AS5048B_ZEROMSB_REG 0x16 //bits 0..7
+#define AS5048B_ZEROLSB_REG 0x17 //bits 0..5
+#define AS5048B_GAIN_REG 0xFA
+#define AS5048B_DIAG_REG 0xFB
+#define AS5048B_MAGNMSB_REG 0xFC //bits 0..7
+#define AS5048B_MAGNLSB_REG 0xFD //bits 0..5
+#define AS5048B_ANGLMSB_REG 0xFE //bits 0..7
+#define AS5048B_ANGLLSB_REG 0xFF //bits 0..5
+
+extern I2C_HandleTypeDef hi2c1;
+
+const uint8_t init_imu_regaddr[] = {0x76, 0x4c, 0x4e, 0x4f, 0x50, 0x51, 0x52, 0x53};
+const uint8_t init_imu_data[] =    {0x00, 0x12, 0x2f, 0x26, 0x67, 0x04, 0x00, 0x00};
+
+fault_status_t fault_status = {0};
+
+void angle_sensor_read(uint16_t *sensor_angle) {
+
+  if (fault_status.left_i2c && fault_status.right_i2c) { // Try to reinitialize halted I2C
+    if (HAL_I2C_Init(&hi2c1) == HAL_OK) {
+      fault_status.left_i2c = 0;
+      fault_status.right_i2c = 0;
+    }
+  }
+
+  uint8_t buf[2];
+  if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_LEFT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 10) == HAL_OK) {
+    sensor_angle[0] = (buf[0] << 6) | (buf[1] & 0x3F);
+    fault_status.left_i2c = 0;
+  } else {
+    fault_status.left_i2c = 1;
+  }
+  if (HAL_I2C_Mem_Read(&hi2c1, (AS5048_ADDRESS_RIGHT<<1), AS5048B_ANGLMSB_REG, I2C_MEMADD_SIZE_8BIT, buf, 2, 10) == HAL_OK) {
+    sensor_angle[1] = (buf[0] << 6) | (buf[1] & 0x3F);
+    fault_status.right_i2c = 0;
+  } else {
+    fault_status.right_i2c = 1;
+  }
+}
+
+void IMU_soft_init(void) {
+  i2c_port_init();
+
+  for (int8_t i = 3; i > 0; i--) {
+    SW_I2C_WriteControl_8Bit((UNKNOWN_IMU<<1), 0x75, 0x00);
+  }
+  for (int8_t i = sizeof(init_imu_regaddr)-1; i >= 0; i--) {
+    SW_I2C_WriteControl_8Bit((UNKNOWN_IMU<<1), init_imu_regaddr[i], init_imu_data[i]);
+  }
+}
+
+void IMU_soft_sensor_read(uint16_t *unknown_imu_data) {
+  static uint8_t buf[12];
+
+  SW_I2C_WriteControl_8Bit((UNKNOWN_IMU<<1), 0x76, 0x00);
+  SW_I2C_Multi_ReadnControl_8Bit((UNKNOWN_IMU<<1), 0x1F, 6, buf);
+  SW_I2C_Multi_ReadnControl_8Bit((UNKNOWN_IMU<<1), 0x25, 6, &buf[6]);
+
+  for (int8_t i = 5; i >= 0; i--) {
+    unknown_imu_data[i] = (buf[i*2] << 8) | (buf[(i*2)+1]);
+  }
+}