diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 2c41265..4fb07df 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/Core/Inc/gc9a01.h b/Core/Inc/gc9a01.h
index ad0bc1e..4f33474 100644
--- a/Core/Inc/gc9a01.h
+++ b/Core/Inc/gc9a01.h
@@ -1,38 +1,48 @@
-/*
- * gc9a01.h
- *
- * Created on: Jul 24, 2025
- * Author: loren
+/**
+ * @file gc9a01.h
+ * @brief Driver GC9A01 pour STM32 (HAL)
+ * @date 09 août 2025
+ * @author Lorent
*/
+
#ifndef INC_GC9A01_H_
#define INC_GC9A01_H_
-
#include "main.h"
#include
#include
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define PIX_BUF 512
+
+
//==============================================================================
-// Configuration des pins (à adapter selon votre câblage)
+// 📌 Configuration matérielle
//==============================================================================
+
+// Chip Select (CS)
#define GC9A01_CS_GPIO_Port GPIOB
#define GC9A01_CS_Pin GPIO_PIN_0
+
+// Data/Command (DC)
#define GC9A01_DC_GPIO_Port GPIOB
#define GC9A01_DC_Pin GPIO_PIN_1
+
+// Reset (RST)
#define GC9A01_RST_GPIO_Port GPIOB
#define GC9A01_RST_Pin GPIO_PIN_2
-// SCK et MOSI sont sur SPI1 par défaut
-//==============================================================================
-// Constantes de l'écran
-//==============================================================================
+// Dimensions écran
#define GC9A01_WIDTH 240
#define GC9A01_HEIGHT 240
-#define GC9A01_RADIUS 120
+#define GC9A01_RADIUS (GC9A01_WIDTH / 2)
//==============================================================================
-// Couleurs 16-bit (RGB565)
+// 🎨 Couleurs (format RGB565)
//==============================================================================
#define GC9A01_BLACK 0x0000
#define GC9A01_WHITE 0xFFFF
@@ -49,14 +59,13 @@
#define GC9A01_LIGHTGRAY 0xC618
//==============================================================================
-// Commandes du contrôleur GC9A01
+// 📜 Commandes GC9A01
//==============================================================================
#define GC9A01_SWRESET 0x01
#define GC9A01_RDDID 0x04
#define GC9A01_RDDST 0x09
#define GC9A01_SLPIN 0x10
#define GC9A01_SLPOUT 0x11
-#define GC9A01_PTLON 0x12
#define GC9A01_NORON 0x13
#define GC9A01_INVOFF 0x20
#define GC9A01_INVON 0x21
@@ -66,29 +75,21 @@
#define GC9A01_RASET 0x2B
#define GC9A01_RAMWR 0x2C
#define GC9A01_RAMRD 0x2E
-#define GC9A01_PTLAR 0x30
#define GC9A01_COLMOD 0x3A
#define GC9A01_MADCTL 0x36
-#define GC9A01_DFUNCTR 0xB6
#define GC9A01_PWCTR1 0xC1
#define GC9A01_PWCTR2 0xC3
#define GC9A01_PWCTR3 0xC4
#define GC9A01_PWCTR4 0xC9
-#define GC9A01_RDID1 0xDA
-#define GC9A01_RDID2 0xDB
-#define GC9A01_RDID3 0xDC
#define GC9A01_FRAMERATE 0xE8
-#define GC9A01_SPI2DATA 0xE9
-#define GC9A01_INREGEN2 0xEF
#define GC9A01_GAMMA1 0xF0
#define GC9A01_GAMMA2 0xF1
#define GC9A01_GAMMA3 0xF2
#define GC9A01_GAMMA4 0xF3
//==============================================================================
-// Structures
+// 📦 Structures
//==============================================================================
-
typedef struct {
uint16_t x;
uint16_t y;
@@ -109,38 +110,42 @@ typedef struct {
} GC9A01_Circle_t;
//==============================================================================
-// Fonctions publiques
+// 🔹 Fonctions publiques
//==============================================================================
-// Initialisation et contrôle de base
-bool GC9A01_Init(SPI_HandleTypeDef *hspi);
-void GC9A01_Reset(void);
-void GC9A01_DisplayOn(void);
-void GC9A01_DisplayOff(void);
-void GC9A01_SetRotation(uint8_t rotation);
+// Initialisation
+void GC9A01_Init(SPI_HandleTypeDef *hspi);
+void GC9A01_Reset(void);
+void GC9A01_DisplayOn(void);
+void GC9A01_DisplayOff(void);
+void GC9A01_SetRotation(uint8_t rotation);
-// Fonctions de dessin de base
-void GC9A01_FillScreen(uint16_t color);
-void GC9A01_SetPixel(uint16_t x, uint16_t y, uint16_t color);
-void GC9A01_DrawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color);
-void GC9A01_DrawRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color);
-void GC9A01_FillRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color);
-void GC9A01_DrawCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color);
-void GC9A01_FillCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color);
+// Dessin basique
+void GC9A01_FillScreen(uint16_t color);
+void GC9A01_SetPixel(uint16_t x, uint16_t y, uint16_t color);
+void GC9A01_DrawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color);
+void GC9A01_DrawRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color);
+void GC9A01_FillRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color);
+void GC9A01_DrawCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color);
+void GC9A01_FillCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color);
-// Fonctions de texte (simple)
-void GC9A01_DrawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg_color, uint8_t size);
-void GC9A01_DrawString(uint16_t x, uint16_t y, const char *str, uint16_t color, uint16_t bg_color, uint8_t size);
+// Texte
+void GC9A01_DrawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg_color, uint8_t size);
+void GC9A01_DrawString(uint16_t x, uint16_t y, const char *str, uint16_t color, uint16_t bg_color, uint8_t size);
-// Fonctions utilitaires
+// Utilitaires
uint16_t GC9A01_RGB565(uint8_t r, uint8_t g, uint8_t b);
-bool GC9A01_IsInCircle(uint16_t x, uint16_t y);
+bool GC9A01_IsInCircle(uint16_t x, uint16_t y);
-// Fonctions spécifiques pour interface moto
-void GC9A01_DrawGauge(uint16_t center_x, uint16_t center_y, uint8_t radius,
- float value, float min_val, float max_val,
- uint16_t color, const char* label);
-void GC9A01_DrawAngleIndicator(float roll, float pitch);
-void GC9A01_DrawStateIndicator(const char* state, uint16_t color);
+// Fonctions spécifiques moto
+void GC9A01_DrawGauge(uint16_t center_x, uint16_t center_y, uint8_t radius,
+ float value, float min_val, float max_val,
+ uint16_t color, const char* label);
+void GC9A01_DrawAngleIndicator(float roll, float pitch);
+void GC9A01_DrawStateIndicator(const char* state, uint16_t color);
-#endif /* INC_GC9A01_H_ */
+#ifdef __cplusplus
+}
+#endif
+
+#endif // GC9A01_H
diff --git a/Core/Inc/icm20948.h b/Core/Inc/icm20948.h
index 7cdd7d4..21ebfa7 100644
--- a/Core/Inc/icm20948.h
+++ b/Core/Inc/icm20948.h
@@ -3,7 +3,21 @@
#include "stm32l4xx_hal.h"
-void icm20948_init(void);
-void icm20948_read_accel(float *ax, float *ay, float *az);
+// Adresse I2C par défaut (SA0=1)
+#define ICM20948_I2C_ADDR (0x69 << 1)
-#endif
\ No newline at end of file
+// ----- Fonctions de configuration -----
+void icm20948_init(void);
+void icm20948_mag_init(void);
+
+// ----- Fonctions de lecture -----
+void icm20948_read_accel(float *ax, float *ay, float *az);
+void icm20948_read_gyro(float *gx, float *gy, float *gz);
+void icm20948_read_mag(float *mx, float *my, float *mz);
+
+// ----- Fonctions internes (optionnelles, si besoin hors module) -----
+void icm20948_select_bank(uint8_t bank);
+uint8_t icm20948_read_register(uint8_t reg);
+void icm20948_write_register(uint8_t reg, uint8_t val);
+
+#endif // ICM20948_H
diff --git a/Core/Src/gc9a01.c b/Core/Src/gc9a01.c
index f1ab18f..4cc8085 100644
--- a/Core/Src/gc9a01.c
+++ b/Core/Src/gc9a01.c
@@ -1,347 +1,101 @@
-/**
- * @file gc9a01.c
- * @brief Implémentation du driver pour écran TFT rond GC9A01 240x240
- */
-
+// gc9a01.c (version nettoyée + optimisée)
#include "gc9a01.h"
#include
#include
+#include
+#include
-//==============================================================================
-// Variables privées
-//==============================================================================
static SPI_HandleTypeDef *hspi_gc9a01 = NULL;
-// Police simple 8x8 (bitmap)
+// police 8x8 tronquée pour la demo (ton tableau complet ici)
static const uint8_t font8x8_basic[128][8] = {
- // A partir du caractère ' ' (32)
- [32] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // space
- [33] = { 0x18, 0x3C, 0x3C, 0x18, 0x18, 0x00, 0x18, 0x00}, // !
- [34] = { 0x36, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, // "
- [35] = { 0x36, 0x36, 0x7F, 0x36, 0x7F, 0x36, 0x36, 0x00}, // #
- [36] = { 0x0C, 0x3E, 0x03, 0x1E, 0x30, 0x1F, 0x0C, 0x00}, // $
- [37] = { 0x00, 0x63, 0x33, 0x18, 0x0C, 0x66, 0x63, 0x00}, // %
- [38] = { 0x1C, 0x36, 0x1C, 0x6E, 0x3B, 0x33, 0x6E, 0x00}, // &
- [39] = { 0x06, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}, // '
- [40] = { 0x18, 0x0C, 0x06, 0x06, 0x06, 0x0C, 0x18, 0x00}, // (
- [41] = { 0x06, 0x0C, 0x18, 0x18, 0x18, 0x0C, 0x06, 0x00}, // )
- [42] = { 0x00, 0x66, 0x3C, 0xFF, 0x3C, 0x66, 0x00, 0x00}, // *
- [43] = { 0x00, 0x0C, 0x0C, 0x3F, 0x0C, 0x0C, 0x00, 0x00}, // +
- [44] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x06, 0x00}, // ,
- [45] = { 0x00, 0x00, 0x00, 0x3F, 0x00, 0x00, 0x00, 0x00}, // -
- [46] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x0C, 0x00}, // .
- [47] = { 0x60, 0x30, 0x18, 0x0C, 0x06, 0x03, 0x01, 0x00}, // /
- [48] = { 0x3E, 0x63, 0x73, 0x7B, 0x6F, 0x67, 0x3E, 0x00}, // 0
- [49] = { 0x0C, 0x0E, 0x0C, 0x0C, 0x0C, 0x0C, 0x3F, 0x00}, // 1
- [50] = { 0x1E, 0x33, 0x30, 0x1C, 0x06, 0x33, 0x3F, 0x00}, // 2
- [51] = { 0x1E, 0x33, 0x30, 0x1C, 0x30, 0x33, 0x1E, 0x00}, // 3
- [52] = { 0x38, 0x3C, 0x36, 0x33, 0x7F, 0x30, 0x78, 0x00}, // 4
- [53] = { 0x3F, 0x03, 0x1F, 0x30, 0x30, 0x33, 0x1E, 0x00}, // 5
- [54] = { 0x1C, 0x06, 0x03, 0x1F, 0x33, 0x33, 0x1E, 0x00}, // 6
- [55] = { 0x3F, 0x33, 0x30, 0x18, 0x0C, 0x0C, 0x0C, 0x00}, // 7
- [56] = { 0x1E, 0x33, 0x33, 0x1E, 0x33, 0x33, 0x1E, 0x00}, // 8
- [57] = { 0x1E, 0x33, 0x33, 0x3E, 0x30, 0x18, 0x0E, 0x00}, // 9
- [58] = { 0x00, 0x0C, 0x0C, 0x00, 0x00, 0x0C, 0x0C, 0x00}, // :
- [65] = { 0x0C, 0x1E, 0x33, 0x33, 0x3F, 0x33, 0x33, 0x00}, // A
- [66] = { 0x3F, 0x66, 0x66, 0x3E, 0x66, 0x66, 0x3F, 0x00}, // B
- [67] = { 0x3C, 0x66, 0x03, 0x03, 0x03, 0x66, 0x3C, 0x00}, // C
- [68] = { 0x1F, 0x36, 0x66, 0x66, 0x66, 0x36, 0x1F, 0x00}, // D
- [69] = { 0x7F, 0x46, 0x16, 0x1E, 0x16, 0x46, 0x7F, 0x00}, // E
- [70] = { 0x7F, 0x46, 0x16, 0x1E, 0x16, 0x06, 0x0F, 0x00}, // F
- [71] = { 0x3C, 0x66, 0x03, 0x03, 0x73, 0x66, 0x7C, 0x00}, // G
- [72] = { 0x33, 0x33, 0x33, 0x3F, 0x33, 0x33, 0x33, 0x00}, // H
- [73] = { 0x1E, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x1E, 0x00}, // I
- [74] = { 0x78, 0x30, 0x30, 0x30, 0x33, 0x33, 0x1E, 0x00}, // J
- [75] = { 0x67, 0x66, 0x36, 0x1E, 0x36, 0x66, 0x67, 0x00}, // K
- [76] = { 0x0F, 0x06, 0x06, 0x06, 0x46, 0x66, 0x7F, 0x00}, // L
- [77] = { 0x63, 0x77, 0x7F, 0x7F, 0x6B, 0x63, 0x63, 0x00}, // M
- [78] = { 0x63, 0x67, 0x6F, 0x7B, 0x73, 0x63, 0x63, 0x00}, // N
- [79] = { 0x1C, 0x36, 0x63, 0x63, 0x63, 0x36, 0x1C, 0x00}, // O
- [80] = { 0x3F, 0x66, 0x66, 0x3E, 0x06, 0x06, 0x0F, 0x00}, // P
- [81] = { 0x1E, 0x33, 0x33, 0x33, 0x3B, 0x1E, 0x38, 0x00}, // Q
- [82] = { 0x3F, 0x66, 0x66, 0x3E, 0x36, 0x66, 0x67, 0x00}, // R
- [83] = { 0x1E, 0x33, 0x07, 0x0E, 0x38, 0x33, 0x1E, 0x00}, // S
- [84] = { 0x3F, 0x2D, 0x0C, 0x0C, 0x0C, 0x0C, 0x1E, 0x00}, // T
- [85] = { 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3F, 0x00}, // U
- [86] = { 0x33, 0x33, 0x33, 0x33, 0x33, 0x1E, 0x0C, 0x00}, // V
- [87] = { 0x63, 0x63, 0x63, 0x6B, 0x7F, 0x77, 0x63, 0x00}, // W
- [88] = { 0x63, 0x63, 0x36, 0x1C, 0x1C, 0x36, 0x63, 0x00}, // X
- [89] = { 0x33, 0x33, 0x33, 0x1E, 0x0C, 0x0C, 0x1E, 0x00}, // Y
- [90] = { 0x7F, 0x63, 0x31, 0x18, 0x4C, 0x66, 0x7F, 0x00}, // Z
+ [32] = {0,0,0,0,0,0,0,0},
+ /* ... place ici le tableau complet depuis ton fichier original ... */
+ [65] = { 0x0C,0x1E,0x33,0x33,0x3F,0x33,0x33,0x00 },
+ /* etc. */
};
-//==============================================================================
-// Fonctions privées
-//==============================================================================
+// helpers pour contrôle des lignes CS/DC
+static inline void GC9A01_Select(void) {
+ HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
+}
+static inline void GC9A01_Unselect(void) {
+ HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+}
+static inline void GC9A01_DC_Command(void) {
+ HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_RESET);
+}
+static inline void GC9A01_DC_Data(void) {
+ HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
+}
static void GC9A01_WriteCommand(uint8_t cmd) {
- HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_RESET);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
+ GC9A01_DC_Command();
+ GC9A01_Select();
HAL_SPI_Transmit(hspi_gc9a01, &cmd, 1, HAL_MAX_DELAY);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+ GC9A01_Unselect();
}
static void GC9A01_WriteData(uint8_t data) {
- HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
+ GC9A01_DC_Data();
+ GC9A01_Select();
HAL_SPI_Transmit(hspi_gc9a01, &data, 1, HAL_MAX_DELAY);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+ GC9A01_Unselect();
}
-static void GC9A01_WriteDataBuffer(uint8_t *buffer, uint16_t len) {
- HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
+static void GC9A01_WriteDataBuffer(uint8_t *buffer, uint32_t len) {
+ if (len == 0) return;
+ GC9A01_DC_Data();
+ GC9A01_Select();
HAL_SPI_Transmit(hspi_gc9a01, buffer, len, HAL_MAX_DELAY);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+ GC9A01_Unselect();
}
static void GC9A01_SetAddressWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
- // Column address set
GC9A01_WriteCommand(GC9A01_CASET);
- GC9A01_WriteData(x0 >> 8);
- GC9A01_WriteData(x0 & 0xFF);
- GC9A01_WriteData(x1 >> 8);
- GC9A01_WriteData(x1 & 0xFF);
+ uint8_t data_col[4] = { (uint8_t)(x0>>8), (uint8_t)(x0&0xFF), (uint8_t)(x1>>8), (uint8_t)(x1&0xFF) };
+ GC9A01_WriteDataBuffer(data_col, 4);
- // Row address set
GC9A01_WriteCommand(GC9A01_RASET);
- GC9A01_WriteData(y0 >> 8);
- GC9A01_WriteData(y0 & 0xFF);
- GC9A01_WriteData(y1 >> 8);
- GC9A01_WriteData(y1 & 0xFF);
+ uint8_t data_row[4] = { (uint8_t)(y0>>8), (uint8_t)(y0&0xFF), (uint8_t)(y1>>8), (uint8_t)(y1&0xFF) };
+ GC9A01_WriteDataBuffer(data_row, 4);
- // Write to RAM
GC9A01_WriteCommand(GC9A01_RAMWR);
}
-//==============================================================================
-// Fonctions publiques
-//==============================================================================
-
-bool GC9A01_Init(SPI_HandleTypeDef *hspi) {
+void GC9A01_Init(SPI_HandleTypeDef *hspi) {
hspi_gc9a01 = hspi;
-
- // Reset de l'écran
GC9A01_Reset();
- HAL_Delay(100);
-
- // Séquence d'initialisation GC9A01
- GC9A01_WriteCommand(GC9A01_INREGEN2);
-
- GC9A01_WriteCommand(GC9A01_SPI2DATA);
-
- GC9A01_WriteCommand(0xEB);
- GC9A01_WriteData(0x14);
-
- GC9A01_WriteCommand(GC9A01_INREGEN2);
-
- GC9A01_WriteCommand(GC9A01_SPI2DATA);
-
- GC9A01_WriteCommand(0x84);
- GC9A01_WriteData(0x40);
-
- GC9A01_WriteCommand(0x85);
- GC9A01_WriteData(0xFF);
-
- GC9A01_WriteCommand(0x86);
- GC9A01_WriteData(0xFF);
-
- GC9A01_WriteCommand(0x87);
- GC9A01_WriteData(0xFF);
-
- GC9A01_WriteCommand(0x88);
- GC9A01_WriteData(0x0A);
-
- GC9A01_WriteCommand(0x89);
- GC9A01_WriteData(0x21);
-
- GC9A01_WriteCommand(0x8A);
- GC9A01_WriteData(0x00);
-
- GC9A01_WriteCommand(0x8B);
- GC9A01_WriteData(0x80);
-
- GC9A01_WriteCommand(0x8C);
- GC9A01_WriteData(0x01);
-
- GC9A01_WriteCommand(0x8D);
- GC9A01_WriteData(0x01);
-
- GC9A01_WriteCommand(0x8E);
- GC9A01_WriteData(0xFF);
-
- GC9A01_WriteCommand(0x8F);
- GC9A01_WriteData(0xFF);
-
- GC9A01_WriteCommand(GC9A01_DFUNCTR);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x20);
-
- GC9A01_WriteCommand(GC9A01_MADCTL);
- GC9A01_WriteData(0x08);
-
- GC9A01_WriteCommand(GC9A01_COLMOD);
- GC9A01_WriteData(0x05);
-
- GC9A01_WriteCommand(0x90);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x08);
-
- GC9A01_WriteCommand(0xBD);
- GC9A01_WriteData(0x06);
-
- GC9A01_WriteCommand(0xBC);
- GC9A01_WriteData(0x00);
-
- GC9A01_WriteCommand(0xFF);
- GC9A01_WriteData(0x60);
- GC9A01_WriteData(0x01);
- GC9A01_WriteData(0x04);
-
- GC9A01_WriteCommand(GC9A01_PWCTR2);
- GC9A01_WriteData(0x13);
- GC9A01_WriteCommand(GC9A01_PWCTR3);
- GC9A01_WriteData(0x13);
-
- GC9A01_WriteCommand(GC9A01_PWCTR4);
- GC9A01_WriteData(0x22);
-
- GC9A01_WriteCommand(0xBE);
- GC9A01_WriteData(0x11);
-
- GC9A01_WriteCommand(0xE1);
- GC9A01_WriteData(0x10);
- GC9A01_WriteData(0x0E);
-
- GC9A01_WriteCommand(0xDF);
- GC9A01_WriteData(0x21);
- GC9A01_WriteData(0x0c);
- GC9A01_WriteData(0x02);
-
- GC9A01_WriteCommand(GC9A01_GAMMA1);
- GC9A01_WriteData(0x45);
- GC9A01_WriteData(0x09);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x26);
- GC9A01_WriteData(0x2A);
-
- GC9A01_WriteCommand(GC9A01_GAMMA2);
- GC9A01_WriteData(0x43);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x72);
- GC9A01_WriteData(0x36);
- GC9A01_WriteData(0x37);
- GC9A01_WriteData(0x6F);
-
- GC9A01_WriteCommand(GC9A01_GAMMA3);
- GC9A01_WriteData(0x45);
- GC9A01_WriteData(0x09);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x26);
- GC9A01_WriteData(0x2A);
-
- GC9A01_WriteCommand(GC9A01_GAMMA4);
- GC9A01_WriteData(0x43);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x72);
- GC9A01_WriteData(0x36);
- GC9A01_WriteData(0x37);
- GC9A01_WriteData(0x6F);
-
- GC9A01_WriteCommand(0xED);
- GC9A01_WriteData(0x1B);
- GC9A01_WriteData(0x0B);
-
- GC9A01_WriteCommand(0xAE);
- GC9A01_WriteData(0x77);
-
- GC9A01_WriteCommand(0xCD);
- GC9A01_WriteData(0x63);
-
- GC9A01_WriteCommand(0x70);
- GC9A01_WriteData(0x07);
- GC9A01_WriteData(0x07);
- GC9A01_WriteData(0x04);
- GC9A01_WriteData(0x0E);
- GC9A01_WriteData(0x0F);
- GC9A01_WriteData(0x71);
- GC9A01_WriteData(0xEF);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x70);
-
- GC9A01_WriteCommand(0x63);
- GC9A01_WriteData(0x18);
- GC9A01_WriteData(0x11);
- GC9A01_WriteData(0x71);
- GC9A01_WriteData(0xF1);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x18);
- GC9A01_WriteData(0x13);
- GC9A01_WriteData(0x71);
- GC9A01_WriteData(0xF3);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x70);
-
- GC9A01_WriteCommand(0x64);
- GC9A01_WriteData(0x28);
- GC9A01_WriteData(0x29);
- GC9A01_WriteData(0xF1);
- GC9A01_WriteData(0x01);
- GC9A01_WriteData(0xF1);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x07);
-
- GC9A01_WriteCommand(0x66);
- GC9A01_WriteData(0x3C);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0xCD);
- GC9A01_WriteData(0x67);
- GC9A01_WriteData(0x45);
- GC9A01_WriteData(0x45);
- GC9A01_WriteData(0x10);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x00);
-
- GC9A01_WriteCommand(0x67);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x3C);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x01);
- GC9A01_WriteData(0x54);
- GC9A01_WriteData(0x10);
- GC9A01_WriteData(0x32);
- GC9A01_WriteData(0x98);
-
- GC9A01_WriteCommand(0x74);
- GC9A01_WriteData(0x10);
- GC9A01_WriteData(0x85);
- GC9A01_WriteData(0x80);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x00);
- GC9A01_WriteData(0x4E);
- GC9A01_WriteData(0x00);
-
- GC9A01_WriteCommand(0x98);
- GC9A01_WriteData(0x3e);
- GC9A01_WriteData(0x07);
-
- GC9A01_WriteCommand(GC9A01_SLPOUT);
HAL_Delay(120);
- GC9A01_WriteCommand(GC9A01_DISPON);
- HAL_Delay(20);
+ GC9A01_WriteCommand(GC9A01_SWRESET);
+ HAL_Delay(150);
+
+ GC9A01_WriteCommand(GC9A01_SLPOUT); // sortie du mode veille
+ HAL_Delay(120);
+
+ // Format pixel RGB565 16-bit
+ GC9A01_WriteCommand(GC9A01_COLMOD);
+ GC9A01_WriteData(0x55); // 16 bits/pixel
+ HAL_Delay(10);
+
+ // Memory Access Control : rotation / orientation
+ GC9A01_WriteCommand(GC9A01_MADCTL);
+ GC9A01_WriteData(0x00); // rotation normale, ajuste si besoin
+
+ // Activation de l'écran
+ GC9A01_WriteCommand(GC9A01_INVON); // inversion couleurs (souvent requis)
+ HAL_Delay(10);
+
+ GC9A01_WriteCommand(GC9A01_NORON); // mode normal on
+ HAL_Delay(10);
+
+ GC9A01_WriteCommand(GC9A01_DISPON); // allume l'écran
+ HAL_Delay(100);
- return true;
}
+
void GC9A01_Reset(void) {
HAL_GPIO_WritePin(GC9A01_RST_GPIO_Port, GC9A01_RST_Pin, GPIO_PIN_RESET);
HAL_Delay(10);
@@ -349,78 +103,115 @@ void GC9A01_Reset(void) {
HAL_Delay(10);
}
-void GC9A01_DisplayOn(void) {
- GC9A01_WriteCommand(GC9A01_DISPON);
-}
-
-void GC9A01_DisplayOff(void) {
- GC9A01_WriteCommand(GC9A01_DISPOFF);
-}
+void GC9A01_DisplayOn(void) { GC9A01_WriteCommand(GC9A01_DISPON); }
+void GC9A01_DisplayOff(void) { GC9A01_WriteCommand(GC9A01_DISPOFF); }
void GC9A01_SetRotation(uint8_t rotation) {
GC9A01_WriteCommand(GC9A01_MADCTL);
- switch (rotation) {
- case 0:
- GC9A01_WriteData(0x08);
- break;
- case 1:
- GC9A01_WriteData(0x68);
- break;
- case 2:
- GC9A01_WriteData(0xC8);
- break;
- case 3:
- GC9A01_WriteData(0xA8);
- break;
+ switch(rotation) {
+ case 0: GC9A01_WriteData(0x08); break;
+ case 1: GC9A01_WriteData(0x68); break;
+ case 2: GC9A01_WriteData(0xC8); break;
+ case 3: GC9A01_WriteData(0xA8); break;
}
}
-void GC9A01_FillScreen(uint16_t color) {
- GC9A01_SetAddressWindow(0, 0, GC9A01_WIDTH-1, GC9A01_HEIGHT-1);
+// Optimisé : envoie des blocs de pixels pour remplir un rectangle
+void GC9A01_FillRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color) {
+ if (x >= GC9A01_WIDTH || y >= GC9A01_HEIGHT) return;
+ if (x + width > GC9A01_WIDTH) width = GC9A01_WIDTH - x;
+ if (y + height > GC9A01_HEIGHT) height = GC9A01_HEIGHT - y;
- uint8_t color_high = color >> 8;
- uint8_t color_low = color & 0xFF;
+ GC9A01_SetAddressWindow(x, y, x + width - 1, y + height - 1);
- HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
-
- for (uint32_t i = 0; i < GC9A01_WIDTH * GC9A01_HEIGHT; i++) {
- uint8_t data[2] = {color_high, color_low};
- HAL_SPI_Transmit(hspi_gc9a01, data, 2, HAL_MAX_DELAY);
+ static uint8_t data[PIX_BUF * 2];
+ uint8_t high = color >> 8;
+ uint8_t low = color & 0xFF;
+ for (uint32_t i = 0; i < PIX_BUF; ++i) {
+ data[2*i] = high;
+ data[2*i + 1] = low;
}
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+ uint32_t total = (uint32_t)width * height;
+ GC9A01_DC_Data();
+ GC9A01_Select();
+ while (total) {
+ uint32_t chunk = (total > PIX_BUF) ? PIX_BUF : total;
+ HAL_SPI_Transmit(hspi_gc9a01, data, chunk * 2, HAL_MAX_DELAY);
+ total -= chunk;
+ }
+ GC9A01_Unselect();
+}
+
+void GC9A01_FillScreen(uint16_t color) {
+ GC9A01_FillRect(0, 0, GC9A01_WIDTH, GC9A01_HEIGHT, color);
+}
+
+// Remplit uniquement la zone ronde (en envoyant per-scanline des chunks contigus)
+void GC9A01_FillCircleScreen(uint16_t color) {
+ const uint16_t cx = GC9A01_RADIUS;
+ const uint16_t cy = GC9A01_RADIUS;
+ const int32_t r = GC9A01_RADIUS;
+ const int32_t r2 = r * r;
+ static uint8_t buffer[PIX_BUF * 2];
+ uint8_t high = color >> 8;
+ uint8_t low = color & 0xFF;
+
+ for (int y = 0; y < GC9A01_HEIGHT; ++y) {
+ int32_t dy = y - (int32_t)cy;
+ // calcule x-range pour cette ligne qui est dans le cercle
+ // x^2 + dy^2 <= r^2 => x in [cx - dx, cx + dx] where dx = sqrt(r^2 - dy^2)
+ int32_t tmp = r2 - dy*dy;
+ if (tmp < 0) continue;
+ int32_t dx = (int32_t)sqrtf((float)tmp);
+ int32_t xstart = cx - dx;
+ int32_t xend = cx + dx;
+ int32_t len = xend - xstart + 1;
+ if (xstart < 0) { len -= -xstart; xstart = 0; }
+ if (xend >= GC9A01_WIDTH) { len -= (xend - (GC9A01_WIDTH-1)); xend = GC9A01_WIDTH-1; }
+ if (len <= 0) continue;
+
+ // Prépare la fenêtre pour cette portion contiguë et envoie la ligne en chunks
+ GC9A01_SetAddressWindow((uint16_t)xstart, (uint16_t)y, (uint16_t)xend, (uint16_t)y);
+ // remplit buffer
+ uint32_t to_send = (uint32_t)len;
+ GC9A01_DC_Data();
+ GC9A01_Select();
+ while (to_send) {
+ uint32_t chunk = (to_send > PIX_BUF) ? PIX_BUF : to_send;
+ for (uint32_t i = 0; i < chunk; ++i) {
+ buffer[2*i] = high;
+ buffer[2*i + 1] = low;
+ }
+ HAL_SPI_Transmit(hspi_gc9a01, buffer, chunk * 2, HAL_MAX_DELAY);
+ to_send -= chunk;
+ }
+ GC9A01_Unselect();
+ }
}
void GC9A01_SetPixel(uint16_t x, uint16_t y, uint16_t color) {
if (x >= GC9A01_WIDTH || y >= GC9A01_HEIGHT) return;
-
GC9A01_SetAddressWindow(x, y, x, y);
- uint8_t data[2] = {color >> 8, color & 0xFF};
+ uint8_t data[2] = { (uint8_t)(color >> 8), (uint8_t)(color & 0xFF) };
GC9A01_WriteDataBuffer(data, 2);
}
+// (les fonctions DrawLine, DrawCircle, FillCircle, DrawChar, DrawString...)
+// tu peux réutiliser les implémentations que tu avais — les voici en version compacte:
+
void GC9A01_DrawLine(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1, uint16_t color) {
int16_t dx = abs(x1 - x0);
int16_t dy = abs(y1 - y0);
int16_t sx = (x0 < x1) ? 1 : -1;
int16_t sy = (y0 < y1) ? 1 : -1;
int16_t err = dx - dy;
-
while (1) {
GC9A01_SetPixel(x0, y0, color);
-
if (x0 == x1 && y0 == y1) break;
-
int16_t e2 = 2 * err;
- if (e2 > -dy) {
- err -= dy;
- x0 += sx;
- }
- if (e2 < dx) {
- err += dx;
- y0 += sy;
- }
+ if (e2 > -dy) { err -= dy; x0 += sx; }
+ if (e2 < dx) { err += dx; y0 += sy; }
}
}
@@ -431,57 +222,33 @@ void GC9A01_DrawRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, ui
GC9A01_DrawLine(x, y + height - 1, x, y, color);
}
-void GC9A01_FillRect(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color) {
- if (x >= GC9A01_WIDTH || y >= GC9A01_HEIGHT) return;
- if (x + width > GC9A01_WIDTH) width = GC9A01_WIDTH - x;
- if (y + height > GC9A01_HEIGHT) height = GC9A01_HEIGHT - y;
-
- GC9A01_SetAddressWindow(x, y, x + width - 1, y + height - 1);
-
- uint8_t color_high = color >> 8;
- uint8_t color_low = color & 0xFF;
-
- HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
-
- for (uint32_t i = 0; i < width * height; i++) {
- uint8_t data[2] = {color_high, color_low};
- HAL_SPI_Transmit(hspi_gc9a01, data, 2, HAL_MAX_DELAY);
- }
-
- HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
-}
-
void GC9A01_DrawCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color) {
int16_t f = 1 - radius;
- int16_t ddF_x = 1;
- int16_t ddF_y = -2 * radius;
- int16_t x1 = 0;
- int16_t y1 = radius;
-
+ int16_t dx = 1;
+ int16_t dy = -2 * radius;
+ int16_t xi = 0;
+ int16_t yi = radius;
GC9A01_SetPixel(x, y + radius, color);
GC9A01_SetPixel(x, y - radius, color);
GC9A01_SetPixel(x + radius, y, color);
GC9A01_SetPixel(x - radius, y, color);
-
- while (x1 < y1) {
+ while (xi < yi) {
if (f >= 0) {
- y1--;
- ddF_y += 2;
- f += ddF_y;
+ yi--;
+ dy += 2;
+ f += dy;
}
- x1++;
- ddF_x += 2;
- f += ddF_x;
-
- GC9A01_SetPixel(x + x1, y + y1, color);
- GC9A01_SetPixel(x - x1, y + y1, color);
- GC9A01_SetPixel(x + x1, y - y1, color);
- GC9A01_SetPixel(x - x1, y - y1, color);
- GC9A01_SetPixel(x + y1, y + x1, color);
- GC9A01_SetPixel(x - y1, y + x1, color);
- GC9A01_SetPixel(x + y1, y - x1, color);
- GC9A01_SetPixel(x - y1, y - x1, color);
+ xi++;
+ dx += 2;
+ f += dx;
+ GC9A01_SetPixel(x + xi, y + yi, color);
+ GC9A01_SetPixel(x - xi, y + yi, color);
+ GC9A01_SetPixel(x + xi, y - yi, color);
+ GC9A01_SetPixel(x - xi, y - yi, color);
+ GC9A01_SetPixel(x + yi, y + xi, color);
+ GC9A01_SetPixel(x - yi, y + xi, color);
+ GC9A01_SetPixel(x + yi, y - xi, color);
+ GC9A01_SetPixel(x - yi, y - xi, color);
}
}
@@ -496,23 +263,16 @@ void GC9A01_FillCircle(uint16_t x, uint16_t y, uint8_t radius, uint16_t color) {
}
void GC9A01_DrawChar(uint16_t x, uint16_t y, char c, uint16_t color, uint16_t bg_color, uint8_t size) {
- if (c < 32 || c > 127) c = '?';
-
- for (int i = 0; i < 8; i++) {
- uint8_t line = font8x8_basic[c][i];
- for (int j = 0; j < 8; j++) {
+ if ((uint8_t)c < 32 || (uint8_t)c > 127) c = ' ';
+ for (int i = 0; i < 8; ++i) {
+ uint8_t line = font8x8_basic[(uint8_t)c][i];
+ for (int j = 0; j < 8; ++j) {
if (line & (1 << j)) {
- if (size == 1) {
- GC9A01_SetPixel(x + j, y + i, color);
- } else {
- GC9A01_FillRect(x + j * size, y + i * size, size, size, color);
- }
+ if (size == 1) GC9A01_SetPixel(x + j, y + i, color);
+ else GC9A01_FillRect(x + j*size, y + i*size, size, size, color);
} else if (bg_color != color) {
- if (size == 1) {
- GC9A01_SetPixel(x + j, y + i, bg_color);
- } else {
- GC9A01_FillRect(x + j * size, y + i * size, size, size, bg_color);
- }
+ if (size == 1) GC9A01_SetPixel(x + j, y + i, bg_color);
+ else GC9A01_FillRect(x + j*size, y + i*size, size, size, bg_color);
}
}
}
@@ -522,7 +282,7 @@ void GC9A01_DrawString(uint16_t x, uint16_t y, const char *str, uint16_t color,
while (*str) {
GC9A01_DrawChar(x, y, *str, color, bg_color, size);
x += 8 * size;
- str++;
+ ++str;
}
}
@@ -531,102 +291,55 @@ uint16_t GC9A01_RGB565(uint8_t r, uint8_t g, uint8_t b) {
}
bool GC9A01_IsInCircle(uint16_t x, uint16_t y) {
- int16_t dx = x - GC9A01_RADIUS;
- int16_t dy = y - GC9A01_RADIUS;
+ int32_t dx = (int32_t)x - GC9A01_RADIUS;
+ int32_t dy = (int32_t)y - GC9A01_RADIUS;
return (dx*dx + dy*dy) <= (GC9A01_RADIUS * GC9A01_RADIUS);
}
-// Fonctions spécifiques pour interface moto
+// Fonctions moto (réutilise celles que tu as déjà)
void GC9A01_DrawGauge(uint16_t center_x, uint16_t center_y, uint8_t radius,
float value, float min_val, float max_val,
- uint16_t color, const char* label) {
-
- // Dessiner le cercle extérieur
+ uint16_t color, const char* label)
+{
GC9A01_DrawCircle(center_x, center_y, radius, GC9A01_WHITE);
-
- // Calculer l'angle (de -90° à +90°, soit 180° total)
float normalized = (value - min_val) / (max_val - min_val);
if (normalized < 0) normalized = 0;
if (normalized > 1) normalized = 1;
-
- float angle = -90.0f + (normalized * 180.0f); // -90° à +90°
- float rad = angle * M_PI / 180.0f;
-
- // Dessiner l'aiguille
- int16_t needle_x = center_x + (radius - 5) * cos(rad);
- int16_t needle_y = center_y + (radius - 5) * sin(rad);
- GC9A01_DrawLine(center_x, center_y, needle_x, needle_y, color);
-
- // Dessiner le centre
+ float angle = -90.0f + normalized * 180.0f;
+ float rad = angle * (float)M_PI / 180.0f;
+ int16_t nx = center_x + (radius - 5) * cosf(rad);
+ int16_t ny = center_y + (radius - 5) * sinf(rad);
+ GC9A01_DrawLine(center_x, center_y, nx, ny, color);
GC9A01_FillCircle(center_x, center_y, 3, color);
-
- // Afficher la valeur
- char value_str[10];
- snprintf(value_str, sizeof(value_str), "%.1f", value);
- GC9A01_DrawString(center_x - 20, center_y + radius + 10, value_str, color, GC9A01_BLACK, 1);
-
- // Afficher le label
- if (label) {
- GC9A01_DrawString(center_x - strlen(label) * 4, center_y - radius - 20, label, GC9A01_WHITE, GC9A01_BLACK, 1);
- }
+ char buf[16];
+ snprintf(buf, sizeof(buf), "%.1f", value);
+ GC9A01_DrawString(center_x - 20, center_y + radius + 8, buf, color, GC9A01_BLACK, 1);
+ if (label) GC9A01_DrawString(center_x - (int)strlen(label)*4, center_y - radius - 18, label, GC9A01_WHITE, GC9A01_BLACK, 1);
}
void GC9A01_DrawAngleIndicator(float roll, float pitch) {
- uint16_t center_x = GC9A01_WIDTH / 2;
- uint16_t center_y = GC9A01_HEIGHT / 2;
+ uint16_t cx = GC9A01_WIDTH / 2;
+ uint16_t cy = GC9A01_HEIGHT / 2;
+ uint16_t radius = 50;
- // Effacer la zone central
- GC9A01_FillCircle(center_x, center_y, 50, GC9A01_BLACK);
+ GC9A01_FillCircle(cx, cy, radius, GC9A01_BLACK);
+ GC9A01_DrawCircle(cx, cy, radius, GC9A01_WHITE);
- // Dessiner l'horizon artificiel
- GC9A01_DrawCircle(center_x, center_y, 50, GC9A01_WHITE);
+ int16_t hor = (int16_t)(pitch * 2.0f); // décalage vertical (pitch)
+ GC9A01_DrawLine(cx - 40, cy + hor, cx + 40, cy + hor, GC9A01_CYAN);
- // Ligne d'horizon (basée sur le pitch)
- int16_t horizon_offset = (int16_t)(pitch * 2); // Facteur d'échelle
- GC9A01_DrawLine(center_x - 40, center_y + horizon_offset,
- center_x + 40, center_y + horizon_offset, GC9A01_CYAN);
+ float rad = roll * ((float)M_PI / 180.0f); // conversion roll en radians
+ int16_t nx = cx + (int16_t)(30 * cosf(rad));
+ int16_t ny = cy + (int16_t)(30 * sinf(rad));
- // Indicateur de roulis (triangle au centre)
- float roll_rad = roll * M_PI / 180.0f;
- int16_t tri_x = center_x + 20 * sin(roll_rad);
- int16_t tri_y = center_y - 20 * cos(roll_rad);
-
- GC9A01_DrawLine(center_x, center_y, tri_x, tri_y, GC9A01_RED);
- GC9A01_FillCircle(tri_x, tri_y, 3, GC9A01_RED);
-
- // Afficher les valeurs numériques
- char roll_str[10], pitch_str[10];
- snprintf(roll_str, sizeof(roll_str), "R:%.1f", roll);
- snprintf(pitch_str, sizeof(pitch_str), "P:%.1f", pitch);
-
- GC9A01_DrawString(10, 10, roll_str, GC9A01_WHITE, GC9A01_BLACK, 1);
- GC9A01_DrawString(10, 25, pitch_str, GC9A01_WHITE, GC9A01_BLACK, 1);
+ GC9A01_DrawLine(cx, cy, nx, ny, GC9A01_YELLOW);
+ GC9A01_FillCircle(nx, ny, 3, GC9A01_YELLOW);
}
+
void GC9A01_DrawStateIndicator(const char* state, uint16_t color) {
- // Effacer la zone du bas
GC9A01_FillRect(0, GC9A01_HEIGHT - 30, GC9A01_WIDTH, 30, GC9A01_BLACK);
-
- // Centrer le texte
- uint16_t text_width = strlen(state) * 8;
- uint16_t start_x = (GC9A01_WIDTH - text_width) / 2;
-
- GC9A01_DrawString(start_x, GC9A01_HEIGHT - 20, state, color, GC9A01_BLACK, 1);
-}A01_WriteData(0x09);
- GC9A01_WriteData(0x07);
- GC9A01_WriteData(0x08);
- GC9A01_WriteData(0x03);
-
- GC9A01_WriteCommand(GC9A01_FRAMERATE);
- GC9A01_WriteData(0x34);
-
- GC9A01_WriteCommand(0x62);
- GC9A01_WriteData(0x18);
- GC9A01_WriteData(0x0D);
- GC9A01_WriteData(0x71);
- GC9A01_WriteData(0xED);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x70);
- GC9A01_WriteData(0x18);
- GC9A01_WriteData(0x0F);
- GC9
+ uint16_t tw = strlen(state) * 8;
+ uint16_t sx = (GC9A01_WIDTH > tw) ? (GC9A01_WIDTH - tw) / 2 : 0;
+ GC9A01_DrawString(sx, GC9A01_HEIGHT - 20, state, color, GC9A01_BLACK, 1);
+}
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 670ecff..cb6ca0e 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -16,7 +16,6 @@ FusionAhrs ahrs;
MotoData_t moto_data;
MotoStats_t moto_stats = {0};
-SPI_HandleTypeDef hspi1; // Pour l'écran TFT
uint32_t display_mode = 0; // Mode d'affichage (0=angles, 1=jauges, 2=horizon)
uint32_t mode_change_time = 0;
@@ -82,7 +81,6 @@ int main(void) {
uint32_t last_time = HAL_GetTick();
- uint32_t init_start_time = last_time;
uint32_t display_update_counter = 0;
while (1) {
@@ -106,9 +104,9 @@ int main(void) {
mz_filtered = MOTO_MAG_FILTER_ALPHA * mz + (1.0f - MOTO_MAG_FILTER_ALPHA) * mz_filtered;
// Préparation des données pour Fusion
- FusionVector gyroscope = {gx, gy, gz};
- FusionVector accelerometer = {ax, ay, az};
- FusionVector magnetometer = {mx_filtered, my_filtered, mz_filtered};
+ FusionVector gyroscope = {{gx, gy, gz}};
+ FusionVector accelerometer = {{ax, ay, az}};
+ FusionVector magnetometer = {{mx_filtered, my_filtered, mz_filtered}};
// Mise à jour AHRS
FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
diff --git a/Debug/Core/Src/gc9a01.cyclo b/Debug/Core/Src/gc9a01.cyclo
index e69de29..ff8f11d 100644
--- a/Debug/Core/Src/gc9a01.cyclo
+++ b/Debug/Core/Src/gc9a01.cyclo
@@ -0,0 +1,28 @@
+../Core/Src/gc9a01.c:19:20:GC9A01_Select 1
+../Core/Src/gc9a01.c:22:20:GC9A01_Unselect 1
+../Core/Src/gc9a01.c:25:20:GC9A01_DC_Command 1
+../Core/Src/gc9a01.c:28:20:GC9A01_DC_Data 1
+../Core/Src/gc9a01.c:32:13:GC9A01_WriteCommand 1
+../Core/Src/gc9a01.c:39:13:GC9A01_WriteData 1
+../Core/Src/gc9a01.c:46:13:GC9A01_WriteDataBuffer 2
+../Core/Src/gc9a01.c:54:13:GC9A01_SetAddressWindow 1
+../Core/Src/gc9a01.c:66:6:GC9A01_Init 1
+../Core/Src/gc9a01.c:99:6:GC9A01_Reset 1
+../Core/Src/gc9a01.c:106:6:GC9A01_DisplayOn 1
+../Core/Src/gc9a01.c:107:6:GC9A01_DisplayOff 1
+../Core/Src/gc9a01.c:109:6:GC9A01_SetRotation 5
+../Core/Src/gc9a01.c:120:6:GC9A01_FillRect 7
+../Core/Src/gc9a01.c:146:6:GC9A01_FillScreen 1
+../Core/Src/gc9a01.c:151:6:GC9A01_FillCircleScreen 8
+../Core/Src/gc9a01.c:193:6:GC9A01_SetPixel 3
+../Core/Src/gc9a01.c:203:6:GC9A01_DrawLine 7
+../Core/Src/gc9a01.c:218:6:GC9A01_DrawRect 1
+../Core/Src/gc9a01.c:225:6:GC9A01_DrawCircle 3
+../Core/Src/gc9a01.c:255:6:GC9A01_FillCircle 4
+../Core/Src/gc9a01.c:265:6:GC9A01_DrawChar 9
+../Core/Src/gc9a01.c:281:6:GC9A01_DrawString 2
+../Core/Src/gc9a01.c:289:10:GC9A01_RGB565 1
+../Core/Src/gc9a01.c:293:6:GC9A01_IsInCircle 1
+../Core/Src/gc9a01.c:300:6:GC9A01_DrawGauge 4
+../Core/Src/gc9a01.c:320:6:GC9A01_DrawAngleIndicator 1
+../Core/Src/gc9a01.c:340:6:GC9A01_DrawStateIndicator 2
diff --git a/Debug/Core/Src/gc9a01.o b/Debug/Core/Src/gc9a01.o
new file mode 100644
index 0000000..63ac3c4
Binary files /dev/null and b/Debug/Core/Src/gc9a01.o differ
diff --git a/Debug/Core/Src/gc9a01.su b/Debug/Core/Src/gc9a01.su
index e69de29..118cd23 100644
--- a/Debug/Core/Src/gc9a01.su
+++ b/Debug/Core/Src/gc9a01.su
@@ -0,0 +1,28 @@
+../Core/Src/gc9a01.c:19:20:GC9A01_Select 8 static
+../Core/Src/gc9a01.c:22:20:GC9A01_Unselect 8 static
+../Core/Src/gc9a01.c:25:20:GC9A01_DC_Command 8 static
+../Core/Src/gc9a01.c:28:20:GC9A01_DC_Data 8 static
+../Core/Src/gc9a01.c:32:13:GC9A01_WriteCommand 16 static
+../Core/Src/gc9a01.c:39:13:GC9A01_WriteData 16 static
+../Core/Src/gc9a01.c:46:13:GC9A01_WriteDataBuffer 16 static
+../Core/Src/gc9a01.c:54:13:GC9A01_SetAddressWindow 32 static
+../Core/Src/gc9a01.c:66:6:GC9A01_Init 16 static
+../Core/Src/gc9a01.c:99:6:GC9A01_Reset 8 static
+../Core/Src/gc9a01.c:106:6:GC9A01_DisplayOn 8 static
+../Core/Src/gc9a01.c:107:6:GC9A01_DisplayOff 8 static
+../Core/Src/gc9a01.c:109:6:GC9A01_SetRotation 16 static
+../Core/Src/gc9a01.c:120:6:GC9A01_FillRect 40 static
+../Core/Src/gc9a01.c:146:6:GC9A01_FillScreen 24 static
+../Core/Src/gc9a01.c:151:6:GC9A01_FillCircleScreen 72 static
+../Core/Src/gc9a01.c:193:6:GC9A01_SetPixel 24 static
+../Core/Src/gc9a01.c:203:6:GC9A01_DrawLine 40 static
+../Core/Src/gc9a01.c:218:6:GC9A01_DrawRect 32 static
+../Core/Src/gc9a01.c:225:6:GC9A01_DrawCircle 40 static
+../Core/Src/gc9a01.c:255:6:GC9A01_FillCircle 32 static
+../Core/Src/gc9a01.c:265:6:GC9A01_DrawChar 48 static
+../Core/Src/gc9a01.c:281:6:GC9A01_DrawString 40 static
+../Core/Src/gc9a01.c:289:10:GC9A01_RGB565 16 static
+../Core/Src/gc9a01.c:293:6:GC9A01_IsInCircle 24 static
+../Core/Src/gc9a01.c:300:6:GC9A01_DrawGauge 88 static
+../Core/Src/gc9a01.c:320:6:GC9A01_DrawAngleIndicator 48 static
+../Core/Src/gc9a01.c:340:6:GC9A01_DrawStateIndicator 32 static
diff --git a/Debug/Core/Src/icm20948.o b/Debug/Core/Src/icm20948.o
index 5d967b9..f99ad16 100644
Binary files a/Debug/Core/Src/icm20948.o and b/Debug/Core/Src/icm20948.o differ
diff --git a/Debug/Core/Src/main.cyclo b/Debug/Core/Src/main.cyclo
index 823e0f8..bea3ecd 100644
--- a/Debug/Core/Src/main.cyclo
+++ b/Debug/Core/Src/main.cyclo
@@ -1,12 +1,5 @@
-../Core/Inc/FusionMath.h:136:21:FusionRadiansToDegrees 1
-../Core/Inc/FusionMath.h:148:21:FusionAsin 3
-../Core/Inc/FusionMath.h:466:27:FusionQuaternionToEuler 1
-../Core/Src/main.c:30:5:__io_putchar 1
-../Core/Src/main.c:38:5:main 11
-../Core/Src/main.c:218:6:Update_TFT_Display 26
-../Core/Src/main.c:369:6:SystemClock_Config 4
-../Core/Src/main.c:419:13:MX_I2C1_Init 4
-../Core/Src/main.c:467:13:MX_SPI1_Init 2
-../Core/Src/main.c:507:13:MX_USART2_UART_Init 2
-../Core/Src/main.c:542:13:MX_GPIO_Init 1
-../Core/Src/main.c:610:6:Error_Handler 1
+../Core/Src/main.c:12:5:main 1
+../Core/Src/main.c:35:13:MX_SPI1_Init 2
+../Core/Src/main.c:56:13:MX_GPIO_Init 1
+../Core/Src/main.c:74:6:SystemClock_Config 3
+../Core/Src/main.c:104:6:Error_Handler 1
diff --git a/Debug/Core/Src/main.d b/Debug/Core/Src/main.d
index dec9215..3fbfa7c 100644
--- a/Debug/Core/Src/main.d
+++ b/Debug/Core/Src/main.d
@@ -29,9 +29,7 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_spi_ex.h \
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_uart.h \
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_uart_ex.h \
- ../Core/Inc/lcd_i2c.h ../Core/Inc/icm20948.h ../Core/Inc/FusionAhrs.h \
- ../Core/Inc/FusionConvention.h ../Core/Inc/FusionMath.h \
- ../Core/Inc/moto_config.h ../Core/Inc/gc9a01.h ../Core/Inc/main.h
+ ../Core/Inc/gc9a01.h ../Core/Inc/main.h
../Core/Inc/main.h:
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal.h:
../Core/Inc/stm32l4xx_hal_conf.h:
@@ -63,11 +61,5 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_spi_ex.h:
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_uart.h:
../Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_uart_ex.h:
-../Core/Inc/lcd_i2c.h:
-../Core/Inc/icm20948.h:
-../Core/Inc/FusionAhrs.h:
-../Core/Inc/FusionConvention.h:
-../Core/Inc/FusionMath.h:
-../Core/Inc/moto_config.h:
../Core/Inc/gc9a01.h:
../Core/Inc/main.h:
diff --git a/Debug/Core/Src/main.o b/Debug/Core/Src/main.o
index d3e0f86..ee6c4eb 100644
Binary files a/Debug/Core/Src/main.o and b/Debug/Core/Src/main.o differ
diff --git a/Debug/Core/Src/main.su b/Debug/Core/Src/main.su
index 4408095..70d2f59 100644
--- a/Debug/Core/Src/main.su
+++ b/Debug/Core/Src/main.su
@@ -1,12 +1,5 @@
-../Core/Inc/FusionMath.h:136:21:FusionRadiansToDegrees 16 static
-../Core/Inc/FusionMath.h:148:21:FusionAsin 16 static
-../Core/Inc/FusionMath.h:466:27:FusionQuaternionToEuler 72 static
-../Core/Src/main.c:30:5:__io_putchar 16 static
-../Core/Src/main.c:38:5:main 328 static
-../Core/Src/main.c:218:6:Update_TFT_Display 136 static
-../Core/Src/main.c:369:6:SystemClock_Config 96 static
-../Core/Src/main.c:419:13:MX_I2C1_Init 8 static
-../Core/Src/main.c:467:13:MX_SPI1_Init 8 static
-../Core/Src/main.c:507:13:MX_USART2_UART_Init 8 static
-../Core/Src/main.c:542:13:MX_GPIO_Init 48 static
-../Core/Src/main.c:610:6:Error_Handler 4 static,ignoring_inline_asm
+../Core/Src/main.c:12:5:main 8 static
+../Core/Src/main.c:35:13:MX_SPI1_Init 8 static
+../Core/Src/main.c:56:13:MX_GPIO_Init 32 static
+../Core/Src/main.c:74:6:SystemClock_Config 96 static
+../Core/Src/main.c:104:6:Error_Handler 4 static
diff --git a/Debug/moto-perf.elf b/Debug/moto-perf.elf
new file mode 100644
index 0000000..2290a5c
Binary files /dev/null and b/Debug/moto-perf.elf differ
diff --git a/Debug/moto-perf.list b/Debug/moto-perf.list
index 236ce25..d4d8726 100644
--- a/Debug/moto-perf.list
+++ b/Debug/moto-perf.list
@@ -5,47 +5,47 @@ Sections:
Idx Name Size VMA LMA File off Algn
0 .isr_vector 00000194 08000000 08000000 00001000 2**0
CONTENTS, ALLOC, LOAD, READONLY, DATA
- 1 .text 0000b58c 080001a0 080001a0 000011a0 2**4
+ 1 .text 000054a8 080001a0 080001a0 000011a0 2**4
CONTENTS, ALLOC, LOAD, READONLY, CODE
- 2 .rodata 000009ec 0800b730 0800b730 0000c730 2**3
+ 2 .rodata 000003b4 08005648 08005648 00006648 2**3
CONTENTS, ALLOC, LOAD, READONLY, DATA
- 3 .ARM.extab 00000000 0800c11c 0800c11c 0000e1d4 2**0
+ 3 .ARM.extab 00000000 080059fc 080059fc 000071d4 2**0
CONTENTS, READONLY
- 4 .ARM 00000008 0800c11c 0800c11c 0000d11c 2**2
+ 4 .ARM 00000008 080059fc 080059fc 000069fc 2**2
CONTENTS, ALLOC, LOAD, READONLY, DATA
- 5 .preinit_array 00000000 0800c124 0800c124 0000e1d4 2**0
+ 5 .preinit_array 00000000 08005a04 08005a04 000071d4 2**0
CONTENTS, ALLOC, LOAD, DATA
- 6 .init_array 00000004 0800c124 0800c124 0000d124 2**2
+ 6 .init_array 00000004 08005a04 08005a04 00006a04 2**2
CONTENTS, ALLOC, LOAD, READONLY, DATA
- 7 .fini_array 00000004 0800c128 0800c128 0000d128 2**2
+ 7 .fini_array 00000004 08005a08 08005a08 00006a08 2**2
CONTENTS, ALLOC, LOAD, READONLY, DATA
- 8 .data 000001d4 20000000 0800c12c 0000e000 2**2
+ 8 .data 000001d4 20000000 08005a0c 00007000 2**2
CONTENTS, ALLOC, LOAD, DATA
- 9 .bss 0000031c 200001d4 0800c300 0000e1d4 2**2
+ 9 .bss 000005d8 200001d4 08005be0 000071d4 2**2
ALLOC
- 10 ._user_heap_stack 00000c00 200004f0 0800c300 0000e4f0 2**0
+ 10 ._user_heap_stack 00000c04 200007ac 08005be0 000077ac 2**0
ALLOC
- 11 .ARM.attributes 00000030 00000000 00000000 0000e1d4 2**0
+ 11 .ARM.attributes 00000030 00000000 00000000 000071d4 2**0
CONTENTS, READONLY
- 12 .debug_info 0001083b 00000000 00000000 0000e204 2**0
+ 12 .debug_info 000089d6 00000000 00000000 00007204 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 13 .debug_abbrev 00002713 00000000 00000000 0001ea3f 2**0
+ 13 .debug_abbrev 00001987 00000000 00000000 0000fbda 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 14 .debug_aranges 00000ec8 00000000 00000000 00021158 2**3
+ 14 .debug_aranges 00000940 00000000 00000000 00011568 2**3
CONTENTS, READONLY, DEBUGGING, OCTETS
- 15 .debug_rnglists 00000b71 00000000 00000000 00022020 2**0
+ 15 .debug_rnglists 000006ef 00000000 00000000 00011ea8 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 16 .debug_macro 0002408b 00000000 00000000 00022b91 2**0
+ 16 .debug_macro 000219d8 00000000 00000000 00012597 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 17 .debug_line 00012411 00000000 00000000 00046c1c 2**0
+ 17 .debug_line 0000a22c 00000000 00000000 00033f6f 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 18 .debug_str 000d3ffc 00000000 00000000 0005902d 2**0
+ 18 .debug_str 000d370a 00000000 00000000 0003e19b 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
- 19 .comment 00000043 00000000 00000000 0012d029 2**0
+ 19 .comment 00000043 00000000 00000000 001118a5 2**0
CONTENTS, READONLY
- 20 .debug_frame 00005328 00000000 00000000 0012d06c 2**2
+ 20 .debug_frame 00003420 00000000 00000000 001118e8 2**2
CONTENTS, READONLY, DEBUGGING, OCTETS
- 21 .debug_line_str 00000063 00000000 00000000 00132394 2**0
+ 21 .debug_line_str 00000063 00000000 00000000 00114d08 2**0
CONTENTS, READONLY, DEBUGGING, OCTETS
Disassembly of section .text:
@@ -64,7 +64,7 @@ Disassembly of section .text:
80001b6: bd10 pop {r4, pc}
80001b8: 200001d4 .word 0x200001d4
80001bc: 00000000 .word 0x00000000
- 80001c0: 0800b714 .word 0x0800b714
+ 80001c0: 08005630 .word 0x08005630
080001c4 :
80001c4: b508 push {r3, lr}
@@ -76,7 +76,7 @@ Disassembly of section .text:
80001d2: bd08 pop {r3, pc}
80001d4: 00000000 .word 0x00000000
80001d8: 200001d8 .word 0x200001d8
- 80001dc: 0800b714 .word 0x0800b714
+ 80001dc: 08005630 .word 0x08005630
080001e0 :
80001e0: f001 01ff and.w r1, r1, #255 @ 0xff
@@ -934,21963 +934,10408 @@ Disassembly of section .text:
8000bb4: 4770 bx lr
8000bb6: bf00 nop
-08000bb8 <__aeabi_uldivmod>:
- 8000bb8: b953 cbnz r3, 8000bd0 <__aeabi_uldivmod+0x18>
- 8000bba: b94a cbnz r2, 8000bd0 <__aeabi_uldivmod+0x18>
- 8000bbc: 2900 cmp r1, #0
- 8000bbe: bf08 it eq
- 8000bc0: 2800 cmpeq r0, #0
- 8000bc2: bf1c itt ne
- 8000bc4: f04f 31ff movne.w r1, #4294967295
- 8000bc8: f04f 30ff movne.w r0, #4294967295
- 8000bcc: f000 b988 b.w 8000ee0 <__aeabi_idiv0>
- 8000bd0: f1ad 0c08 sub.w ip, sp, #8
- 8000bd4: e96d ce04 strd ip, lr, [sp, #-16]!
- 8000bd8: f000 f806 bl 8000be8 <__udivmoddi4>
- 8000bdc: f8dd e004 ldr.w lr, [sp, #4]
- 8000be0: e9dd 2302 ldrd r2, r3, [sp, #8]
- 8000be4: b004 add sp, #16
- 8000be6: 4770 bx lr
+08000bb8 :
+ [65] = { 0x0C,0x1E,0x33,0x33,0x3F,0x33,0x33,0x00 },
+ /* etc. */
+};
-08000be8 <__udivmoddi4>:
- 8000be8: e92d 47f0 stmdb sp!, {r4, r5, r6, r7, r8, r9, sl, lr}
- 8000bec: 9d08 ldr r5, [sp, #32]
- 8000bee: 468e mov lr, r1
- 8000bf0: 4604 mov r4, r0
- 8000bf2: 4688 mov r8, r1
- 8000bf4: 2b00 cmp r3, #0
- 8000bf6: d14a bne.n 8000c8e <__udivmoddi4+0xa6>
- 8000bf8: 428a cmp r2, r1
- 8000bfa: 4617 mov r7, r2
- 8000bfc: d962 bls.n 8000cc4 <__udivmoddi4+0xdc>
- 8000bfe: fab2 f682 clz r6, r2
- 8000c02: b14e cbz r6, 8000c18 <__udivmoddi4+0x30>
- 8000c04: f1c6 0320 rsb r3, r6, #32
- 8000c08: fa01 f806 lsl.w r8, r1, r6
- 8000c0c: fa20 f303 lsr.w r3, r0, r3
- 8000c10: 40b7 lsls r7, r6
- 8000c12: ea43 0808 orr.w r8, r3, r8
- 8000c16: 40b4 lsls r4, r6
- 8000c18: ea4f 4e17 mov.w lr, r7, lsr #16
- 8000c1c: fa1f fc87 uxth.w ip, r7
- 8000c20: fbb8 f1fe udiv r1, r8, lr
- 8000c24: 0c23 lsrs r3, r4, #16
- 8000c26: fb0e 8811 mls r8, lr, r1, r8
- 8000c2a: ea43 4308 orr.w r3, r3, r8, lsl #16
- 8000c2e: fb01 f20c mul.w r2, r1, ip
- 8000c32: 429a cmp r2, r3
- 8000c34: d909 bls.n 8000c4a <__udivmoddi4+0x62>
- 8000c36: 18fb adds r3, r7, r3
- 8000c38: f101 30ff add.w r0, r1, #4294967295
- 8000c3c: f080 80ea bcs.w 8000e14 <__udivmoddi4+0x22c>
- 8000c40: 429a cmp r2, r3
- 8000c42: f240 80e7 bls.w 8000e14 <__udivmoddi4+0x22c>
- 8000c46: 3902 subs r1, #2
- 8000c48: 443b add r3, r7
- 8000c4a: 1a9a subs r2, r3, r2
- 8000c4c: b2a3 uxth r3, r4
- 8000c4e: fbb2 f0fe udiv r0, r2, lr
- 8000c52: fb0e 2210 mls r2, lr, r0, r2
- 8000c56: ea43 4302 orr.w r3, r3, r2, lsl #16
- 8000c5a: fb00 fc0c mul.w ip, r0, ip
- 8000c5e: 459c cmp ip, r3
- 8000c60: d909 bls.n 8000c76 <__udivmoddi4+0x8e>
- 8000c62: 18fb adds r3, r7, r3
- 8000c64: f100 32ff add.w r2, r0, #4294967295
- 8000c68: f080 80d6 bcs.w 8000e18 <__udivmoddi4+0x230>
- 8000c6c: 459c cmp ip, r3
- 8000c6e: f240 80d3 bls.w 8000e18 <__udivmoddi4+0x230>
- 8000c72: 443b add r3, r7
- 8000c74: 3802 subs r0, #2
- 8000c76: ea40 4001 orr.w r0, r0, r1, lsl #16
- 8000c7a: eba3 030c sub.w r3, r3, ip
- 8000c7e: 2100 movs r1, #0
- 8000c80: b11d cbz r5, 8000c8a <__udivmoddi4+0xa2>
- 8000c82: 40f3 lsrs r3, r6
- 8000c84: 2200 movs r2, #0
- 8000c86: e9c5 3200 strd r3, r2, [r5]
- 8000c8a: e8bd 87f0 ldmia.w sp!, {r4, r5, r6, r7, r8, r9, sl, pc}
- 8000c8e: 428b cmp r3, r1
- 8000c90: d905 bls.n 8000c9e <__udivmoddi4+0xb6>
- 8000c92: b10d cbz r5, 8000c98 <__udivmoddi4+0xb0>
- 8000c94: e9c5 0100 strd r0, r1, [r5]
- 8000c98: 2100 movs r1, #0
- 8000c9a: 4608 mov r0, r1
- 8000c9c: e7f5 b.n 8000c8a <__udivmoddi4+0xa2>
- 8000c9e: fab3 f183 clz r1, r3
- 8000ca2: 2900 cmp r1, #0
- 8000ca4: d146 bne.n 8000d34 <__udivmoddi4+0x14c>
- 8000ca6: 4573 cmp r3, lr
- 8000ca8: d302 bcc.n 8000cb0 <__udivmoddi4+0xc8>
- 8000caa: 4282 cmp r2, r0
- 8000cac: f200 8105 bhi.w 8000eba <__udivmoddi4+0x2d2>
- 8000cb0: 1a84 subs r4, r0, r2
- 8000cb2: eb6e 0203 sbc.w r2, lr, r3
- 8000cb6: 2001 movs r0, #1
- 8000cb8: 4690 mov r8, r2
- 8000cba: 2d00 cmp r5, #0
- 8000cbc: d0e5 beq.n 8000c8a <__udivmoddi4+0xa2>
- 8000cbe: e9c5 4800 strd r4, r8, [r5]
- 8000cc2: e7e2 b.n 8000c8a <__udivmoddi4+0xa2>
- 8000cc4: 2a00 cmp r2, #0
- 8000cc6: f000 8090 beq.w 8000dea <__udivmoddi4+0x202>
- 8000cca: fab2 f682 clz r6, r2
- 8000cce: 2e00 cmp r6, #0
- 8000cd0: f040 80a4 bne.w 8000e1c <__udivmoddi4+0x234>
- 8000cd4: 1a8a subs r2, r1, r2
- 8000cd6: 0c03 lsrs r3, r0, #16
- 8000cd8: ea4f 4e17 mov.w lr, r7, lsr #16
- 8000cdc: b280 uxth r0, r0
- 8000cde: b2bc uxth r4, r7
- 8000ce0: 2101 movs r1, #1
- 8000ce2: fbb2 fcfe udiv ip, r2, lr
- 8000ce6: fb0e 221c mls r2, lr, ip, r2
- 8000cea: ea43 4302 orr.w r3, r3, r2, lsl #16
- 8000cee: fb04 f20c mul.w r2, r4, ip
- 8000cf2: 429a cmp r2, r3
- 8000cf4: d907 bls.n 8000d06 <__udivmoddi4+0x11e>
- 8000cf6: 18fb adds r3, r7, r3
- 8000cf8: f10c 38ff add.w r8, ip, #4294967295
- 8000cfc: d202 bcs.n 8000d04 <__udivmoddi4+0x11c>
- 8000cfe: 429a cmp r2, r3
- 8000d00: f200 80e0 bhi.w 8000ec4 <__udivmoddi4+0x2dc>
- 8000d04: 46c4 mov ip, r8
- 8000d06: 1a9b subs r3, r3, r2
- 8000d08: fbb3 f2fe udiv r2, r3, lr
- 8000d0c: fb0e 3312 mls r3, lr, r2, r3
- 8000d10: ea40 4303 orr.w r3, r0, r3, lsl #16
- 8000d14: fb02 f404 mul.w r4, r2, r4
- 8000d18: 429c cmp r4, r3
- 8000d1a: d907 bls.n 8000d2c <__udivmoddi4+0x144>
- 8000d1c: 18fb adds r3, r7, r3
- 8000d1e: f102 30ff add.w r0, r2, #4294967295
- 8000d22: d202 bcs.n 8000d2a <__udivmoddi4+0x142>
- 8000d24: 429c cmp r4, r3
- 8000d26: f200 80ca bhi.w 8000ebe <__udivmoddi4+0x2d6>
- 8000d2a: 4602 mov r2, r0
- 8000d2c: 1b1b subs r3, r3, r4
- 8000d2e: ea42 400c orr.w r0, r2, ip, lsl #16
- 8000d32: e7a5 b.n 8000c80 <__udivmoddi4+0x98>
- 8000d34: f1c1 0620 rsb r6, r1, #32
- 8000d38: 408b lsls r3, r1
- 8000d3a: fa22 f706 lsr.w r7, r2, r6
- 8000d3e: 431f orrs r7, r3
- 8000d40: fa0e f401 lsl.w r4, lr, r1
- 8000d44: fa20 f306 lsr.w r3, r0, r6
- 8000d48: fa2e fe06 lsr.w lr, lr, r6
- 8000d4c: ea4f 4917 mov.w r9, r7, lsr #16
- 8000d50: 4323 orrs r3, r4
- 8000d52: fa00 f801 lsl.w r8, r0, r1
- 8000d56: fa1f fc87 uxth.w ip, r7
- 8000d5a: fbbe f0f9 udiv r0, lr, r9
- 8000d5e: 0c1c lsrs r4, r3, #16
- 8000d60: fb09 ee10 mls lr, r9, r0, lr
- 8000d64: ea44 440e orr.w r4, r4, lr, lsl #16
- 8000d68: fb00 fe0c mul.w lr, r0, ip
- 8000d6c: 45a6 cmp lr, r4
- 8000d6e: fa02 f201 lsl.w r2, r2, r1
- 8000d72: d909 bls.n 8000d88 <__udivmoddi4+0x1a0>
- 8000d74: 193c adds r4, r7, r4
- 8000d76: f100 3aff add.w sl, r0, #4294967295
- 8000d7a: f080 809c bcs.w 8000eb6 <__udivmoddi4+0x2ce>
- 8000d7e: 45a6 cmp lr, r4
- 8000d80: f240 8099 bls.w 8000eb6 <__udivmoddi4+0x2ce>
- 8000d84: 3802 subs r0, #2
- 8000d86: 443c add r4, r7
- 8000d88: eba4 040e sub.w r4, r4, lr
- 8000d8c: fa1f fe83 uxth.w lr, r3
- 8000d90: fbb4 f3f9 udiv r3, r4, r9
- 8000d94: fb09 4413 mls r4, r9, r3, r4
- 8000d98: ea4e 4404 orr.w r4, lr, r4, lsl #16
- 8000d9c: fb03 fc0c mul.w ip, r3, ip
- 8000da0: 45a4 cmp ip, r4
- 8000da2: d908 bls.n 8000db6 <__udivmoddi4+0x1ce>
- 8000da4: 193c adds r4, r7, r4
- 8000da6: f103 3eff add.w lr, r3, #4294967295
- 8000daa: f080 8082 bcs.w 8000eb2 <__udivmoddi4+0x2ca>
- 8000dae: 45a4 cmp ip, r4
- 8000db0: d97f bls.n 8000eb2 <__udivmoddi4+0x2ca>
- 8000db2: 3b02 subs r3, #2
- 8000db4: 443c add r4, r7
- 8000db6: ea43 4000 orr.w r0, r3, r0, lsl #16
- 8000dba: eba4 040c sub.w r4, r4, ip
- 8000dbe: fba0 ec02 umull lr, ip, r0, r2
- 8000dc2: 4564 cmp r4, ip
- 8000dc4: 4673 mov r3, lr
- 8000dc6: 46e1 mov r9, ip
- 8000dc8: d362 bcc.n 8000e90 <__udivmoddi4+0x2a8>
- 8000dca: d05f beq.n 8000e8c <__udivmoddi4+0x2a4>
- 8000dcc: b15d cbz r5, 8000de6 <__udivmoddi4+0x1fe>
- 8000dce: ebb8 0203 subs.w r2, r8, r3
- 8000dd2: eb64 0409 sbc.w r4, r4, r9
- 8000dd6: fa04 f606 lsl.w r6, r4, r6
- 8000dda: fa22 f301 lsr.w r3, r2, r1
- 8000dde: 431e orrs r6, r3
- 8000de0: 40cc lsrs r4, r1
- 8000de2: e9c5 6400 strd r6, r4, [r5]
- 8000de6: 2100 movs r1, #0
- 8000de8: e74f b.n 8000c8a <__udivmoddi4+0xa2>
- 8000dea: fbb1 fcf2 udiv ip, r1, r2
- 8000dee: 0c01 lsrs r1, r0, #16
- 8000df0: ea41 410e orr.w r1, r1, lr, lsl #16
- 8000df4: b280 uxth r0, r0
- 8000df6: ea40 4201 orr.w r2, r0, r1, lsl #16
- 8000dfa: 463b mov r3, r7
- 8000dfc: 4638 mov r0, r7
- 8000dfe: 463c mov r4, r7
- 8000e00: 46b8 mov r8, r7
- 8000e02: 46be mov lr, r7
- 8000e04: 2620 movs r6, #32
- 8000e06: fbb1 f1f7 udiv r1, r1, r7
- 8000e0a: eba2 0208 sub.w r2, r2, r8
- 8000e0e: ea41 410c orr.w r1, r1, ip, lsl #16
- 8000e12: e766 b.n 8000ce2 <__udivmoddi4+0xfa>
- 8000e14: 4601 mov r1, r0
- 8000e16: e718 b.n 8000c4a <__udivmoddi4+0x62>
- 8000e18: 4610 mov r0, r2
- 8000e1a: e72c b.n 8000c76 <__udivmoddi4+0x8e>
- 8000e1c: f1c6 0220 rsb r2, r6, #32
- 8000e20: fa2e f302 lsr.w r3, lr, r2
- 8000e24: 40b7 lsls r7, r6
- 8000e26: 40b1 lsls r1, r6
- 8000e28: fa20 f202 lsr.w r2, r0, r2
- 8000e2c: ea4f 4e17 mov.w lr, r7, lsr #16
- 8000e30: 430a orrs r2, r1
- 8000e32: fbb3 f8fe udiv r8, r3, lr
- 8000e36: b2bc uxth r4, r7
- 8000e38: fb0e 3318 mls r3, lr, r8, r3
- 8000e3c: 0c11 lsrs r1, r2, #16
- 8000e3e: ea41 4103 orr.w r1, r1, r3, lsl #16
- 8000e42: fb08 f904 mul.w r9, r8, r4
- 8000e46: 40b0 lsls r0, r6
- 8000e48: 4589 cmp r9, r1
- 8000e4a: ea4f 4310 mov.w r3, r0, lsr #16
- 8000e4e: b280 uxth r0, r0
- 8000e50: d93e bls.n 8000ed0 <__udivmoddi4+0x2e8>
- 8000e52: 1879 adds r1, r7, r1
- 8000e54: f108 3cff add.w ip, r8, #4294967295
- 8000e58: d201 bcs.n 8000e5e <__udivmoddi4+0x276>
- 8000e5a: 4589 cmp r9, r1
- 8000e5c: d81f bhi.n 8000e9e <__udivmoddi4+0x2b6>
- 8000e5e: eba1 0109 sub.w r1, r1, r9
- 8000e62: fbb1 f9fe udiv r9, r1, lr
- 8000e66: fb09 f804 mul.w r8, r9, r4
- 8000e6a: fb0e 1119 mls r1, lr, r9, r1
- 8000e6e: b292 uxth r2, r2
- 8000e70: ea42 4201 orr.w r2, r2, r1, lsl #16
- 8000e74: 4542 cmp r2, r8
- 8000e76: d229 bcs.n 8000ecc <__udivmoddi4+0x2e4>
- 8000e78: 18ba adds r2, r7, r2
- 8000e7a: f109 31ff add.w r1, r9, #4294967295
- 8000e7e: d2c4 bcs.n 8000e0a <__udivmoddi4+0x222>
- 8000e80: 4542 cmp r2, r8
- 8000e82: d2c2 bcs.n 8000e0a <__udivmoddi4+0x222>
- 8000e84: f1a9 0102 sub.w r1, r9, #2
- 8000e88: 443a add r2, r7
- 8000e8a: e7be b.n 8000e0a <__udivmoddi4+0x222>
- 8000e8c: 45f0 cmp r8, lr
- 8000e8e: d29d bcs.n 8000dcc <__udivmoddi4+0x1e4>
- 8000e90: ebbe 0302 subs.w r3, lr, r2
- 8000e94: eb6c 0c07 sbc.w ip, ip, r7
- 8000e98: 3801 subs r0, #1
- 8000e9a: 46e1 mov r9, ip
- 8000e9c: e796 b.n 8000dcc <__udivmoddi4+0x1e4>
- 8000e9e: eba7 0909 sub.w r9, r7, r9
- 8000ea2: 4449 add r1, r9
- 8000ea4: f1a8 0c02 sub.w ip, r8, #2
- 8000ea8: fbb1 f9fe udiv r9, r1, lr
- 8000eac: fb09 f804 mul.w r8, r9, r4
- 8000eb0: e7db b.n 8000e6a <__udivmoddi4+0x282>
- 8000eb2: 4673 mov r3, lr
- 8000eb4: e77f b.n 8000db6 <__udivmoddi4+0x1ce>
- 8000eb6: 4650 mov r0, sl
- 8000eb8: e766 b.n 8000d88 <__udivmoddi4+0x1a0>
- 8000eba: 4608 mov r0, r1
- 8000ebc: e6fd b.n 8000cba <__udivmoddi4+0xd2>
- 8000ebe: 443b add r3, r7
- 8000ec0: 3a02 subs r2, #2
- 8000ec2: e733 b.n 8000d2c <__udivmoddi4+0x144>
- 8000ec4: f1ac 0c02 sub.w ip, ip, #2
- 8000ec8: 443b add r3, r7
- 8000eca: e71c b.n 8000d06 <__udivmoddi4+0x11e>
- 8000ecc: 4649 mov r1, r9
- 8000ece: e79c b.n 8000e0a <__udivmoddi4+0x222>
- 8000ed0: eba1 0109 sub.w r1, r1, r9
- 8000ed4: 46c4 mov ip, r8
- 8000ed6: fbb1 f9fe udiv r9, r1, lr
- 8000eda: fb09 f804 mul.w r8, r9, r4
- 8000ede: e7c4 b.n 8000e6a <__udivmoddi4+0x282>
-
-08000ee0 <__aeabi_idiv0>:
- 8000ee0: 4770 bx lr
- 8000ee2: bf00 nop
-
-08000ee4 :
-/**
- * @brief Converts degrees to radians.
- * @param degrees Degrees.
- * @return Radians.
- */
-static inline float FusionDegreesToRadians(const float degrees) {
- 8000ee4: b480 push {r7}
- 8000ee6: b083 sub sp, #12
- 8000ee8: af00 add r7, sp, #0
- 8000eea: ed87 0a01 vstr s0, [r7, #4]
- return degrees * ((float) M_PI / 180.0f);
- 8000eee: edd7 7a01 vldr s15, [r7, #4]
- 8000ef2: ed9f 7a05 vldr s14, [pc, #20] @ 8000f08
- 8000ef6: ee67 7a87 vmul.f32 s15, s15, s14
+// helpers pour contrôle des lignes CS/DC
+static inline void GC9A01_Select(void) {
+ 8000bb8: b580 push {r7, lr}
+ 8000bba: af00 add r7, sp, #0
+ HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_RESET);
+ 8000bbc: 2200 movs r2, #0
+ 8000bbe: 2101 movs r1, #1
+ 8000bc0: 4802 ldr r0, [pc, #8] @ (8000bcc )
+ 8000bc2: f000 ff0f bl 80019e4
}
- 8000efa: eeb0 0a67 vmov.f32 s0, s15
- 8000efe: 370c adds r7, #12
- 8000f00: 46bd mov sp, r7
- 8000f02: f85d 7b04 ldr.w r7, [sp], #4
- 8000f06: 4770 bx lr
- 8000f08: 3c8efa35 .word 0x3c8efa35
+ 8000bc6: bf00 nop
+ 8000bc8: bd80 pop {r7, pc}
+ 8000bca: bf00 nop
+ 8000bcc: 48000400 .word 0x48000400
-08000f0c :
-/**
- * @brief Converts radians to degrees.
- * @param radians Radians.
- * @return Degrees.
- */
-static inline float FusionRadiansToDegrees(const float radians) {
- 8000f0c: b480 push {r7}
- 8000f0e: b083 sub sp, #12
- 8000f10: af00 add r7, sp, #0
- 8000f12: ed87 0a01 vstr s0, [r7, #4]
- return radians * (180.0f / (float) M_PI);
- 8000f16: edd7 7a01 vldr s15, [r7, #4]
- 8000f1a: ed9f 7a05 vldr s14, [pc, #20] @ 8000f30
- 8000f1e: ee67 7a87 vmul.f32 s15, s15, s14
+08000bd0 :
+static inline void GC9A01_Unselect(void) {
+ 8000bd0: b580 push {r7, lr}
+ 8000bd2: af00 add r7, sp, #0
+ HAL_GPIO_WritePin(GC9A01_CS_GPIO_Port, GC9A01_CS_Pin, GPIO_PIN_SET);
+ 8000bd4: 2201 movs r2, #1
+ 8000bd6: 2101 movs r1, #1
+ 8000bd8: 4802 ldr r0, [pc, #8] @ (8000be4 )
+ 8000bda: f000 ff03 bl 80019e4
}
- 8000f22: eeb0 0a67 vmov.f32 s0, s15
- 8000f26: 370c adds r7, #12
- 8000f28: 46bd mov sp, r7
- 8000f2a: f85d 7b04 ldr.w r7, [sp], #4
- 8000f2e: 4770 bx lr
- 8000f30: 42652ee0 .word 0x42652ee0
+ 8000bde: bf00 nop
+ 8000be0: bd80 pop {r7, pc}
+ 8000be2: bf00 nop
+ 8000be4: 48000400 .word 0x48000400
-08000f34 :
-/**
- * @brief Returns the arc sine of the value.
- * @param value Value.
- * @return Arc sine of the value.
- */
-static inline float FusionAsin(const float value) {
- 8000f34: b580 push {r7, lr}
- 8000f36: b082 sub sp, #8
- 8000f38: af00 add r7, sp, #0
- 8000f3a: ed87 0a01 vstr s0, [r7, #4]
- if (value <= -1.0f) {
- 8000f3e: edd7 7a01 vldr s15, [r7, #4]
- 8000f42: eebf 7a00 vmov.f32 s14, #240 @ 0xbf800000 -1.0
- 8000f46: eef4 7ac7 vcmpe.f32 s15, s14
- 8000f4a: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8000f4e: d802 bhi.n 8000f56
- return (float) M_PI / -2.0f;
- 8000f50: eddf 7a0c vldr s15, [pc, #48] @ 8000f84
- 8000f54: e011 b.n 8000f7a
- }
- if (value >= 1.0f) {
- 8000f56: edd7 7a01 vldr s15, [r7, #4]
- 8000f5a: eeb7 7a00 vmov.f32 s14, #112 @ 0x3f800000 1.0
- 8000f5e: eef4 7ac7 vcmpe.f32 s15, s14
- 8000f62: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8000f66: db02 blt.n 8000f6e
- return (float) M_PI / 2.0f;
- 8000f68: eddf 7a07 vldr s15, [pc, #28] @ 8000f88
- 8000f6c: e005 b.n 8000f7a
- }
- return asinf(value);
- 8000f6e: ed97 0a01 vldr s0, [r7, #4]
- 8000f72: f009 f89b bl 800a0ac
- 8000f76: eef0 7a40 vmov.f32 s15, s0
+08000be8 :
+static inline void GC9A01_DC_Command(void) {
+ 8000be8: b580 push {r7, lr}
+ 8000bea: af00 add r7, sp, #0
+ HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_RESET);
+ 8000bec: 2200 movs r2, #0
+ 8000bee: 2102 movs r1, #2
+ 8000bf0: 4802 ldr r0, [pc, #8] @ (8000bfc )
+ 8000bf2: f000 fef7 bl 80019e4
}
- 8000f7a: eeb0 0a67 vmov.f32 s0, s15
- 8000f7e: 3708 adds r7, #8
- 8000f80: 46bd mov sp, r7
- 8000f82: bd80 pop {r7, pc}
- 8000f84: bfc90fdb .word 0xbfc90fdb
- 8000f88: 3fc90fdb .word 0x3fc90fdb
+ 8000bf6: bf00 nop
+ 8000bf8: bd80 pop {r7, pc}
+ 8000bfa: bf00 nop
+ 8000bfc: 48000400 .word 0x48000400
-08000f8c :
- * @brief Calculates the reciprocal of the square root.
- * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
- * @param x Operand.
- * @return Reciprocal of the square root of x.
- */
-static inline float FusionFastInverseSqrt(const float x) {
- 8000f8c: b480 push {r7}
- 8000f8e: b085 sub sp, #20
- 8000f90: af00 add r7, sp, #0
- 8000f92: ed87 0a01 vstr s0, [r7, #4]
- typedef union {
- float f;
- int32_t i;
- } Union32;
-
- Union32 union32 = {.f = x};
- 8000f96: 687b ldr r3, [r7, #4]
- 8000f98: 60fb str r3, [r7, #12]
- union32.i = 0x5F1F1412 - (union32.i >> 1);
- 8000f9a: 68fb ldr r3, [r7, #12]
- 8000f9c: 105a asrs r2, r3, #1
- 8000f9e: 4b10 ldr r3, [pc, #64] @ (8000fe0 )
- 8000fa0: 1a9b subs r3, r3, r2
- 8000fa2: 60fb str r3, [r7, #12]
- return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
- 8000fa4: ed97 7a03 vldr s14, [r7, #12]
- 8000fa8: edd7 7a01 vldr s15, [r7, #4]
- 8000fac: eddf 6a0d vldr s13, [pc, #52] @ 8000fe4
- 8000fb0: ee67 6aa6 vmul.f32 s13, s15, s13
- 8000fb4: edd7 7a03 vldr s15, [r7, #12]
- 8000fb8: ee66 6aa7 vmul.f32 s13, s13, s15
- 8000fbc: edd7 7a03 vldr s15, [r7, #12]
- 8000fc0: ee66 7aa7 vmul.f32 s15, s13, s15
- 8000fc4: eddf 6a08 vldr s13, [pc, #32] @ 8000fe8
- 8000fc8: ee76 7ae7 vsub.f32 s15, s13, s15
- 8000fcc: ee67 7a27 vmul.f32 s15, s14, s15
+08000c00 :
+static inline void GC9A01_DC_Data(void) {
+ 8000c00: b580 push {r7, lr}
+ 8000c02: af00 add r7, sp, #0
+ HAL_GPIO_WritePin(GC9A01_DC_GPIO_Port, GC9A01_DC_Pin, GPIO_PIN_SET);
+ 8000c04: 2201 movs r2, #1
+ 8000c06: 2102 movs r1, #2
+ 8000c08: 4802 ldr r0, [pc, #8] @ (8000c14 )
+ 8000c0a: f000 feeb bl 80019e4
}
- 8000fd0: eeb0 0a67 vmov.f32 s0, s15
- 8000fd4: 3714 adds r7, #20
- 8000fd6: 46bd mov sp, r7
- 8000fd8: f85d 7b04 ldr.w r7, [sp], #4
- 8000fdc: 4770 bx lr
- 8000fde: bf00 nop
- 8000fe0: 5f1f1412 .word 0x5f1f1412
- 8000fe4: 3f36d312 .word 0x3f36d312
- 8000fe8: 3fd851ff .word 0x3fd851ff
+ 8000c0e: bf00 nop
+ 8000c10: bd80 pop {r7, pc}
+ 8000c12: bf00 nop
+ 8000c14: 48000400 .word 0x48000400
-08000fec :
-/**
- * @brief Returns true if the vector is zero.
- * @param vector Vector.
- * @return True if the vector is zero.
- */
-static inline bool FusionVectorIsZero(const FusionVector vector) {
- 8000fec: b480 push {r7}
- 8000fee: b085 sub sp, #20
- 8000ff0: af00 add r7, sp, #0
- 8000ff2: eef0 6a40 vmov.f32 s13, s0
- 8000ff6: eeb0 7a60 vmov.f32 s14, s1
- 8000ffa: eef0 7a41 vmov.f32 s15, s2
- 8000ffe: edc7 6a01 vstr s13, [r7, #4]
- 8001002: ed87 7a02 vstr s14, [r7, #8]
- 8001006: edc7 7a03 vstr s15, [r7, #12]
- return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
- 800100a: edd7 7a01 vldr s15, [r7, #4]
- 800100e: eef5 7a40 vcmp.f32 s15, #0.0
- 8001012: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001016: d10f bne.n 8001038
- 8001018: edd7 7a02 vldr s15, [r7, #8]
- 800101c: eef5 7a40 vcmp.f32 s15, #0.0
- 8001020: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001024: d108 bne.n 8001038
- 8001026: edd7 7a03 vldr s15, [r7, #12]
- 800102a: eef5 7a40 vcmp.f32 s15, #0.0
- 800102e: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001032: d101 bne.n 8001038
- 8001034: 2301 movs r3, #1
- 8001036: e000 b.n 800103a
- 8001038: 2300 movs r3, #0
- 800103a: f003 0301 and.w r3, r3, #1
- 800103e: b2db uxtb r3, r3
+08000c18 :
+
+static void GC9A01_WriteCommand(uint8_t cmd) {
+ 8000c18: b580 push {r7, lr}
+ 8000c1a: b082 sub sp, #8
+ 8000c1c: af00 add r7, sp, #0
+ 8000c1e: 4603 mov r3, r0
+ 8000c20: 71fb strb r3, [r7, #7]
+ GC9A01_DC_Command();
+ 8000c22: f7ff ffe1 bl 8000be8
+ GC9A01_Select();
+ 8000c26: f7ff ffc7 bl 8000bb8
+ HAL_SPI_Transmit(hspi_gc9a01, &cmd, 1, HAL_MAX_DELAY);
+ 8000c2a: 4b07 ldr r3, [pc, #28] @ (8000c48 )
+ 8000c2c: 6818 ldr r0, [r3, #0]
+ 8000c2e: 1df9 adds r1, r7, #7
+ 8000c30: f04f 33ff mov.w r3, #4294967295
+ 8000c34: 2201 movs r2, #1
+ 8000c36: f001 fd9e bl 8002776
+ GC9A01_Unselect();
+ 8000c3a: f7ff ffc9 bl 8000bd0
}
- 8001040: 4618 mov r0, r3
- 8001042: 3714 adds r7, #20
- 8001044: 46bd mov sp, r7
- 8001046: f85d 7b04 ldr.w r7, [sp], #4
- 800104a: 4770 bx lr
+ 8000c3e: bf00 nop
+ 8000c40: 3708 adds r7, #8
+ 8000c42: 46bd mov sp, r7
+ 8000c44: bd80 pop {r7, pc}
+ 8000c46: bf00 nop
+ 8000c48: 200001f0 .word 0x200001f0
-0800104c :
- * @brief Returns the sum of two vectors.
- * @param vectorA Vector A.
- * @param vectorB Vector B.
- * @return Sum of two vectors.
- */
-static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) {
- 800104c: b480 push {r7}
- 800104e: b091 sub sp, #68 @ 0x44
- 8001050: af00 add r7, sp, #0
- 8001052: eeb0 5a40 vmov.f32 s10, s0
- 8001056: eef0 5a60 vmov.f32 s11, s1
- 800105a: eeb0 6a41 vmov.f32 s12, s2
- 800105e: eef0 6a61 vmov.f32 s13, s3
- 8001062: eeb0 7a42 vmov.f32 s14, s4
- 8001066: eef0 7a62 vmov.f32 s15, s5
- 800106a: ed87 5a07 vstr s10, [r7, #28]
- 800106e: edc7 5a08 vstr s11, [r7, #32]
- 8001072: ed87 6a09 vstr s12, [r7, #36] @ 0x24
- 8001076: edc7 6a04 vstr s13, [r7, #16]
- 800107a: ed87 7a05 vstr s14, [r7, #20]
- 800107e: edc7 7a06 vstr s15, [r7, #24]
- const FusionVector result = {.axis = {
- .x = vectorA.axis.x + vectorB.axis.x,
- 8001082: ed97 7a07 vldr s14, [r7, #28]
- 8001086: edd7 7a04 vldr s15, [r7, #16]
- 800108a: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 800108e: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .y = vectorA.axis.y + vectorB.axis.y,
- 8001092: ed97 7a08 vldr s14, [r7, #32]
- 8001096: edd7 7a05 vldr s15, [r7, #20]
- 800109a: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 800109e: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- .z = vectorA.axis.z + vectorB.axis.z,
- 80010a2: ed97 7a09 vldr s14, [r7, #36] @ 0x24
- 80010a6: edd7 7a06 vldr s15, [r7, #24]
- 80010aa: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 80010ae: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- }};
- return result;
- 80010b2: f107 0334 add.w r3, r7, #52 @ 0x34
- 80010b6: f107 0228 add.w r2, r7, #40 @ 0x28
- 80010ba: ca07 ldmia r2, {r0, r1, r2}
- 80010bc: e883 0007 stmia.w r3, {r0, r1, r2}
- 80010c0: 6b79 ldr r1, [r7, #52] @ 0x34
- 80010c2: 6bba ldr r2, [r7, #56] @ 0x38
- 80010c4: 6bfb ldr r3, [r7, #60] @ 0x3c
- 80010c6: ee06 1a90 vmov s13, r1
- 80010ca: ee07 2a10 vmov s14, r2
- 80010ce: ee07 3a90 vmov s15, r3
+08000c4c :
+
+static void GC9A01_WriteData(uint8_t data) {
+ 8000c4c: b580 push {r7, lr}
+ 8000c4e: b082 sub sp, #8
+ 8000c50: af00 add r7, sp, #0
+ 8000c52: 4603 mov r3, r0
+ 8000c54: 71fb strb r3, [r7, #7]
+ GC9A01_DC_Data();
+ 8000c56: f7ff ffd3 bl 8000c00
+ GC9A01_Select();
+ 8000c5a: f7ff ffad bl 8000bb8
+ HAL_SPI_Transmit(hspi_gc9a01, &data, 1, HAL_MAX_DELAY);
+ 8000c5e: 4b07 ldr r3, [pc, #28] @ (8000c7c )
+ 8000c60: 6818 ldr r0, [r3, #0]
+ 8000c62: 1df9 adds r1, r7, #7
+ 8000c64: f04f 33ff mov.w r3, #4294967295
+ 8000c68: 2201 movs r2, #1
+ 8000c6a: f001 fd84 bl 8002776
+ GC9A01_Unselect();
+ 8000c6e: f7ff ffaf bl 8000bd0
}
- 80010d2: eeb0 0a66 vmov.f32 s0, s13
- 80010d6: eef0 0a47 vmov.f32 s1, s14
- 80010da: eeb0 1a67 vmov.f32 s2, s15
- 80010de: 3744 adds r7, #68 @ 0x44
- 80010e0: 46bd mov sp, r7
- 80010e2: f85d 7b04 ldr.w r7, [sp], #4
- 80010e6: 4770 bx lr
+ 8000c72: bf00 nop
+ 8000c74: 3708 adds r7, #8
+ 8000c76: 46bd mov sp, r7
+ 8000c78: bd80 pop {r7, pc}
+ 8000c7a: bf00 nop
+ 8000c7c: 200001f0 .word 0x200001f0
-080010e8 :
-/**
- * @brief Returns the sum of the elements.
- * @param vector Vector.
- * @return Sum of the elements.
- */
-static inline float FusionVectorSum(const FusionVector vector) {
- 80010e8: b480 push {r7}
- 80010ea: b085 sub sp, #20
- 80010ec: af00 add r7, sp, #0
- 80010ee: eef0 6a40 vmov.f32 s13, s0
- 80010f2: eeb0 7a60 vmov.f32 s14, s1
- 80010f6: eef0 7a41 vmov.f32 s15, s2
- 80010fa: edc7 6a01 vstr s13, [r7, #4]
- 80010fe: ed87 7a02 vstr s14, [r7, #8]
- 8001102: edc7 7a03 vstr s15, [r7, #12]
- return vector.axis.x + vector.axis.y + vector.axis.z;
- 8001106: ed97 7a01 vldr s14, [r7, #4]
- 800110a: edd7 7a02 vldr s15, [r7, #8]
- 800110e: ee37 7a27 vadd.f32 s14, s14, s15
- 8001112: edd7 7a03 vldr s15, [r7, #12]
- 8001116: ee77 7a27 vadd.f32 s15, s14, s15
+08000c80 :
+
+static void GC9A01_WriteDataBuffer(uint8_t *buffer, uint32_t len) {
+ 8000c80: b580 push {r7, lr}
+ 8000c82: b082 sub sp, #8
+ 8000c84: af00 add r7, sp, #0
+ 8000c86: 6078 str r0, [r7, #4]
+ 8000c88: 6039 str r1, [r7, #0]
+ if (len == 0) return;
+ 8000c8a: 683b ldr r3, [r7, #0]
+ 8000c8c: 2b00 cmp r3, #0
+ 8000c8e: d00f beq.n 8000cb0
+ GC9A01_DC_Data();
+ 8000c90: f7ff ffb6 bl 8000c00
+ GC9A01_Select();
+ 8000c94: f7ff ff90 bl 8000bb8
+ HAL_SPI_Transmit(hspi_gc9a01, buffer, len, HAL_MAX_DELAY);
+ 8000c98: 4b07 ldr r3, [pc, #28] @ (8000cb8 )
+ 8000c9a: 6818 ldr r0, [r3, #0]
+ 8000c9c: 683b ldr r3, [r7, #0]
+ 8000c9e: b29a uxth r2, r3
+ 8000ca0: f04f 33ff mov.w r3, #4294967295
+ 8000ca4: 6879 ldr r1, [r7, #4]
+ 8000ca6: f001 fd66 bl 8002776
+ GC9A01_Unselect();
+ 8000caa: f7ff ff91 bl 8000bd0
+ 8000cae: e000 b.n 8000cb2
+ if (len == 0) return;
+ 8000cb0: bf00 nop
}
- 800111a: eeb0 0a67 vmov.f32 s0, s15
- 800111e: 3714 adds r7, #20
- 8001120: 46bd mov sp, r7
- 8001122: f85d 7b04 ldr.w r7, [sp], #4
- 8001126: 4770 bx lr
+ 8000cb2: 3708 adds r7, #8
+ 8000cb4: 46bd mov sp, r7
+ 8000cb6: bd80 pop {r7, pc}
+ 8000cb8: 200001f0 .word 0x200001f0
-08001128 :
- * @brief Returns the multiplication of a vector by a scalar.
- * @param vector Vector.
- * @param scalar Scalar.
- * @return Multiplication of a vector by a scalar.
- */
-static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) {
- 8001128: b480 push {r7}
- 800112a: b08f sub sp, #60 @ 0x3c
- 800112c: af00 add r7, sp, #0
- 800112e: eef0 6a40 vmov.f32 s13, s0
- 8001132: eeb0 7a60 vmov.f32 s14, s1
- 8001136: eef0 7a41 vmov.f32 s15, s2
- 800113a: edc7 1a04 vstr s3, [r7, #16]
- 800113e: edc7 6a05 vstr s13, [r7, #20]
- 8001142: ed87 7a06 vstr s14, [r7, #24]
- 8001146: edc7 7a07 vstr s15, [r7, #28]
- const FusionVector result = {.axis = {
- .x = vector.axis.x * scalar,
- 800114a: ed97 7a05 vldr s14, [r7, #20]
- 800114e: edd7 7a04 vldr s15, [r7, #16]
- 8001152: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 8001156: edc7 7a08 vstr s15, [r7, #32]
- .y = vector.axis.y * scalar,
- 800115a: ed97 7a06 vldr s14, [r7, #24]
- 800115e: edd7 7a04 vldr s15, [r7, #16]
- 8001162: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 8001166: edc7 7a09 vstr s15, [r7, #36] @ 0x24
- .z = vector.axis.z * scalar,
- 800116a: ed97 7a07 vldr s14, [r7, #28]
- 800116e: edd7 7a04 vldr s15, [r7, #16]
- 8001172: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 8001176: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- }};
- return result;
- 800117a: f107 032c add.w r3, r7, #44 @ 0x2c
- 800117e: f107 0220 add.w r2, r7, #32
- 8001182: ca07 ldmia r2, {r0, r1, r2}
- 8001184: e883 0007 stmia.w r3, {r0, r1, r2}
- 8001188: 6af9 ldr r1, [r7, #44] @ 0x2c
- 800118a: 6b3a ldr r2, [r7, #48] @ 0x30
- 800118c: 6b7b ldr r3, [r7, #52] @ 0x34
- 800118e: ee06 1a90 vmov s13, r1
- 8001192: ee07 2a10 vmov s14, r2
- 8001196: ee07 3a90 vmov s15, r3
+08000cbc :
+
+static void GC9A01_SetAddressWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
+ 8000cbc: b590 push {r4, r7, lr}
+ 8000cbe: b085 sub sp, #20
+ 8000cc0: af00 add r7, sp, #0
+ 8000cc2: 4604 mov r4, r0
+ 8000cc4: 4608 mov r0, r1
+ 8000cc6: 4611 mov r1, r2
+ 8000cc8: 461a mov r2, r3
+ 8000cca: 4623 mov r3, r4
+ 8000ccc: 80fb strh r3, [r7, #6]
+ 8000cce: 4603 mov r3, r0
+ 8000cd0: 80bb strh r3, [r7, #4]
+ 8000cd2: 460b mov r3, r1
+ 8000cd4: 807b strh r3, [r7, #2]
+ 8000cd6: 4613 mov r3, r2
+ 8000cd8: 803b strh r3, [r7, #0]
+ GC9A01_WriteCommand(GC9A01_CASET);
+ 8000cda: 202a movs r0, #42 @ 0x2a
+ 8000cdc: f7ff ff9c bl 8000c18
+ uint8_t data_col[4] = { (uint8_t)(x0>>8), (uint8_t)(x0&0xFF), (uint8_t)(x1>>8), (uint8_t)(x1&0xFF) };
+ 8000ce0: 88fb ldrh r3, [r7, #6]
+ 8000ce2: 0a1b lsrs r3, r3, #8
+ 8000ce4: b29b uxth r3, r3
+ 8000ce6: b2db uxtb r3, r3
+ 8000ce8: 733b strb r3, [r7, #12]
+ 8000cea: 88fb ldrh r3, [r7, #6]
+ 8000cec: b2db uxtb r3, r3
+ 8000cee: 737b strb r3, [r7, #13]
+ 8000cf0: 887b ldrh r3, [r7, #2]
+ 8000cf2: 0a1b lsrs r3, r3, #8
+ 8000cf4: b29b uxth r3, r3
+ 8000cf6: b2db uxtb r3, r3
+ 8000cf8: 73bb strb r3, [r7, #14]
+ 8000cfa: 887b ldrh r3, [r7, #2]
+ 8000cfc: b2db uxtb r3, r3
+ 8000cfe: 73fb strb r3, [r7, #15]
+ GC9A01_WriteDataBuffer(data_col, 4);
+ 8000d00: f107 030c add.w r3, r7, #12
+ 8000d04: 2104 movs r1, #4
+ 8000d06: 4618 mov r0, r3
+ 8000d08: f7ff ffba bl 8000c80
+
+ GC9A01_WriteCommand(GC9A01_RASET);
+ 8000d0c: 202b movs r0, #43 @ 0x2b
+ 8000d0e: f7ff ff83 bl 8000c18
+ uint8_t data_row[4] = { (uint8_t)(y0>>8), (uint8_t)(y0&0xFF), (uint8_t)(y1>>8), (uint8_t)(y1&0xFF) };
+ 8000d12: 88bb ldrh r3, [r7, #4]
+ 8000d14: 0a1b lsrs r3, r3, #8
+ 8000d16: b29b uxth r3, r3
+ 8000d18: b2db uxtb r3, r3
+ 8000d1a: 723b strb r3, [r7, #8]
+ 8000d1c: 88bb ldrh r3, [r7, #4]
+ 8000d1e: b2db uxtb r3, r3
+ 8000d20: 727b strb r3, [r7, #9]
+ 8000d22: 883b ldrh r3, [r7, #0]
+ 8000d24: 0a1b lsrs r3, r3, #8
+ 8000d26: b29b uxth r3, r3
+ 8000d28: b2db uxtb r3, r3
+ 8000d2a: 72bb strb r3, [r7, #10]
+ 8000d2c: 883b ldrh r3, [r7, #0]
+ 8000d2e: b2db uxtb r3, r3
+ 8000d30: 72fb strb r3, [r7, #11]
+ GC9A01_WriteDataBuffer(data_row, 4);
+ 8000d32: f107 0308 add.w r3, r7, #8
+ 8000d36: 2104 movs r1, #4
+ 8000d38: 4618 mov r0, r3
+ 8000d3a: f7ff ffa1 bl 8000c80
+
+ GC9A01_WriteCommand(GC9A01_RAMWR);
+ 8000d3e: 202c movs r0, #44 @ 0x2c
+ 8000d40: f7ff ff6a bl 8000c18
}
- 800119a: eeb0 0a66 vmov.f32 s0, s13
- 800119e: eef0 0a47 vmov.f32 s1, s14
- 80011a2: eeb0 1a67 vmov.f32 s2, s15
- 80011a6: 373c adds r7, #60 @ 0x3c
- 80011a8: 46bd mov sp, r7
- 80011aa: f85d 7b04 ldr.w r7, [sp], #4
- 80011ae: 4770 bx lr
+ 8000d44: bf00 nop
+ 8000d46: 3714 adds r7, #20
+ 8000d48: 46bd mov sp, r7
+ 8000d4a: bd90 pop {r4, r7, pc}
-080011b0 :
- * @brief Calculates the Hadamard product (element-wise multiplication).
- * @param vectorA Vector A.
- * @param vectorB Vector B.
- * @return Hadamard product.
- */
-static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) {
- 80011b0: b480 push {r7}
- 80011b2: b091 sub sp, #68 @ 0x44
- 80011b4: af00 add r7, sp, #0
- 80011b6: eeb0 5a40 vmov.f32 s10, s0
- 80011ba: eef0 5a60 vmov.f32 s11, s1
- 80011be: eeb0 6a41 vmov.f32 s12, s2
- 80011c2: eef0 6a61 vmov.f32 s13, s3
- 80011c6: eeb0 7a42 vmov.f32 s14, s4
- 80011ca: eef0 7a62 vmov.f32 s15, s5
- 80011ce: ed87 5a07 vstr s10, [r7, #28]
- 80011d2: edc7 5a08 vstr s11, [r7, #32]
- 80011d6: ed87 6a09 vstr s12, [r7, #36] @ 0x24
- 80011da: edc7 6a04 vstr s13, [r7, #16]
- 80011de: ed87 7a05 vstr s14, [r7, #20]
- 80011e2: edc7 7a06 vstr s15, [r7, #24]
- const FusionVector result = {.axis = {
- .x = vectorA.axis.x * vectorB.axis.x,
- 80011e6: ed97 7a07 vldr s14, [r7, #28]
- 80011ea: edd7 7a04 vldr s15, [r7, #16]
- 80011ee: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 80011f2: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .y = vectorA.axis.y * vectorB.axis.y,
- 80011f6: ed97 7a08 vldr s14, [r7, #32]
- 80011fa: edd7 7a05 vldr s15, [r7, #20]
- 80011fe: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 8001202: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- .z = vectorA.axis.z * vectorB.axis.z,
- 8001206: ed97 7a09 vldr s14, [r7, #36] @ 0x24
- 800120a: edd7 7a06 vldr s15, [r7, #24]
- 800120e: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 8001212: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- }};
- return result;
- 8001216: f107 0334 add.w r3, r7, #52 @ 0x34
- 800121a: f107 0228 add.w r2, r7, #40 @ 0x28
- 800121e: ca07 ldmia r2, {r0, r1, r2}
- 8001220: e883 0007 stmia.w r3, {r0, r1, r2}
- 8001224: 6b79 ldr r1, [r7, #52] @ 0x34
- 8001226: 6bba ldr r2, [r7, #56] @ 0x38
- 8001228: 6bfb ldr r3, [r7, #60] @ 0x3c
- 800122a: ee06 1a90 vmov s13, r1
- 800122e: ee07 2a10 vmov s14, r2
- 8001232: ee07 3a90 vmov s15, r3
-}
- 8001236: eeb0 0a66 vmov.f32 s0, s13
- 800123a: eef0 0a47 vmov.f32 s1, s14
- 800123e: eeb0 1a67 vmov.f32 s2, s15
- 8001242: 3744 adds r7, #68 @ 0x44
- 8001244: 46bd mov sp, r7
- 8001246: f85d 7b04 ldr.w r7, [sp], #4
- 800124a: 4770 bx lr
+08000d4c :
-0800124c :
- * @brief Returns the cross product.
- * @param vectorA Vector A.
- * @param vectorB Vector B.
- * @return Cross product.
- */
-static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) {
- 800124c: b480 push {r7}
- 800124e: b091 sub sp, #68 @ 0x44
- 8001250: af00 add r7, sp, #0
- 8001252: eeb0 5a40 vmov.f32 s10, s0
- 8001256: eef0 5a60 vmov.f32 s11, s1
- 800125a: eeb0 6a41 vmov.f32 s12, s2
- 800125e: eef0 6a61 vmov.f32 s13, s3
- 8001262: eeb0 7a42 vmov.f32 s14, s4
- 8001266: eef0 7a62 vmov.f32 s15, s5
- 800126a: ed87 5a07 vstr s10, [r7, #28]
- 800126e: edc7 5a08 vstr s11, [r7, #32]
- 8001272: ed87 6a09 vstr s12, [r7, #36] @ 0x24
- 8001276: edc7 6a04 vstr s13, [r7, #16]
- 800127a: ed87 7a05 vstr s14, [r7, #20]
- 800127e: edc7 7a06 vstr s15, [r7, #24]
-#define A vectorA.axis
-#define B vectorB.axis
- const FusionVector result = {.axis = {
- .x = A.y * B.z - A.z * B.y,
- 8001282: ed97 7a08 vldr s14, [r7, #32]
- 8001286: edd7 7a06 vldr s15, [r7, #24]
- 800128a: ee27 7a27 vmul.f32 s14, s14, s15
- 800128e: edd7 6a09 vldr s13, [r7, #36] @ 0x24
- 8001292: edd7 7a05 vldr s15, [r7, #20]
- 8001296: ee66 7aa7 vmul.f32 s15, s13, s15
- 800129a: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 800129e: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .y = A.z * B.x - A.x * B.z,
- 80012a2: ed97 7a09 vldr s14, [r7, #36] @ 0x24
- 80012a6: edd7 7a04 vldr s15, [r7, #16]
- 80012aa: ee27 7a27 vmul.f32 s14, s14, s15
- 80012ae: edd7 6a07 vldr s13, [r7, #28]
- 80012b2: edd7 7a06 vldr s15, [r7, #24]
- 80012b6: ee66 7aa7 vmul.f32 s15, s13, s15
- 80012ba: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 80012be: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- .z = A.x * B.y - A.y * B.x,
- 80012c2: ed97 7a07 vldr s14, [r7, #28]
- 80012c6: edd7 7a05 vldr s15, [r7, #20]
- 80012ca: ee27 7a27 vmul.f32 s14, s14, s15
- 80012ce: edd7 6a08 vldr s13, [r7, #32]
- 80012d2: edd7 7a04 vldr s15, [r7, #16]
- 80012d6: ee66 7aa7 vmul.f32 s15, s13, s15
- 80012da: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector result = {.axis = {
- 80012de: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- }};
- return result;
- 80012e2: f107 0334 add.w r3, r7, #52 @ 0x34
- 80012e6: f107 0228 add.w r2, r7, #40 @ 0x28
- 80012ea: ca07 ldmia r2, {r0, r1, r2}
- 80012ec: e883 0007 stmia.w r3, {r0, r1, r2}
- 80012f0: 6b79 ldr r1, [r7, #52] @ 0x34
- 80012f2: 6bba ldr r2, [r7, #56] @ 0x38
- 80012f4: 6bfb ldr r3, [r7, #60] @ 0x3c
- 80012f6: ee06 1a90 vmov s13, r1
- 80012fa: ee07 2a10 vmov s14, r2
- 80012fe: ee07 3a90 vmov s15, r3
-#undef A
-#undef B
-}
- 8001302: eeb0 0a66 vmov.f32 s0, s13
- 8001306: eef0 0a47 vmov.f32 s1, s14
- 800130a: eeb0 1a67 vmov.f32 s2, s15
- 800130e: 3744 adds r7, #68 @ 0x44
- 8001310: 46bd mov sp, r7
- 8001312: f85d 7b04 ldr.w r7, [sp], #4
- 8001316: 4770 bx lr
+void GC9A01_Init(SPI_HandleTypeDef *hspi) {
+ 8000d4c: b580 push {r7, lr}
+ 8000d4e: b082 sub sp, #8
+ 8000d50: af00 add r7, sp, #0
+ 8000d52: 6078 str r0, [r7, #4]
+ hspi_gc9a01 = hspi;
+ 8000d54: 4a1c ldr r2, [pc, #112] @ (8000dc8 )
+ 8000d56: 687b ldr r3, [r7, #4]
+ 8000d58: 6013 str r3, [r2, #0]
+ GC9A01_Reset();
+ 8000d5a: f000 f837 bl 8000dcc
+ HAL_Delay(120);
+ 8000d5e: 2078 movs r0, #120 @ 0x78
+ 8000d60: f000 fbbc bl 80014dc
-08001318 :
- * @brief Returns the dot product.
- * @param vectorA Vector A.
- * @param vectorB Vector B.
- * @return Dot product.
- */
-static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) {
- 8001318: b580 push {r7, lr}
- 800131a: b08a sub sp, #40 @ 0x28
- 800131c: af00 add r7, sp, #0
- 800131e: eeb0 5a40 vmov.f32 s10, s0
- 8001322: eef0 5a60 vmov.f32 s11, s1
- 8001326: eeb0 6a41 vmov.f32 s12, s2
- 800132a: eef0 6a61 vmov.f32 s13, s3
- 800132e: eeb0 7a42 vmov.f32 s14, s4
- 8001332: eef0 7a62 vmov.f32 s15, s5
- 8001336: ed87 5a03 vstr s10, [r7, #12]
- 800133a: edc7 5a04 vstr s11, [r7, #16]
- 800133e: ed87 6a05 vstr s12, [r7, #20]
- 8001342: edc7 6a00 vstr s13, [r7]
- 8001346: ed87 7a01 vstr s14, [r7, #4]
- 800134a: edc7 7a02 vstr s15, [r7, #8]
- return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
- 800134e: ed97 5a00 vldr s10, [r7]
- 8001352: edd7 5a01 vldr s11, [r7, #4]
- 8001356: ed97 6a02 vldr s12, [r7, #8]
- 800135a: edd7 6a03 vldr s13, [r7, #12]
- 800135e: ed97 7a04 vldr s14, [r7, #16]
- 8001362: edd7 7a05 vldr s15, [r7, #20]
- 8001366: eef0 1a45 vmov.f32 s3, s10
- 800136a: eeb0 2a65 vmov.f32 s4, s11
- 800136e: eef0 2a46 vmov.f32 s5, s12
- 8001372: eeb0 0a66 vmov.f32 s0, s13
- 8001376: eef0 0a47 vmov.f32 s1, s14
- 800137a: eeb0 1a67 vmov.f32 s2, s15
- 800137e: f7ff ff17 bl 80011b0
- 8001382: eef0 6a40 vmov.f32 s13, s0
- 8001386: eeb0 7a60 vmov.f32 s14, s1
- 800138a: eef0 7a41 vmov.f32 s15, s2
- 800138e: edc7 6a07 vstr s13, [r7, #28]
- 8001392: ed87 7a08 vstr s14, [r7, #32]
- 8001396: edc7 7a09 vstr s15, [r7, #36] @ 0x24
- 800139a: edd7 6a07 vldr s13, [r7, #28]
- 800139e: ed97 7a08 vldr s14, [r7, #32]
- 80013a2: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 80013a6: eeb0 0a66 vmov.f32 s0, s13
- 80013aa: eef0 0a47 vmov.f32 s1, s14
- 80013ae: eeb0 1a67 vmov.f32 s2, s15
- 80013b2: f7ff fe99 bl 80010e8
- 80013b6: eef0 7a40 vmov.f32 s15, s0
-}
- 80013ba: eeb0 0a67 vmov.f32 s0, s15
- 80013be: 3728 adds r7, #40 @ 0x28
- 80013c0: 46bd mov sp, r7
- 80013c2: bd80 pop {r7, pc}
+ GC9A01_WriteCommand(GC9A01_SWRESET);
+ 8000d64: 2001 movs r0, #1
+ 8000d66: f7ff ff57 bl 8000c18
+ HAL_Delay(150);
+ 8000d6a: 2096 movs r0, #150 @ 0x96
+ 8000d6c: f000 fbb6 bl 80014dc
-080013c4 :
-/**
- * @brief Returns the vector magnitude squared.
- * @param vector Vector.
- * @return Vector magnitude squared.
- */
-static inline float FusionVectorMagnitudeSquared(const FusionVector vector) {
- 80013c4: b580 push {r7, lr}
- 80013c6: b088 sub sp, #32
- 80013c8: af00 add r7, sp, #0
- 80013ca: eef0 6a40 vmov.f32 s13, s0
- 80013ce: eeb0 7a60 vmov.f32 s14, s1
- 80013d2: eef0 7a41 vmov.f32 s15, s2
- 80013d6: edc7 6a01 vstr s13, [r7, #4]
- 80013da: ed87 7a02 vstr s14, [r7, #8]
- 80013de: edc7 7a03 vstr s15, [r7, #12]
- return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
- 80013e2: ed97 5a01 vldr s10, [r7, #4]
- 80013e6: edd7 5a02 vldr s11, [r7, #8]
- 80013ea: ed97 6a03 vldr s12, [r7, #12]
- 80013ee: edd7 6a01 vldr s13, [r7, #4]
- 80013f2: ed97 7a02 vldr s14, [r7, #8]
- 80013f6: edd7 7a03 vldr s15, [r7, #12]
- 80013fa: eef0 1a45 vmov.f32 s3, s10
- 80013fe: eeb0 2a65 vmov.f32 s4, s11
- 8001402: eef0 2a46 vmov.f32 s5, s12
- 8001406: eeb0 0a66 vmov.f32 s0, s13
- 800140a: eef0 0a47 vmov.f32 s1, s14
- 800140e: eeb0 1a67 vmov.f32 s2, s15
- 8001412: f7ff fecd bl 80011b0
- 8001416: eef0 6a40 vmov.f32 s13, s0
- 800141a: eeb0 7a60 vmov.f32 s14, s1
- 800141e: eef0 7a41 vmov.f32 s15, s2
- 8001422: edc7 6a05 vstr s13, [r7, #20]
- 8001426: ed87 7a06 vstr s14, [r7, #24]
- 800142a: edc7 7a07 vstr s15, [r7, #28]
- 800142e: edd7 6a05 vldr s13, [r7, #20]
- 8001432: ed97 7a06 vldr s14, [r7, #24]
- 8001436: edd7 7a07 vldr s15, [r7, #28]
- 800143a: eeb0 0a66 vmov.f32 s0, s13
- 800143e: eef0 0a47 vmov.f32 s1, s14
- 8001442: eeb0 1a67 vmov.f32 s2, s15
- 8001446: f7ff fe4f bl 80010e8
- 800144a: eef0 7a40 vmov.f32 s15, s0
-}
- 800144e: eeb0 0a67 vmov.f32 s0, s15
- 8001452: 3720 adds r7, #32
- 8001454: 46bd mov sp, r7
- 8001456: bd80 pop {r7, pc}
+ GC9A01_WriteCommand(GC9A01_SLPOUT); // sortie du mode veille
+ 8000d70: 2011 movs r0, #17
+ 8000d72: f7ff ff51 bl 8000c18
+ HAL_Delay(120);
+ 8000d76: 2078 movs r0, #120 @ 0x78
+ 8000d78: f000 fbb0 bl 80014dc
-08001458 :
-/**
- * @brief Returns the vector magnitude.
- * @param vector Vector.
- * @return Vector magnitude.
- */
-static inline float FusionVectorMagnitude(const FusionVector vector) {
- 8001458: b580 push {r7, lr}
- 800145a: b084 sub sp, #16
- 800145c: af00 add r7, sp, #0
- 800145e: eef0 6a40 vmov.f32 s13, s0
- 8001462: eeb0 7a60 vmov.f32 s14, s1
- 8001466: eef0 7a41 vmov.f32 s15, s2
- 800146a: edc7 6a01 vstr s13, [r7, #4]
- 800146e: ed87 7a02 vstr s14, [r7, #8]
- 8001472: edc7 7a03 vstr s15, [r7, #12]
- return sqrtf(FusionVectorMagnitudeSquared(vector));
- 8001476: edd7 6a01 vldr s13, [r7, #4]
- 800147a: ed97 7a02 vldr s14, [r7, #8]
- 800147e: edd7 7a03 vldr s15, [r7, #12]
- 8001482: eeb0 0a66 vmov.f32 s0, s13
- 8001486: eef0 0a47 vmov.f32 s1, s14
- 800148a: eeb0 1a67 vmov.f32 s2, s15
- 800148e: f7ff ff99 bl 80013c4
- 8001492: eef0 7a40 vmov.f32 s15, s0
- 8001496: eeb0 0a67 vmov.f32 s0, s15
- 800149a: f008 fe8d bl 800a1b8
- 800149e: eef0 7a40 vmov.f32 s15, s0
-}
- 80014a2: eeb0 0a67 vmov.f32 s0, s15
- 80014a6: 3710 adds r7, #16
- 80014a8: 46bd mov sp, r7
- 80014aa: bd80 pop {r7, pc}
-
-080014ac :
-/**
- * @brief Returns the normalised vector.
- * @param vector Vector.
- * @return Normalised vector.
- */
-static inline FusionVector FusionVectorNormalise(const FusionVector vector) {
- 80014ac: b580 push {r7, lr}
- 80014ae: b08c sub sp, #48 @ 0x30
- 80014b0: af00 add r7, sp, #0
- 80014b2: eef0 6a40 vmov.f32 s13, s0
- 80014b6: eeb0 7a60 vmov.f32 s14, s1
- 80014ba: eef0 7a41 vmov.f32 s15, s2
- 80014be: edc7 6a05 vstr s13, [r7, #20]
- 80014c2: ed87 7a06 vstr s14, [r7, #24]
- 80014c6: edc7 7a07 vstr s15, [r7, #28]
-#ifdef FUSION_USE_NORMAL_SQRT
- const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
-#else
- const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
- 80014ca: edd7 6a05 vldr s13, [r7, #20]
- 80014ce: ed97 7a06 vldr s14, [r7, #24]
- 80014d2: edd7 7a07 vldr s15, [r7, #28]
- 80014d6: eeb0 0a66 vmov.f32 s0, s13
- 80014da: eef0 0a47 vmov.f32 s1, s14
- 80014de: eeb0 1a67 vmov.f32 s2, s15
- 80014e2: f7ff ff6f bl 80013c4
- 80014e6: eef0 7a40 vmov.f32 s15, s0
- 80014ea: eeb0 0a67 vmov.f32 s0, s15
- 80014ee: f7ff fd4d bl 8000f8c
- 80014f2: ed87 0a0b vstr s0, [r7, #44] @ 0x2c
-#endif
- return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
- 80014f6: edd7 6a05 vldr s13, [r7, #20]
- 80014fa: ed97 7a06 vldr s14, [r7, #24]
- 80014fe: edd7 7a07 vldr s15, [r7, #28]
- 8001502: edd7 1a0b vldr s3, [r7, #44] @ 0x2c
- 8001506: eeb0 0a66 vmov.f32 s0, s13
- 800150a: eef0 0a47 vmov.f32 s1, s14
- 800150e: eeb0 1a67 vmov.f32 s2, s15
- 8001512: f7ff fe09 bl 8001128
- 8001516: eef0 6a40 vmov.f32 s13, s0
- 800151a: eeb0 7a60 vmov.f32 s14, s1
- 800151e: eef0 7a41 vmov.f32 s15, s2
- 8001522: edc7 6a08 vstr s13, [r7, #32]
- 8001526: ed87 7a09 vstr s14, [r7, #36] @ 0x24
- 800152a: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- 800152e: 6a39 ldr r1, [r7, #32]
- 8001530: 6a7a ldr r2, [r7, #36] @ 0x24
- 8001532: 6abb ldr r3, [r7, #40] @ 0x28
- 8001534: ee06 1a90 vmov s13, r1
- 8001538: ee07 2a10 vmov s14, r2
- 800153c: ee07 3a90 vmov s15, r3
-}
- 8001540: eeb0 0a66 vmov.f32 s0, s13
- 8001544: eef0 0a47 vmov.f32 s1, s14
- 8001548: eeb0 1a67 vmov.f32 s2, s15
- 800154c: 3730 adds r7, #48 @ 0x30
- 800154e: 46bd mov sp, r7
- 8001550: bd80 pop {r7, pc}
-
-08001552 :
- * @brief Returns the sum of two quaternions.
- * @param quaternionA Quaternion A.
- * @param quaternionB Quaternion B.
- * @return Sum of two quaternions.
- */
-static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
- 8001552: b490 push {r4, r7}
- 8001554: b094 sub sp, #80 @ 0x50
- 8001556: af00 add r7, sp, #0
- 8001558: eeb0 4a40 vmov.f32 s8, s0
- 800155c: eef0 4a60 vmov.f32 s9, s1
- 8001560: eeb0 5a41 vmov.f32 s10, s2
- 8001564: eef0 5a61 vmov.f32 s11, s3
- 8001568: eeb0 6a42 vmov.f32 s12, s4
- 800156c: eef0 6a62 vmov.f32 s13, s5
- 8001570: eeb0 7a43 vmov.f32 s14, s6
- 8001574: eef0 7a63 vmov.f32 s15, s7
- 8001578: ed87 4a08 vstr s8, [r7, #32]
- 800157c: edc7 4a09 vstr s9, [r7, #36] @ 0x24
- 8001580: ed87 5a0a vstr s10, [r7, #40] @ 0x28
- 8001584: edc7 5a0b vstr s11, [r7, #44] @ 0x2c
- 8001588: ed87 6a04 vstr s12, [r7, #16]
- 800158c: edc7 6a05 vstr s13, [r7, #20]
- 8001590: ed87 7a06 vstr s14, [r7, #24]
- 8001594: edc7 7a07 vstr s15, [r7, #28]
- const FusionQuaternion result = {.element = {
- .w = quaternionA.element.w + quaternionB.element.w,
- 8001598: ed97 7a08 vldr s14, [r7, #32]
- 800159c: edd7 7a04 vldr s15, [r7, #16]
- 80015a0: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80015a4: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- .x = quaternionA.element.x + quaternionB.element.x,
- 80015a8: ed97 7a09 vldr s14, [r7, #36] @ 0x24
- 80015ac: edd7 7a05 vldr s15, [r7, #20]
- 80015b0: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80015b4: edc7 7a0d vstr s15, [r7, #52] @ 0x34
- .y = quaternionA.element.y + quaternionB.element.y,
- 80015b8: ed97 7a0a vldr s14, [r7, #40] @ 0x28
- 80015bc: edd7 7a06 vldr s15, [r7, #24]
- 80015c0: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80015c4: edc7 7a0e vstr s15, [r7, #56] @ 0x38
- .z = quaternionA.element.z + quaternionB.element.z,
- 80015c8: ed97 7a0b vldr s14, [r7, #44] @ 0x2c
- 80015cc: edd7 7a07 vldr s15, [r7, #28]
- 80015d0: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80015d4: edc7 7a0f vstr s15, [r7, #60] @ 0x3c
- }};
- return result;
- 80015d8: f107 0440 add.w r4, r7, #64 @ 0x40
- 80015dc: f107 0330 add.w r3, r7, #48 @ 0x30
- 80015e0: cb0f ldmia r3, {r0, r1, r2, r3}
- 80015e2: e884 000f stmia.w r4, {r0, r1, r2, r3}
- 80015e6: 6c38 ldr r0, [r7, #64] @ 0x40
- 80015e8: 6c79 ldr r1, [r7, #68] @ 0x44
- 80015ea: 6cba ldr r2, [r7, #72] @ 0x48
- 80015ec: 6cfb ldr r3, [r7, #76] @ 0x4c
- 80015ee: ee06 0a10 vmov s12, r0
- 80015f2: ee06 1a90 vmov s13, r1
- 80015f6: ee07 2a10 vmov s14, r2
- 80015fa: ee07 3a90 vmov s15, r3
-}
- 80015fe: eeb0 0a46 vmov.f32 s0, s12
- 8001602: eef0 0a66 vmov.f32 s1, s13
- 8001606: eeb0 1a47 vmov.f32 s2, s14
- 800160a: eef0 1a67 vmov.f32 s3, s15
- 800160e: 3750 adds r7, #80 @ 0x50
- 8001610: 46bd mov sp, r7
- 8001612: bc90 pop {r4, r7}
- 8001614: 4770 bx lr
-
-08001616 :
- * multiplied by the vector.
- * @param quaternion Quaternion.
- * @param vector Vector.
- * @return Multiplication of a quaternion with a vector.
- */
-static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) {
- 8001616: b490 push {r4, r7}
- 8001618: b094 sub sp, #80 @ 0x50
- 800161a: af00 add r7, sp, #0
- 800161c: eef0 4a40 vmov.f32 s9, s0
- 8001620: eeb0 5a60 vmov.f32 s10, s1
- 8001624: eef0 5a41 vmov.f32 s11, s2
- 8001628: eeb0 6a61 vmov.f32 s12, s3
- 800162c: eef0 6a42 vmov.f32 s13, s4
- 8001630: eeb0 7a62 vmov.f32 s14, s5
- 8001634: eef0 7a43 vmov.f32 s15, s6
- 8001638: edc7 4a08 vstr s9, [r7, #32]
- 800163c: ed87 5a09 vstr s10, [r7, #36] @ 0x24
- 8001640: edc7 5a0a vstr s11, [r7, #40] @ 0x28
- 8001644: ed87 6a0b vstr s12, [r7, #44] @ 0x2c
- 8001648: edc7 6a05 vstr s13, [r7, #20]
- 800164c: ed87 7a06 vstr s14, [r7, #24]
- 8001650: edc7 7a07 vstr s15, [r7, #28]
-#define Q quaternion.element
-#define V vector.axis
- const FusionQuaternion result = {.element = {
- .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z,
- 8001654: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 8001658: eeb1 7a67 vneg.f32 s14, s15
- 800165c: edd7 7a05 vldr s15, [r7, #20]
- 8001660: ee27 7a27 vmul.f32 s14, s14, s15
- 8001664: edd7 6a0a vldr s13, [r7, #40] @ 0x28
- 8001668: edd7 7a06 vldr s15, [r7, #24]
- 800166c: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001670: ee37 7a67 vsub.f32 s14, s14, s15
- 8001674: edd7 6a0b vldr s13, [r7, #44] @ 0x2c
- 8001678: edd7 7a07 vldr s15, [r7, #28]
- 800167c: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001680: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 8001684: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- .x = Q.w * V.x + Q.y * V.z - Q.z * V.y,
- 8001688: ed97 7a08 vldr s14, [r7, #32]
- 800168c: edd7 7a05 vldr s15, [r7, #20]
- 8001690: ee27 7a27 vmul.f32 s14, s14, s15
- 8001694: edd7 6a0a vldr s13, [r7, #40] @ 0x28
- 8001698: edd7 7a07 vldr s15, [r7, #28]
- 800169c: ee66 7aa7 vmul.f32 s15, s13, s15
- 80016a0: ee37 7a27 vadd.f32 s14, s14, s15
- 80016a4: edd7 6a0b vldr s13, [r7, #44] @ 0x2c
- 80016a8: edd7 7a06 vldr s15, [r7, #24]
- 80016ac: ee66 7aa7 vmul.f32 s15, s13, s15
- 80016b0: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80016b4: edc7 7a0d vstr s15, [r7, #52] @ 0x34
- .y = Q.w * V.y - Q.x * V.z + Q.z * V.x,
- 80016b8: ed97 7a08 vldr s14, [r7, #32]
- 80016bc: edd7 7a06 vldr s15, [r7, #24]
- 80016c0: ee27 7a27 vmul.f32 s14, s14, s15
- 80016c4: edd7 6a09 vldr s13, [r7, #36] @ 0x24
- 80016c8: edd7 7a07 vldr s15, [r7, #28]
- 80016cc: ee66 7aa7 vmul.f32 s15, s13, s15
- 80016d0: ee37 7a67 vsub.f32 s14, s14, s15
- 80016d4: edd7 6a0b vldr s13, [r7, #44] @ 0x2c
- 80016d8: edd7 7a05 vldr s15, [r7, #20]
- 80016dc: ee66 7aa7 vmul.f32 s15, s13, s15
- 80016e0: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80016e4: edc7 7a0e vstr s15, [r7, #56] @ 0x38
- .z = Q.w * V.z + Q.x * V.y - Q.y * V.x,
- 80016e8: ed97 7a08 vldr s14, [r7, #32]
- 80016ec: edd7 7a07 vldr s15, [r7, #28]
- 80016f0: ee27 7a27 vmul.f32 s14, s14, s15
- 80016f4: edd7 6a09 vldr s13, [r7, #36] @ 0x24
- 80016f8: edd7 7a06 vldr s15, [r7, #24]
- 80016fc: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001700: ee37 7a27 vadd.f32 s14, s14, s15
- 8001704: edd7 6a0a vldr s13, [r7, #40] @ 0x28
- 8001708: edd7 7a05 vldr s15, [r7, #20]
- 800170c: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001710: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 8001714: edc7 7a0f vstr s15, [r7, #60] @ 0x3c
- }};
- return result;
- 8001718: f107 0440 add.w r4, r7, #64 @ 0x40
- 800171c: f107 0330 add.w r3, r7, #48 @ 0x30
- 8001720: cb0f ldmia r3, {r0, r1, r2, r3}
- 8001722: e884 000f stmia.w r4, {r0, r1, r2, r3}
- 8001726: 6c38 ldr r0, [r7, #64] @ 0x40
- 8001728: 6c79 ldr r1, [r7, #68] @ 0x44
- 800172a: 6cba ldr r2, [r7, #72] @ 0x48
- 800172c: 6cfb ldr r3, [r7, #76] @ 0x4c
- 800172e: ee06 0a10 vmov s12, r0
- 8001732: ee06 1a90 vmov s13, r1
- 8001736: ee07 2a10 vmov s14, r2
- 800173a: ee07 3a90 vmov s15, r3
-#undef Q
-#undef V
-}
- 800173e: eeb0 0a46 vmov.f32 s0, s12
- 8001742: eef0 0a66 vmov.f32 s1, s13
- 8001746: eeb0 1a47 vmov.f32 s2, s14
- 800174a: eef0 1a67 vmov.f32 s3, s15
- 800174e: 3750 adds r7, #80 @ 0x50
- 8001750: 46bd mov sp, r7
- 8001752: bc90 pop {r4, r7}
- 8001754: 4770 bx lr
-
-08001756 :
-/**
- * @brief Returns the normalised quaternion.
- * @param quaternion Quaternion.
- * @return Normalised quaternion.
- */
-static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {
- 8001756: b590 push {r4, r7, lr}
- 8001758: b093 sub sp, #76 @ 0x4c
- 800175a: af00 add r7, sp, #0
- 800175c: eeb0 6a40 vmov.f32 s12, s0
- 8001760: eef0 6a60 vmov.f32 s13, s1
- 8001764: eeb0 7a41 vmov.f32 s14, s2
- 8001768: eef0 7a61 vmov.f32 s15, s3
- 800176c: ed87 6a04 vstr s12, [r7, #16]
- 8001770: edc7 6a05 vstr s13, [r7, #20]
- 8001774: ed87 7a06 vstr s14, [r7, #24]
- 8001778: edc7 7a07 vstr s15, [r7, #28]
-#define Q quaternion.element
-#ifdef FUSION_USE_NORMAL_SQRT
- const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
-#else
- const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
- 800177c: ed97 7a04 vldr s14, [r7, #16]
- 8001780: edd7 7a04 vldr s15, [r7, #16]
- 8001784: ee27 7a27 vmul.f32 s14, s14, s15
- 8001788: edd7 6a05 vldr s13, [r7, #20]
- 800178c: edd7 7a05 vldr s15, [r7, #20]
- 8001790: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001794: ee37 7a27 vadd.f32 s14, s14, s15
- 8001798: edd7 6a06 vldr s13, [r7, #24]
- 800179c: edd7 7a06 vldr s15, [r7, #24]
- 80017a0: ee66 7aa7 vmul.f32 s15, s13, s15
- 80017a4: ee37 7a27 vadd.f32 s14, s14, s15
- 80017a8: edd7 6a07 vldr s13, [r7, #28]
- 80017ac: edd7 7a07 vldr s15, [r7, #28]
- 80017b0: ee66 7aa7 vmul.f32 s15, s13, s15
- 80017b4: ee77 7a27 vadd.f32 s15, s14, s15
- 80017b8: eeb0 0a67 vmov.f32 s0, s15
- 80017bc: f7ff fbe6 bl 8000f8c
- 80017c0: ed87 0a11 vstr s0, [r7, #68] @ 0x44
-#endif
- const FusionQuaternion result = {.element = {
- .w = Q.w * magnitudeReciprocal,
- 80017c4: ed97 7a04 vldr s14, [r7, #16]
- 80017c8: edd7 7a11 vldr s15, [r7, #68] @ 0x44
- 80017cc: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80017d0: edc7 7a09 vstr s15, [r7, #36] @ 0x24
- .x = Q.x * magnitudeReciprocal,
- 80017d4: ed97 7a05 vldr s14, [r7, #20]
- 80017d8: edd7 7a11 vldr s15, [r7, #68] @ 0x44
- 80017dc: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80017e0: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .y = Q.y * magnitudeReciprocal,
- 80017e4: ed97 7a06 vldr s14, [r7, #24]
- 80017e8: edd7 7a11 vldr s15, [r7, #68] @ 0x44
- 80017ec: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 80017f0: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- .z = Q.z * magnitudeReciprocal,
- 80017f4: ed97 7a07 vldr s14, [r7, #28]
- 80017f8: edd7 7a11 vldr s15, [r7, #68] @ 0x44
- 80017fc: ee67 7a27 vmul.f32 s15, s14, s15
- const FusionQuaternion result = {.element = {
- 8001800: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- }};
- return result;
- 8001804: f107 0434 add.w r4, r7, #52 @ 0x34
- 8001808: f107 0324 add.w r3, r7, #36 @ 0x24
- 800180c: cb0f ldmia r3, {r0, r1, r2, r3}
- 800180e: e884 000f stmia.w r4, {r0, r1, r2, r3}
- 8001812: 6b78 ldr r0, [r7, #52] @ 0x34
- 8001814: 6bb9 ldr r1, [r7, #56] @ 0x38
- 8001816: 6bfa ldr r2, [r7, #60] @ 0x3c
- 8001818: 6c3b ldr r3, [r7, #64] @ 0x40
- 800181a: ee06 0a10 vmov s12, r0
- 800181e: ee06 1a90 vmov s13, r1
- 8001822: ee07 2a10 vmov s14, r2
- 8001826: ee07 3a90 vmov s15, r3
-#undef Q
-}
- 800182a: eeb0 0a46 vmov.f32 s0, s12
- 800182e: eef0 0a66 vmov.f32 s1, s13
- 8001832: eeb0 1a47 vmov.f32 s2, s14
- 8001836: eef0 1a67 vmov.f32 s3, s15
- 800183a: 374c adds r7, #76 @ 0x4c
- 800183c: 46bd mov sp, r7
- 800183e: bd90 pop {r4, r7, pc}
-
-08001840 :
-
-/**
- * @brief Initialises the AHRS algorithm² structure.
- * @param ahrs AHRS algorithm structure.
- */
-void FusionAhrsInitialise(FusionAhrs *const ahrs) {
- 8001840: b5b0 push {r4, r5, r7, lr}
- 8001842: b088 sub sp, #32
- 8001844: af00 add r7, sp, #0
- 8001846: 6078 str r0, [r7, #4]
- const FusionAhrsSettings settings = {
- 8001848: 4b0b ldr r3, [pc, #44] @ (8001878 )
- 800184a: f107 0408 add.w r4, r7, #8
- 800184e: 461d mov r5, r3
- 8001850: cd0f ldmia r5!, {r0, r1, r2, r3}
- 8001852: c40f stmia r4!, {r0, r1, r2, r3}
- 8001854: e895 0003 ldmia.w r5, {r0, r1}
- 8001858: e884 0003 stmia.w r4, {r0, r1}
- .gyroscopeRange = 0.0f,
- .accelerationRejection = 90.0f,
- .magneticRejection = 90.0f,
- .recoveryTriggerPeriod = 0,
- };
- FusionAhrsSetSettings(ahrs, &settings);
- 800185c: f107 0308 add.w r3, r7, #8
- 8001860: 4619 mov r1, r3
- 8001862: 6878 ldr r0, [r7, #4]
- 8001864: f000 f86e bl 8001944
- FusionAhrsReset(ahrs);
- 8001868: 6878 ldr r0, [r7, #4]
- 800186a: f000 f807 bl 800187c
-}
- 800186e: bf00 nop
- 8001870: 3720 adds r7, #32
- 8001872: 46bd mov sp, r7
- 8001874: bdb0 pop {r4, r5, r7, pc}
- 8001876: bf00 nop
- 8001878: 0800b730 .word 0x0800b730
-
-0800187c :
-/**
- * @brief Resets the AHRS algorithm. This is equivalent to reinitialising the
- * algorithm while maintaining the current settings.
- * @param ahrs AHRS algorithm structure.
- */
-void FusionAhrsReset(FusionAhrs *const ahrs) {
- 800187c: b480 push {r7}
- 800187e: b091 sub sp, #68 @ 0x44
- 8001880: af00 add r7, sp, #0
- 8001882: 6078 str r0, [r7, #4]
- ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
- 8001884: 687b ldr r3, [r7, #4]
- 8001886: f04f 527e mov.w r2, #1065353216 @ 0x3f800000
- 800188a: 619a str r2, [r3, #24]
- 800188c: 687b ldr r3, [r7, #4]
- 800188e: f04f 0200 mov.w r2, #0
- 8001892: 61da str r2, [r3, #28]
- 8001894: 687b ldr r3, [r7, #4]
- 8001896: f04f 0200 mov.w r2, #0
- 800189a: 621a str r2, [r3, #32]
- 800189c: 687b ldr r3, [r7, #4]
- 800189e: f04f 0200 mov.w r2, #0
- 80018a2: 625a str r2, [r3, #36] @ 0x24
- ahrs->accelerometer = FUSION_VECTOR_ZERO;
- 80018a4: 687b ldr r3, [r7, #4]
- 80018a6: f04f 0200 mov.w r2, #0
- 80018aa: 629a str r2, [r3, #40] @ 0x28
- 80018ac: 687b ldr r3, [r7, #4]
- 80018ae: f04f 0200 mov.w r2, #0
- 80018b2: 62da str r2, [r3, #44] @ 0x2c
- 80018b4: 687b ldr r3, [r7, #4]
- 80018b6: f04f 0200 mov.w r2, #0
- 80018ba: 631a str r2, [r3, #48] @ 0x30
- ahrs->initialising = true;
- 80018bc: 687b ldr r3, [r7, #4]
- 80018be: 2201 movs r2, #1
- 80018c0: f883 2034 strb.w r2, [r3, #52] @ 0x34
- ahrs->rampedGain = INITIAL_GAIN;
- 80018c4: 687b ldr r3, [r7, #4]
- 80018c6: 4a1e ldr r2, [pc, #120] @ (8001940 )
- 80018c8: 639a str r2, [r3, #56] @ 0x38
- ahrs->angularRateRecovery = false;
- 80018ca: 687b ldr r3, [r7, #4]
- 80018cc: 2200 movs r2, #0
- 80018ce: f883 2040 strb.w r2, [r3, #64] @ 0x40
- ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
- 80018d2: 687b ldr r3, [r7, #4]
- 80018d4: f04f 0200 mov.w r2, #0
- 80018d8: 645a str r2, [r3, #68] @ 0x44
- 80018da: 687b ldr r3, [r7, #4]
- 80018dc: f04f 0200 mov.w r2, #0
- 80018e0: 649a str r2, [r3, #72] @ 0x48
- 80018e2: 687b ldr r3, [r7, #4]
- 80018e4: f04f 0200 mov.w r2, #0
- 80018e8: 64da str r2, [r3, #76] @ 0x4c
- ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
- 80018ea: 687b ldr r3, [r7, #4]
- 80018ec: f04f 0200 mov.w r2, #0
- 80018f0: 651a str r2, [r3, #80] @ 0x50
- 80018f2: 687b ldr r3, [r7, #4]
- 80018f4: f04f 0200 mov.w r2, #0
- 80018f8: 655a str r2, [r3, #84] @ 0x54
- 80018fa: 687b ldr r3, [r7, #4]
- 80018fc: f04f 0200 mov.w r2, #0
- 8001900: 659a str r2, [r3, #88] @ 0x58
- ahrs->accelerometerIgnored = false;
- 8001902: 687b ldr r3, [r7, #4]
- 8001904: 2200 movs r2, #0
- 8001906: f883 205c strb.w r2, [r3, #92] @ 0x5c
- ahrs->accelerationRecoveryTrigger = 0;
- 800190a: 687b ldr r3, [r7, #4]
- 800190c: 2200 movs r2, #0
- 800190e: 661a str r2, [r3, #96] @ 0x60
- ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001910: 687b ldr r3, [r7, #4]
- 8001912: 695b ldr r3, [r3, #20]
- 8001914: 461a mov r2, r3
- 8001916: 687b ldr r3, [r7, #4]
- 8001918: 665a str r2, [r3, #100] @ 0x64
- ahrs->magnetometerIgnored = false;
- 800191a: 687b ldr r3, [r7, #4]
- 800191c: 2200 movs r2, #0
- 800191e: f883 2068 strb.w r2, [r3, #104] @ 0x68
- ahrs->magneticRecoveryTrigger = 0;
- 8001922: 687b ldr r3, [r7, #4]
- 8001924: 2200 movs r2, #0
- 8001926: 66da str r2, [r3, #108] @ 0x6c
- ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001928: 687b ldr r3, [r7, #4]
- 800192a: 695b ldr r3, [r3, #20]
- 800192c: 461a mov r2, r3
- 800192e: 687b ldr r3, [r7, #4]
- 8001930: 671a str r2, [r3, #112] @ 0x70
-}
- 8001932: bf00 nop
- 8001934: 3744 adds r7, #68 @ 0x44
- 8001936: 46bd mov sp, r7
- 8001938: f85d 7b04 ldr.w r7, [sp], #4
- 800193c: 4770 bx lr
- 800193e: bf00 nop
- 8001940: 41200000 .word 0x41200000
-
-08001944 :
-/**
- * @brief Sets the AHRS algorithm settings.
- * @param ahrs AHRS algorithm structure.
- * @param settings Settings.
- */
-void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) {
- 8001944: b580 push {r7, lr}
- 8001946: b082 sub sp, #8
- 8001948: af00 add r7, sp, #0
- 800194a: 6078 str r0, [r7, #4]
- 800194c: 6039 str r1, [r7, #0]
- ahrs->settings.convention = settings->convention;
- 800194e: 683b ldr r3, [r7, #0]
- 8001950: 781a ldrb r2, [r3, #0]
- 8001952: 687b ldr r3, [r7, #4]
- 8001954: 701a strb r2, [r3, #0]
- ahrs->settings.gain = settings->gain;
- 8001956: 683b ldr r3, [r7, #0]
- 8001958: 685a ldr r2, [r3, #4]
- 800195a: 687b ldr r3, [r7, #4]
- 800195c: 605a str r2, [r3, #4]
- ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
- 800195e: 683b ldr r3, [r7, #0]
- 8001960: edd3 7a02 vldr s15, [r3, #8]
- 8001964: eef5 7a40 vcmp.f32 s15, #0.0
- 8001968: eef1 fa10 vmrs APSR_nzcv, fpscr
- 800196c: d007 beq.n 800197e
- 800196e: 683b ldr r3, [r7, #0]
- 8001970: edd3 7a02 vldr s15, [r3, #8]
- 8001974: ed9f 7a4c vldr s14, [pc, #304] @ 8001aa8
- 8001978: ee67 7a87 vmul.f32 s15, s15, s14
- 800197c: e001 b.n 8001982
- 800197e: eddf 7a4b vldr s15, [pc, #300] @ 8001aac
- 8001982: 687b ldr r3, [r7, #4]
- 8001984: edc3 7a02 vstr s15, [r3, #8]
- ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
- 8001988: 683b ldr r3, [r7, #0]
- 800198a: edd3 7a03 vldr s15, [r3, #12]
- 800198e: eef5 7a40 vcmp.f32 s15, #0.0
- 8001992: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001996: d01b beq.n 80019d0
- 8001998: 683b ldr r3, [r7, #0]
- 800199a: edd3 7a03 vldr s15, [r3, #12]
- 800199e: eeb0 0a67 vmov.f32 s0, s15
- 80019a2: f7ff fa9f bl 8000ee4
- 80019a6: eef0 7a40 vmov.f32 s15, s0
- 80019aa: eeb0 0a67 vmov.f32 s0, s15
- 80019ae: f008 fc29 bl 800a204
- 80019b2: eef0 7a40 vmov.f32 s15, s0
- 80019b6: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 80019ba: ee67 7a87 vmul.f32 s15, s15, s14
- 80019be: eef0 0a00 vmov.f32 s1, #0 @ 0x40000000 2.0
- 80019c2: eeb0 0a67 vmov.f32 s0, s15
- 80019c6: f008 fb9f bl 800a108
- 80019ca: eef0 7a40 vmov.f32 s15, s0
- 80019ce: e001 b.n 80019d4
- 80019d0: eddf 7a36 vldr s15, [pc, #216] @ 8001aac
- 80019d4: 687b ldr r3, [r7, #4]
- 80019d6: edc3 7a03 vstr s15, [r3, #12]
- ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
- 80019da: 683b ldr r3, [r7, #0]
- 80019dc: edd3 7a04 vldr s15, [r3, #16]
- 80019e0: eef5 7a40 vcmp.f32 s15, #0.0
- 80019e4: eef1 fa10 vmrs APSR_nzcv, fpscr
- 80019e8: d01b beq.n 8001a22
- 80019ea: 683b ldr r3, [r7, #0]
- 80019ec: edd3 7a04 vldr s15, [r3, #16]
- 80019f0: eeb0 0a67 vmov.f32 s0, s15
- 80019f4: f7ff fa76 bl 8000ee4
- 80019f8: eef0 7a40 vmov.f32 s15, s0
- 80019fc: eeb0 0a67 vmov.f32 s0, s15
- 8001a00: f008 fc00 bl 800a204
- 8001a04: eef0 7a40 vmov.f32 s15, s0
- 8001a08: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 8001a0c: ee67 7a87 vmul.f32 s15, s15, s14
- 8001a10: eef0 0a00 vmov.f32 s1, #0 @ 0x40000000 2.0
- 8001a14: eeb0 0a67 vmov.f32 s0, s15
- 8001a18: f008 fb76 bl 800a108
- 8001a1c: eef0 7a40 vmov.f32 s15, s0
- 8001a20: e001 b.n 8001a26
- 8001a22: eddf 7a22 vldr s15, [pc, #136] @ 8001aac
- 8001a26: 687b ldr r3, [r7, #4]
- 8001a28: edc3 7a04 vstr s15, [r3, #16]
- ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
- 8001a2c: 683b ldr r3, [r7, #0]
- 8001a2e: 695a ldr r2, [r3, #20]
- 8001a30: 687b ldr r3, [r7, #4]
- 8001a32: 615a str r2, [r3, #20]
- ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001a34: 687b ldr r3, [r7, #4]
- 8001a36: 695b ldr r3, [r3, #20]
- 8001a38: 461a mov r2, r3
- 8001a3a: 687b ldr r3, [r7, #4]
- 8001a3c: 665a str r2, [r3, #100] @ 0x64
- ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001a3e: 687b ldr r3, [r7, #4]
- 8001a40: 695b ldr r3, [r3, #20]
- 8001a42: 461a mov r2, r3
- 8001a44: 687b ldr r3, [r7, #4]
- 8001a46: 671a str r2, [r3, #112] @ 0x70
- if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
- 8001a48: 683b ldr r3, [r7, #0]
- 8001a4a: edd3 7a01 vldr s15, [r3, #4]
- 8001a4e: eef5 7a40 vcmp.f32 s15, #0.0
- 8001a52: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001a56: d003 beq.n 8001a60
- 8001a58: 683b ldr r3, [r7, #0]
- 8001a5a: 695b ldr r3, [r3, #20]
- 8001a5c: 2b00 cmp r3, #0
- 8001a5e: d105 bne.n 8001a6c
- ahrs->settings.accelerationRejection = FLT_MAX;
- 8001a60: 687b ldr r3, [r7, #4]
- 8001a62: 4a13 ldr r2, [pc, #76] @ (8001ab0 )
- 8001a64: 60da str r2, [r3, #12]
- ahrs->settings.magneticRejection = FLT_MAX;
- 8001a66: 687b ldr r3, [r7, #4]
- 8001a68: 4a11 ldr r2, [pc, #68] @ (8001ab0 )
- 8001a6a: 611a str r2, [r3, #16]
- }
- if (ahrs->initialising == false) {
- 8001a6c: 687b ldr r3, [r7, #4]
- 8001a6e: f893 3034 ldrb.w r3, [r3, #52] @ 0x34
- 8001a72: f083 0301 eor.w r3, r3, #1
- 8001a76: b2db uxtb r3, r3
- 8001a78: 2b00 cmp r3, #0
- 8001a7a: d003 beq.n 8001a84
- ahrs->rampedGain = ahrs->settings.gain;
- 8001a7c: 687b ldr r3, [r7, #4]
- 8001a7e: 685a ldr r2, [r3, #4]
- 8001a80: 687b ldr r3, [r7, #4]
- 8001a82: 639a str r2, [r3, #56] @ 0x38
- }
- ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
- 8001a84: 687b ldr r3, [r7, #4]
- 8001a86: edd3 7a01 vldr s15, [r3, #4]
- 8001a8a: eeb2 7a04 vmov.f32 s14, #36 @ 0x41200000 10.0
- 8001a8e: ee37 7a67 vsub.f32 s14, s14, s15
- 8001a92: eef0 6a08 vmov.f32 s13, #8 @ 0x40400000 3.0
- 8001a96: eec7 7a26 vdiv.f32 s15, s14, s13
- 8001a9a: 687b ldr r3, [r7, #4]
- 8001a9c: edc3 7a0f vstr s15, [r3, #60] @ 0x3c
-}
- 8001aa0: bf00 nop
- 8001aa2: 3708 adds r7, #8
- 8001aa4: 46bd mov sp, r7
- 8001aa6: bd80 pop {r7, pc}
- 8001aa8: 3f7ae148 .word 0x3f7ae148
- 8001aac: 7f7fffff .word 0x7f7fffff
- 8001ab0: 7f7fffff .word 0x7f7fffff
-
-08001ab4 :
- * @param gyroscope Gyroscope measurement in degrees per second.
- * @param accelerometer Accelerometer measurement in g.
- * @param magnetometer Magnetometer measurement in arbitrary units.
- * @param deltaTime Delta time in seconds.
- */
-void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) {
- 8001ab4: b590 push {r4, r7, lr}
- 8001ab6: b0bf sub sp, #252 @ 0xfc
- 8001ab8: af00 add r7, sp, #0
- 8001aba: 62f8 str r0, [r7, #44] @ 0x2c
- 8001abc: eeb0 5a61 vmov.f32 s10, s3
- 8001ac0: eef0 5a42 vmov.f32 s11, s4
- 8001ac4: eeb0 6a62 vmov.f32 s12, s5
- 8001ac8: eef0 6a43 vmov.f32 s13, s6
- 8001acc: eeb0 7a63 vmov.f32 s14, s7
- 8001ad0: eef0 7a44 vmov.f32 s15, s8
- 8001ad4: edc7 4a01 vstr s9, [r7, #4]
- 8001ad8: ed87 0a08 vstr s0, [r7, #32]
- 8001adc: edc7 0a09 vstr s1, [r7, #36] @ 0x24
- 8001ae0: ed87 1a0a vstr s2, [r7, #40] @ 0x28
- 8001ae4: ed87 5a05 vstr s10, [r7, #20]
- 8001ae8: edc7 5a06 vstr s11, [r7, #24]
- 8001aec: ed87 6a07 vstr s12, [r7, #28]
- 8001af0: edc7 6a02 vstr s13, [r7, #8]
- 8001af4: ed87 7a03 vstr s14, [r7, #12]
- 8001af8: edc7 7a04 vstr s15, [r7, #16]
-#define Q ahrs->quaternion.element
-
- // Store accelerometer
- ahrs->accelerometer = accelerometer;
- 8001afc: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001afe: 3328 adds r3, #40 @ 0x28
- 8001b00: f107 0214 add.w r2, r7, #20
- 8001b04: ca07 ldmia r2, {r0, r1, r2}
- 8001b06: e883 0007 stmia.w r3, {r0, r1, r2}
-
- // Reinitialise if gyroscope range exceeded
- if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
- 8001b0a: edd7 7a08 vldr s15, [r7, #32]
- 8001b0e: eeb0 7ae7 vabs.f32 s14, s15
- 8001b12: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b14: edd3 7a02 vldr s15, [r3, #8]
- 8001b18: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001b1c: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001b20: dc17 bgt.n 8001b52
- 8001b22: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 8001b26: eeb0 7ae7 vabs.f32 s14, s15
- 8001b2a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b2c: edd3 7a02 vldr s15, [r3, #8]
- 8001b30: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001b34: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001b38: dc0b bgt.n 8001b52
- 8001b3a: edd7 7a0a vldr s15, [r7, #40] @ 0x28
- 8001b3e: eeb0 7ae7 vabs.f32 s14, s15
- 8001b42: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b44: edd3 7a02 vldr s15, [r3, #8]
- 8001b48: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001b4c: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001b50: dd15 ble.n 8001b7e
- const FusionQuaternion quaternion = ahrs->quaternion;
- 8001b52: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b54: f107 043c add.w r4, r7, #60 @ 0x3c
- 8001b58: 3318 adds r3, #24
- 8001b5a: cb0f ldmia r3, {r0, r1, r2, r3}
- 8001b5c: e884 000f stmia.w r4, {r0, r1, r2, r3}
- FusionAhrsReset(ahrs);
- 8001b60: 6af8 ldr r0, [r7, #44] @ 0x2c
- 8001b62: f7ff fe8b bl 800187c
- ahrs->quaternion = quaternion;
- 8001b66: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b68: f103 0418 add.w r4, r3, #24
- 8001b6c: f107 033c add.w r3, r7, #60 @ 0x3c
- 8001b70: cb0f ldmia r3, {r0, r1, r2, r3}
- 8001b72: e884 000f stmia.w r4, {r0, r1, r2, r3}
- ahrs->angularRateRecovery = true;
- 8001b76: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b78: 2201 movs r2, #1
- 8001b7a: f883 2040 strb.w r2, [r3, #64] @ 0x40
- }
-
- // Ramp down gain during initialisation
- if (ahrs->initialising) {
- 8001b7e: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b80: f893 3034 ldrb.w r3, [r3, #52] @ 0x34
- 8001b84: 2b00 cmp r3, #0
- 8001b86: d02d beq.n 8001be4
- ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
- 8001b88: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b8a: ed93 7a0e vldr s14, [r3, #56] @ 0x38
- 8001b8e: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001b90: edd3 6a0f vldr s13, [r3, #60] @ 0x3c
- 8001b94: edd7 7a01 vldr s15, [r7, #4]
- 8001b98: ee66 7aa7 vmul.f32 s15, s13, s15
- 8001b9c: ee77 7a67 vsub.f32 s15, s14, s15
- 8001ba0: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ba2: edc3 7a0e vstr s15, [r3, #56] @ 0x38
- if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
- 8001ba6: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ba8: ed93 7a0e vldr s14, [r3, #56] @ 0x38
- 8001bac: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bae: edd3 7a01 vldr s15, [r3, #4]
- 8001bb2: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001bb6: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001bba: d407 bmi.n 8001bcc
- 8001bbc: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bbe: edd3 7a01 vldr s15, [r3, #4]
- 8001bc2: eef5 7a40 vcmp.f32 s15, #0.0
- 8001bc6: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001bca: d10b bne.n 8001be4
- ahrs->rampedGain = ahrs->settings.gain;
- 8001bcc: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bce: 685a ldr r2, [r3, #4]
- 8001bd0: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bd2: 639a str r2, [r3, #56] @ 0x38
- ahrs->initialising = false;
- 8001bd4: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bd6: 2200 movs r2, #0
- 8001bd8: f883 2034 strb.w r2, [r3, #52] @ 0x34
- ahrs->angularRateRecovery = false;
- 8001bdc: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001bde: 2200 movs r2, #0
- 8001be0: f883 2040 strb.w r2, [r3, #64] @ 0x40
- }
- }
-
- // Calculate direction of gravity indicated by algorithm
- const FusionVector halfGravity = HalfGravity(ahrs);
- 8001be4: 6af8 ldr r0, [r7, #44] @ 0x2c
- 8001be6: f000 faf4 bl 80021d2
- 8001bea: eef0 6a40 vmov.f32 s13, s0
- 8001bee: eeb0 7a60 vmov.f32 s14, s1
- 8001bf2: eef0 7a41 vmov.f32 s15, s2
- 8001bf6: edc7 6a25 vstr s13, [r7, #148] @ 0x94
- 8001bfa: ed87 7a26 vstr s14, [r7, #152] @ 0x98
- 8001bfe: edc7 7a27 vstr s15, [r7, #156] @ 0x9c
-
- // Calculate accelerometer feedback
- FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
- 8001c02: f04f 0300 mov.w r3, #0
- 8001c06: f8c7 3088 str.w r3, [r7, #136] @ 0x88
- 8001c0a: f04f 0300 mov.w r3, #0
- 8001c0e: f8c7 308c str.w r3, [r7, #140] @ 0x8c
- 8001c12: f04f 0300 mov.w r3, #0
- 8001c16: f8c7 3090 str.w r3, [r7, #144] @ 0x90
- ahrs->accelerometerIgnored = true;
- 8001c1a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001c1c: 2201 movs r2, #1
- 8001c1e: f883 205c strb.w r2, [r3, #92] @ 0x5c
- if (FusionVectorIsZero(accelerometer) == false) {
- 8001c22: edd7 6a05 vldr s13, [r7, #20]
- 8001c26: ed97 7a06 vldr s14, [r7, #24]
- 8001c2a: edd7 7a07 vldr s15, [r7, #28]
- 8001c2e: eeb0 0a66 vmov.f32 s0, s13
- 8001c32: eef0 0a47 vmov.f32 s1, s14
- 8001c36: eeb0 1a67 vmov.f32 s2, s15
- 8001c3a: f7ff f9d7 bl 8000fec
- 8001c3e: 4603 mov r3, r0
- 8001c40: f083 0301 eor.w r3, r3, #1
- 8001c44: b2db uxtb r3, r3
- 8001c46: 2b00 cmp r3, #0
- 8001c48: f000 809c beq.w 8001d84
-
- // Calculate accelerometer feedback scaled by 0.5
- ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
- 8001c4c: edd7 6a05 vldr s13, [r7, #20]
- 8001c50: ed97 7a06 vldr s14, [r7, #24]
- 8001c54: edd7 7a07 vldr s15, [r7, #28]
- 8001c58: eeb0 0a66 vmov.f32 s0, s13
- 8001c5c: eef0 0a47 vmov.f32 s1, s14
- 8001c60: eeb0 1a67 vmov.f32 s2, s15
- 8001c64: f7ff fc22 bl 80014ac
- 8001c68: eef0 6a40 vmov.f32 s13, s0
- 8001c6c: eeb0 7a60 vmov.f32 s14, s1
- 8001c70: eef0 7a41 vmov.f32 s15, s2
- 8001c74: edc7 6a28 vstr s13, [r7, #160] @ 0xa0
- 8001c78: ed87 7a29 vstr s14, [r7, #164] @ 0xa4
- 8001c7c: edc7 7a2a vstr s15, [r7, #168] @ 0xa8
- 8001c80: 6afc ldr r4, [r7, #44] @ 0x2c
- 8001c82: ed97 5a25 vldr s10, [r7, #148] @ 0x94
- 8001c86: edd7 5a26 vldr s11, [r7, #152] @ 0x98
- 8001c8a: ed97 6a27 vldr s12, [r7, #156] @ 0x9c
- 8001c8e: edd7 6a28 vldr s13, [r7, #160] @ 0xa0
- 8001c92: ed97 7a29 vldr s14, [r7, #164] @ 0xa4
- 8001c96: edd7 7a2a vldr s15, [r7, #168] @ 0xa8
- 8001c9a: eef0 1a45 vmov.f32 s3, s10
- 8001c9e: eeb0 2a65 vmov.f32 s4, s11
- 8001ca2: eef0 2a46 vmov.f32 s5, s12
- 8001ca6: eeb0 0a66 vmov.f32 s0, s13
- 8001caa: eef0 0a47 vmov.f32 s1, s14
- 8001cae: eeb0 1a67 vmov.f32 s2, s15
- 8001cb2: f000 fc55 bl 8002560
- 8001cb6: eef0 6a40 vmov.f32 s13, s0
- 8001cba: eeb0 7a60 vmov.f32 s14, s1
- 8001cbe: eef0 7a41 vmov.f32 s15, s2
- 8001cc2: edc4 6a11 vstr s13, [r4, #68] @ 0x44
- 8001cc6: ed84 7a12 vstr s14, [r4, #72] @ 0x48
- 8001cca: edc4 7a13 vstr s15, [r4, #76] @ 0x4c
-
- // Don't ignore accelerometer if acceleration error below threshold
- if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
- 8001cce: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001cd0: f893 3034 ldrb.w r3, [r3, #52] @ 0x34
- 8001cd4: 2b00 cmp r3, #0
- 8001cd6: d118 bne.n 8001d0a
- 8001cd8: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001cda: edd3 6a11 vldr s13, [r3, #68] @ 0x44
- 8001cde: ed93 7a12 vldr s14, [r3, #72] @ 0x48
- 8001ce2: edd3 7a13 vldr s15, [r3, #76] @ 0x4c
- 8001ce6: eeb0 0a66 vmov.f32 s0, s13
- 8001cea: eef0 0a47 vmov.f32 s1, s14
- 8001cee: eeb0 1a67 vmov.f32 s2, s15
- 8001cf2: f7ff fb67 bl 80013c4
- 8001cf6: eeb0 7a40 vmov.f32 s14, s0
- 8001cfa: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001cfc: edd3 7a03 vldr s15, [r3, #12]
- 8001d00: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001d04: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001d08: d80a bhi.n 8001d20
- ahrs->accelerometerIgnored = false;
- 8001d0a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d0c: 2200 movs r2, #0
- 8001d0e: f883 205c strb.w r2, [r3, #92] @ 0x5c
- ahrs->accelerationRecoveryTrigger -= 9;
- 8001d12: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d14: 6e1b ldr r3, [r3, #96] @ 0x60
- 8001d16: f1a3 0209 sub.w r2, r3, #9
- 8001d1a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d1c: 661a str r2, [r3, #96] @ 0x60
- 8001d1e: e004 b.n 8001d2a
- } else {
- ahrs->accelerationRecoveryTrigger += 1;
- 8001d20: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d22: 6e1b ldr r3, [r3, #96] @ 0x60
- 8001d24: 1c5a adds r2, r3, #1
- 8001d26: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d28: 661a str r2, [r3, #96] @ 0x60
- }
-
- // Don't ignore accelerometer during acceleration recovery
- if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
- 8001d2a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d2c: 6e1a ldr r2, [r3, #96] @ 0x60
- 8001d2e: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d30: 6e5b ldr r3, [r3, #100] @ 0x64
- 8001d32: 429a cmp r2, r3
- 8001d34: dd07 ble.n 8001d46
- ahrs->accelerationRecoveryTimeout = 0;
- 8001d36: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d38: 2200 movs r2, #0
- 8001d3a: 665a str r2, [r3, #100] @ 0x64
- ahrs->accelerometerIgnored = false;
- 8001d3c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d3e: 2200 movs r2, #0
- 8001d40: f883 205c strb.w r2, [r3, #92] @ 0x5c
- 8001d44: e004 b.n 8001d50
- } else {
- ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001d46: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d48: 695b ldr r3, [r3, #20]
- 8001d4a: 461a mov r2, r3
- 8001d4c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d4e: 665a str r2, [r3, #100] @ 0x64
- }
- ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
- 8001d50: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d52: 6e18 ldr r0, [r3, #96] @ 0x60
- 8001d54: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d56: 695b ldr r3, [r3, #20]
- 8001d58: 461a mov r2, r3
- 8001d5a: 2100 movs r1, #0
- 8001d5c: f000 fcb5 bl 80026ca
- 8001d60: 4602 mov r2, r0
- 8001d62: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d64: 661a str r2, [r3, #96] @ 0x60
-
- // Apply accelerometer feedback
- if (ahrs->accelerometerIgnored == false) {
- 8001d66: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d68: f893 305c ldrb.w r3, [r3, #92] @ 0x5c
- 8001d6c: f083 0301 eor.w r3, r3, #1
- 8001d70: b2db uxtb r3, r3
- 8001d72: 2b00 cmp r3, #0
- 8001d74: d006 beq.n 8001d84
- halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
- 8001d76: 6afa ldr r2, [r7, #44] @ 0x2c
- 8001d78: f107 0388 add.w r3, r7, #136 @ 0x88
- 8001d7c: 3244 adds r2, #68 @ 0x44
- 8001d7e: ca07 ldmia r2, {r0, r1, r2}
- 8001d80: e883 0007 stmia.w r3, {r0, r1, r2}
- }
- }
-
- // Calculate magnetometer feedback
- FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
- 8001d84: f04f 0300 mov.w r3, #0
- 8001d88: 673b str r3, [r7, #112] @ 0x70
- 8001d8a: f04f 0300 mov.w r3, #0
- 8001d8e: 677b str r3, [r7, #116] @ 0x74
- 8001d90: f04f 0300 mov.w r3, #0
- 8001d94: 67bb str r3, [r7, #120] @ 0x78
- ahrs->magnetometerIgnored = true;
- 8001d96: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001d98: 2201 movs r2, #1
- 8001d9a: f883 2068 strb.w r2, [r3, #104] @ 0x68
- if (FusionVectorIsZero(magnetometer) == false) {
- 8001d9e: edd7 6a02 vldr s13, [r7, #8]
- 8001da2: ed97 7a03 vldr s14, [r7, #12]
- 8001da6: edd7 7a04 vldr s15, [r7, #16]
- 8001daa: eeb0 0a66 vmov.f32 s0, s13
- 8001dae: eef0 0a47 vmov.f32 s1, s14
- 8001db2: eeb0 1a67 vmov.f32 s2, s15
- 8001db6: f7ff f919 bl 8000fec
- 8001dba: 4603 mov r3, r0
- 8001dbc: f083 0301 eor.w r3, r3, #1
- 8001dc0: b2db uxtb r3, r3
- 8001dc2: 2b00 cmp r3, #0
- 8001dc4: f000 80d1 beq.w 8001f6a
-
- // Calculate direction of magnetic field indicated by algorithm
- const FusionVector halfMagnetic = HalfMagnetic(ahrs);
- 8001dc8: 6af8 ldr r0, [r7, #44] @ 0x2c
- 8001dca: f000 fabf bl 800234c
- 8001dce: eef0 6a40 vmov.f32 s13, s0
- 8001dd2: eeb0 7a60 vmov.f32 s14, s1
- 8001dd6: eef0 7a41 vmov.f32 s15, s2
- 8001dda: edc7 6a0c vstr s13, [r7, #48] @ 0x30
- 8001dde: ed87 7a0d vstr s14, [r7, #52] @ 0x34
- 8001de2: edc7 7a0e vstr s15, [r7, #56] @ 0x38
-
- // Calculate magnetometer feedback scaled by 0.5
- ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
- 8001de6: ed97 5a02 vldr s10, [r7, #8]
- 8001dea: edd7 5a03 vldr s11, [r7, #12]
- 8001dee: ed97 6a04 vldr s12, [r7, #16]
- 8001df2: edd7 6a25 vldr s13, [r7, #148] @ 0x94
- 8001df6: ed97 7a26 vldr s14, [r7, #152] @ 0x98
- 8001dfa: edd7 7a27 vldr s15, [r7, #156] @ 0x9c
- 8001dfe: eef0 1a45 vmov.f32 s3, s10
- 8001e02: eeb0 2a65 vmov.f32 s4, s11
- 8001e06: eef0 2a46 vmov.f32 s5, s12
- 8001e0a: eeb0 0a66 vmov.f32 s0, s13
- 8001e0e: eef0 0a47 vmov.f32 s1, s14
- 8001e12: eeb0 1a67 vmov.f32 s2, s15
- 8001e16: f7ff fa19 bl 800124c
- 8001e1a: eef0 6a40 vmov.f32 s13, s0
- 8001e1e: eeb0 7a60 vmov.f32 s14, s1
- 8001e22: eef0 7a41 vmov.f32 s15, s2
- 8001e26: edc7 6a2b vstr s13, [r7, #172] @ 0xac
- 8001e2a: ed87 7a2c vstr s14, [r7, #176] @ 0xb0
- 8001e2e: edc7 7a2d vstr s15, [r7, #180] @ 0xb4
- 8001e32: edd7 6a2b vldr s13, [r7, #172] @ 0xac
- 8001e36: ed97 7a2c vldr s14, [r7, #176] @ 0xb0
- 8001e3a: edd7 7a2d vldr s15, [r7, #180] @ 0xb4
- 8001e3e: eeb0 0a66 vmov.f32 s0, s13
- 8001e42: eef0 0a47 vmov.f32 s1, s14
- 8001e46: eeb0 1a67 vmov.f32 s2, s15
- 8001e4a: f7ff fb2f bl 80014ac
- 8001e4e: eef0 6a40 vmov.f32 s13, s0
- 8001e52: eeb0 7a60 vmov.f32 s14, s1
- 8001e56: eef0 7a41 vmov.f32 s15, s2
- 8001e5a: edc7 6a2e vstr s13, [r7, #184] @ 0xb8
- 8001e5e: ed87 7a2f vstr s14, [r7, #188] @ 0xbc
- 8001e62: edc7 7a30 vstr s15, [r7, #192] @ 0xc0
- 8001e66: 6afc ldr r4, [r7, #44] @ 0x2c
- 8001e68: ed97 5a0c vldr s10, [r7, #48] @ 0x30
- 8001e6c: edd7 5a0d vldr s11, [r7, #52] @ 0x34
- 8001e70: ed97 6a0e vldr s12, [r7, #56] @ 0x38
- 8001e74: edd7 6a2e vldr s13, [r7, #184] @ 0xb8
- 8001e78: ed97 7a2f vldr s14, [r7, #188] @ 0xbc
- 8001e7c: edd7 7a30 vldr s15, [r7, #192] @ 0xc0
- 8001e80: eef0 1a45 vmov.f32 s3, s10
- 8001e84: eeb0 2a65 vmov.f32 s4, s11
- 8001e88: eef0 2a46 vmov.f32 s5, s12
- 8001e8c: eeb0 0a66 vmov.f32 s0, s13
- 8001e90: eef0 0a47 vmov.f32 s1, s14
- 8001e94: eeb0 1a67 vmov.f32 s2, s15
- 8001e98: f000 fb62 bl 8002560
- 8001e9c: eef0 6a40 vmov.f32 s13, s0
- 8001ea0: eeb0 7a60 vmov.f32 s14, s1
- 8001ea4: eef0 7a41 vmov.f32 s15, s2
- 8001ea8: edc4 6a14 vstr s13, [r4, #80] @ 0x50
- 8001eac: ed84 7a15 vstr s14, [r4, #84] @ 0x54
- 8001eb0: edc4 7a16 vstr s15, [r4, #88] @ 0x58
-
- // Don't ignore magnetometer if magnetic error below threshold
- if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
- 8001eb4: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001eb6: f893 3034 ldrb.w r3, [r3, #52] @ 0x34
- 8001eba: 2b00 cmp r3, #0
- 8001ebc: d118 bne.n 8001ef0
- 8001ebe: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ec0: edd3 6a14 vldr s13, [r3, #80] @ 0x50
- 8001ec4: ed93 7a15 vldr s14, [r3, #84] @ 0x54
- 8001ec8: edd3 7a16 vldr s15, [r3, #88] @ 0x58
- 8001ecc: eeb0 0a66 vmov.f32 s0, s13
- 8001ed0: eef0 0a47 vmov.f32 s1, s14
- 8001ed4: eeb0 1a67 vmov.f32 s2, s15
- 8001ed8: f7ff fa74 bl 80013c4
- 8001edc: eeb0 7a40 vmov.f32 s14, s0
- 8001ee0: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ee2: edd3 7a04 vldr s15, [r3, #16]
- 8001ee6: eeb4 7ae7 vcmpe.f32 s14, s15
- 8001eea: eef1 fa10 vmrs APSR_nzcv, fpscr
- 8001eee: d80a bhi.n 8001f06
- ahrs->magnetometerIgnored = false;
- 8001ef0: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ef2: 2200 movs r2, #0
- 8001ef4: f883 2068 strb.w r2, [r3, #104] @ 0x68
- ahrs->magneticRecoveryTrigger -= 9;
- 8001ef8: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001efa: 6edb ldr r3, [r3, #108] @ 0x6c
- 8001efc: f1a3 0209 sub.w r2, r3, #9
- 8001f00: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f02: 66da str r2, [r3, #108] @ 0x6c
- 8001f04: e004 b.n 8001f10
- } else {
- ahrs->magneticRecoveryTrigger += 1;
- 8001f06: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f08: 6edb ldr r3, [r3, #108] @ 0x6c
- 8001f0a: 1c5a adds r2, r3, #1
- 8001f0c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f0e: 66da str r2, [r3, #108] @ 0x6c
- }
-
- // Don't ignore magnetometer during magnetic recovery
- if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
- 8001f10: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f12: 6eda ldr r2, [r3, #108] @ 0x6c
- 8001f14: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f16: 6f1b ldr r3, [r3, #112] @ 0x70
- 8001f18: 429a cmp r2, r3
- 8001f1a: dd07 ble.n 8001f2c
- ahrs->magneticRecoveryTimeout = 0;
- 8001f1c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f1e: 2200 movs r2, #0
- 8001f20: 671a str r2, [r3, #112] @ 0x70
- ahrs->magnetometerIgnored = false;
- 8001f22: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f24: 2200 movs r2, #0
- 8001f26: f883 2068 strb.w r2, [r3, #104] @ 0x68
- 8001f2a: e004 b.n 8001f36
- } else {
- ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
- 8001f2c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f2e: 695b ldr r3, [r3, #20]
- 8001f30: 461a mov r2, r3
- 8001f32: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f34: 671a str r2, [r3, #112] @ 0x70
- }
- ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
- 8001f36: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f38: 6ed8 ldr r0, [r3, #108] @ 0x6c
- 8001f3a: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f3c: 695b ldr r3, [r3, #20]
- 8001f3e: 461a mov r2, r3
- 8001f40: 2100 movs r1, #0
- 8001f42: f000 fbc2 bl 80026ca
- 8001f46: 4602 mov r2, r0
- 8001f48: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f4a: 66da str r2, [r3, #108] @ 0x6c
-
- // Apply magnetometer feedback
- if (ahrs->magnetometerIgnored == false) {
- 8001f4c: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001f4e: f893 3068 ldrb.w r3, [r3, #104] @ 0x68
- 8001f52: f083 0301 eor.w r3, r3, #1
- 8001f56: b2db uxtb r3, r3
- 8001f58: 2b00 cmp r3, #0
- 8001f5a: d006 beq.n 8001f6a
- halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
- 8001f5c: 6afa ldr r2, [r7, #44] @ 0x2c
- 8001f5e: f107 0370 add.w r3, r7, #112 @ 0x70
- 8001f62: 3250 adds r2, #80 @ 0x50
- 8001f64: ca07 ldmia r2, {r0, r1, r2}
- 8001f66: e883 0007 stmia.w r3, {r0, r1, r2}
- }
- }
-
- // Convert gyroscope to radians per second scaled by 0.5
- const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
- 8001f6a: eeb6 0a00 vmov.f32 s0, #96 @ 0x3f000000 0.5
- 8001f6e: f7fe ffb9 bl 8000ee4
- 8001f72: eeb0 6a40 vmov.f32 s12, s0
- 8001f76: edd7 6a08 vldr s13, [r7, #32]
- 8001f7a: ed97 7a09 vldr s14, [r7, #36] @ 0x24
- 8001f7e: edd7 7a0a vldr s15, [r7, #40] @ 0x28
- 8001f82: eef0 1a46 vmov.f32 s3, s12
- 8001f86: eeb0 0a66 vmov.f32 s0, s13
- 8001f8a: eef0 0a47 vmov.f32 s1, s14
- 8001f8e: eeb0 1a67 vmov.f32 s2, s15
- 8001f92: f7ff f8c9 bl 8001128
- 8001f96: eef0 6a40 vmov.f32 s13, s0
- 8001f9a: eeb0 7a60 vmov.f32 s14, s1
- 8001f9e: eef0 7a41 vmov.f32 s15, s2
- 8001fa2: edc7 6a16 vstr s13, [r7, #88] @ 0x58
- 8001fa6: ed87 7a17 vstr s14, [r7, #92] @ 0x5c
- 8001faa: edc7 7a18 vstr s15, [r7, #96] @ 0x60
-
- // Apply feedback to gyroscope
- const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
- 8001fae: ed97 5a1c vldr s10, [r7, #112] @ 0x70
- 8001fb2: edd7 5a1d vldr s11, [r7, #116] @ 0x74
- 8001fb6: ed97 6a1e vldr s12, [r7, #120] @ 0x78
- 8001fba: edd7 6a22 vldr s13, [r7, #136] @ 0x88
- 8001fbe: ed97 7a23 vldr s14, [r7, #140] @ 0x8c
- 8001fc2: edd7 7a24 vldr s15, [r7, #144] @ 0x90
- 8001fc6: eef0 1a45 vmov.f32 s3, s10
- 8001fca: eeb0 2a65 vmov.f32 s4, s11
- 8001fce: eef0 2a46 vmov.f32 s5, s12
- 8001fd2: eeb0 0a66 vmov.f32 s0, s13
- 8001fd6: eef0 0a47 vmov.f32 s1, s14
- 8001fda: eeb0 1a67 vmov.f32 s2, s15
- 8001fde: f7ff f835 bl 800104c
- 8001fe2: eef0 6a40 vmov.f32 s13, s0
- 8001fe6: eeb0 7a60 vmov.f32 s14, s1
- 8001fea: eef0 7a41 vmov.f32 s15, s2
- 8001fee: edc7 6a31 vstr s13, [r7, #196] @ 0xc4
- 8001ff2: ed87 7a32 vstr s14, [r7, #200] @ 0xc8
- 8001ff6: edc7 7a33 vstr s15, [r7, #204] @ 0xcc
- 8001ffa: 6afb ldr r3, [r7, #44] @ 0x2c
- 8001ffc: ed93 6a0e vldr s12, [r3, #56] @ 0x38
- 8002000: edd7 6a31 vldr s13, [r7, #196] @ 0xc4
- 8002004: ed97 7a32 vldr s14, [r7, #200] @ 0xc8
- 8002008: edd7 7a33 vldr s15, [r7, #204] @ 0xcc
- 800200c: eef0 1a46 vmov.f32 s3, s12
- 8002010: eeb0 0a66 vmov.f32 s0, s13
- 8002014: eef0 0a47 vmov.f32 s1, s14
- 8002018: eeb0 1a67 vmov.f32 s2, s15
- 800201c: f7ff f884 bl 8001128
- 8002020: eef0 6a40 vmov.f32 s13, s0
- 8002024: eeb0 7a60 vmov.f32 s14, s1
- 8002028: eef0 7a41 vmov.f32 s15, s2
- 800202c: edc7 6a34 vstr s13, [r7, #208] @ 0xd0
- 8002030: ed87 7a35 vstr s14, [r7, #212] @ 0xd4
- 8002034: edc7 7a36 vstr s15, [r7, #216] @ 0xd8
- 8002038: ed97 5a34 vldr s10, [r7, #208] @ 0xd0
- 800203c: edd7 5a35 vldr s11, [r7, #212] @ 0xd4
- 8002040: ed97 6a36 vldr s12, [r7, #216] @ 0xd8
- 8002044: edd7 6a16 vldr s13, [r7, #88] @ 0x58
- 8002048: ed97 7a17 vldr s14, [r7, #92] @ 0x5c
- 800204c: edd7 7a18 vldr s15, [r7, #96] @ 0x60
- 8002050: eef0 1a45 vmov.f32 s3, s10
- 8002054: eeb0 2a65 vmov.f32 s4, s11
- 8002058: eef0 2a46 vmov.f32 s5, s12
- 800205c: eeb0 0a66 vmov.f32 s0, s13
- 8002060: eef0 0a47 vmov.f32 s1, s14
- 8002064: eeb0 1a67 vmov.f32 s2, s15
- 8002068: f7fe fff0 bl 800104c
- 800206c: eef0 6a40 vmov.f32 s13, s0
- 8002070: eeb0 7a60 vmov.f32 s14, s1
- 8002074: eef0 7a41 vmov.f32 s15, s2
- 8002078: edc7 6a13 vstr s13, [r7, #76] @ 0x4c
- 800207c: ed87 7a14 vstr s14, [r7, #80] @ 0x50
- 8002080: edc7 7a15 vstr s15, [r7, #84] @ 0x54
-
- // Integrate rate of change of quaternion
- ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
- 8002084: edd7 6a13 vldr s13, [r7, #76] @ 0x4c
- 8002088: ed97 7a14 vldr s14, [r7, #80] @ 0x50
- 800208c: edd7 7a15 vldr s15, [r7, #84] @ 0x54
- 8002090: edd7 1a01 vldr s3, [r7, #4]
- 8002094: eeb0 0a66 vmov.f32 s0, s13
- 8002098: eef0 0a47 vmov.f32 s1, s14
- 800209c: eeb0 1a67 vmov.f32 s2, s15
- 80020a0: f7ff f842 bl 8001128
- 80020a4: eef0 6a40 vmov.f32 s13, s0
- 80020a8: eeb0 7a60 vmov.f32 s14, s1
- 80020ac: eef0 7a41 vmov.f32 s15, s2
- 80020b0: edc7 6a37 vstr s13, [r7, #220] @ 0xdc
- 80020b4: ed87 7a38 vstr s14, [r7, #224] @ 0xe0
- 80020b8: edc7 7a39 vstr s15, [r7, #228] @ 0xe4
- 80020bc: edd7 4a37 vldr s9, [r7, #220] @ 0xdc
- 80020c0: ed97 5a38 vldr s10, [r7, #224] @ 0xe0
- 80020c4: edd7 5a39 vldr s11, [r7, #228] @ 0xe4
- 80020c8: 6afb ldr r3, [r7, #44] @ 0x2c
- 80020ca: ed93 6a06 vldr s12, [r3, #24]
- 80020ce: edd3 6a07 vldr s13, [r3, #28]
- 80020d2: ed93 7a08 vldr s14, [r3, #32]
- 80020d6: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80020da: eeb0 2a64 vmov.f32 s4, s9
- 80020de: eef0 2a45 vmov.f32 s5, s10
- 80020e2: eeb0 3a65 vmov.f32 s6, s11
- 80020e6: eeb0 0a46 vmov.f32 s0, s12
- 80020ea: eef0 0a66 vmov.f32 s1, s13
- 80020ee: eeb0 1a47 vmov.f32 s2, s14
- 80020f2: eef0 1a67 vmov.f32 s3, s15
- 80020f6: f7ff fa8e bl 8001616
- 80020fa: eeb0 6a40 vmov.f32 s12, s0
- 80020fe: eef0 6a60 vmov.f32 s13, s1
- 8002102: eeb0 7a41 vmov.f32 s14, s2
- 8002106: eef0 7a61 vmov.f32 s15, s3
- 800210a: ed87 6a3a vstr s12, [r7, #232] @ 0xe8
- 800210e: edc7 6a3b vstr s13, [r7, #236] @ 0xec
- 8002112: ed87 7a3c vstr s14, [r7, #240] @ 0xf0
- 8002116: edc7 7a3d vstr s15, [r7, #244] @ 0xf4
- 800211a: 6afc ldr r4, [r7, #44] @ 0x2c
- 800211c: ed97 4a3a vldr s8, [r7, #232] @ 0xe8
- 8002120: edd7 4a3b vldr s9, [r7, #236] @ 0xec
- 8002124: ed97 5a3c vldr s10, [r7, #240] @ 0xf0
- 8002128: edd7 5a3d vldr s11, [r7, #244] @ 0xf4
- 800212c: 6afb ldr r3, [r7, #44] @ 0x2c
- 800212e: ed93 6a06 vldr s12, [r3, #24]
- 8002132: edd3 6a07 vldr s13, [r3, #28]
- 8002136: ed93 7a08 vldr s14, [r3, #32]
- 800213a: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 800213e: eeb0 2a44 vmov.f32 s4, s8
- 8002142: eef0 2a64 vmov.f32 s5, s9
- 8002146: eeb0 3a45 vmov.f32 s6, s10
- 800214a: eef0 3a65 vmov.f32 s7, s11
- 800214e: eeb0 0a46 vmov.f32 s0, s12
- 8002152: eef0 0a66 vmov.f32 s1, s13
- 8002156: eeb0 1a47 vmov.f32 s2, s14
- 800215a: eef0 1a67 vmov.f32 s3, s15
- 800215e: f7ff f9f8 bl 8001552
- 8002162: eeb0 6a40 vmov.f32 s12, s0
- 8002166: eef0 6a60 vmov.f32 s13, s1
- 800216a: eeb0 7a41 vmov.f32 s14, s2
- 800216e: eef0 7a61 vmov.f32 s15, s3
- 8002172: ed84 6a06 vstr s12, [r4, #24]
- 8002176: edc4 6a07 vstr s13, [r4, #28]
- 800217a: ed84 7a08 vstr s14, [r4, #32]
- 800217e: edc4 7a09 vstr s15, [r4, #36] @ 0x24
-
- // Normalise quaternion
- ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
- 8002182: 6afc ldr r4, [r7, #44] @ 0x2c
- 8002184: 6afb ldr r3, [r7, #44] @ 0x2c
- 8002186: ed93 6a06 vldr s12, [r3, #24]
- 800218a: edd3 6a07 vldr s13, [r3, #28]
- 800218e: ed93 7a08 vldr s14, [r3, #32]
- 8002192: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 8002196: eeb0 0a46 vmov.f32 s0, s12
- 800219a: eef0 0a66 vmov.f32 s1, s13
- 800219e: eeb0 1a47 vmov.f32 s2, s14
- 80021a2: eef0 1a67 vmov.f32 s3, s15
- 80021a6: f7ff fad6 bl 8001756
- 80021aa: eeb0 6a40 vmov.f32 s12, s0
- 80021ae: eef0 6a60 vmov.f32 s13, s1
- 80021b2: eeb0 7a41 vmov.f32 s14, s2
- 80021b6: eef0 7a61 vmov.f32 s15, s3
- 80021ba: ed84 6a06 vstr s12, [r4, #24]
- 80021be: edc4 6a07 vstr s13, [r4, #28]
- 80021c2: ed84 7a08 vstr s14, [r4, #32]
- 80021c6: edc4 7a09 vstr s15, [r4, #36] @ 0x24
-#undef Q
-}
- 80021ca: bf00 nop
- 80021cc: 37fc adds r7, #252 @ 0xfc
- 80021ce: 46bd mov sp, r7
- 80021d0: bd90 pop {r4, r7, pc}
-
-080021d2 :
-/**
- * @brief Returns the direction of gravity scaled by 0.5.
- * @param ahrs AHRS algorithm structure.
- * @return Direction of gravity scaled by 0.5.
- */
-static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) {
- 80021d2: b480 push {r7}
- 80021d4: b093 sub sp, #76 @ 0x4c
- 80021d6: af00 add r7, sp, #0
- 80021d8: 6178 str r0, [r7, #20]
-#define Q ahrs->quaternion.element
- switch (ahrs->settings.convention) {
- 80021da: 697b ldr r3, [r7, #20]
- 80021dc: 781b ldrb r3, [r3, #0]
- 80021de: 2b01 cmp r3, #1
- 80021e0: dc02 bgt.n 80021e8
- 80021e2: 2b00 cmp r3, #0
- 80021e4: da03 bge.n 80021ee
- 80021e6: e094 b.n 8002312
- 80021e8: 2b02 cmp r3, #2
- 80021ea: d048 beq.n 800227e
- 80021ec: e091 b.n 8002312
- case FusionConventionNwu:
- case FusionConventionEnu: {
- const FusionVector halfGravity = {.axis = {
- .x = Q.x * Q.z - Q.w * Q.y,
- 80021ee: 697b ldr r3, [r7, #20]
- 80021f0: ed93 7a07 vldr s14, [r3, #28]
- 80021f4: 697b ldr r3, [r7, #20]
- 80021f6: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80021fa: ee27 7a27 vmul.f32 s14, s14, s15
- 80021fe: 697b ldr r3, [r7, #20]
- 8002200: edd3 6a06 vldr s13, [r3, #24]
- 8002204: 697b ldr r3, [r7, #20]
- 8002206: edd3 7a08 vldr s15, [r3, #32]
- 800220a: ee66 7aa7 vmul.f32 s15, s13, s15
- 800220e: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfGravity = {.axis = {
- 8002212: edc7 7a09 vstr s15, [r7, #36] @ 0x24
- .y = Q.y * Q.z + Q.w * Q.x,
- 8002216: 697b ldr r3, [r7, #20]
- 8002218: ed93 7a08 vldr s14, [r3, #32]
- 800221c: 697b ldr r3, [r7, #20]
- 800221e: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 8002222: ee27 7a27 vmul.f32 s14, s14, s15
- 8002226: 697b ldr r3, [r7, #20]
- 8002228: edd3 6a06 vldr s13, [r3, #24]
- 800222c: 697b ldr r3, [r7, #20]
- 800222e: edd3 7a07 vldr s15, [r3, #28]
- 8002232: ee66 7aa7 vmul.f32 s15, s13, s15
- 8002236: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector halfGravity = {.axis = {
- 800223a: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .z = Q.w * Q.w - 0.5f + Q.z * Q.z,
- 800223e: 697b ldr r3, [r7, #20]
- 8002240: ed93 7a06 vldr s14, [r3, #24]
- 8002244: 697b ldr r3, [r7, #20]
- 8002246: edd3 7a06 vldr s15, [r3, #24]
- 800224a: ee67 7a27 vmul.f32 s15, s14, s15
- 800224e: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 8002252: ee37 7ac7 vsub.f32 s14, s15, s14
- 8002256: 697b ldr r3, [r7, #20]
- 8002258: edd3 6a09 vldr s13, [r3, #36] @ 0x24
- 800225c: 697b ldr r3, [r7, #20]
- 800225e: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 8002262: ee66 7aa7 vmul.f32 s15, s13, s15
- 8002266: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector halfGravity = {.axis = {
- 800226a: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- }}; // third column of transposed rotation matrix scaled by 0.5
- return halfGravity;
- 800226e: f107 033c add.w r3, r7, #60 @ 0x3c
- 8002272: f107 0224 add.w r2, r7, #36 @ 0x24
- 8002276: ca07 ldmia r2, {r0, r1, r2}
- 8002278: e883 0007 stmia.w r3, {r0, r1, r2}
- 800227c: e052 b.n 8002324
- }
- case FusionConventionNed: {
- const FusionVector halfGravity = {.axis = {
- .x = Q.w * Q.y - Q.x * Q.z,
- 800227e: 697b ldr r3, [r7, #20]
- 8002280: ed93 7a06 vldr s14, [r3, #24]
- 8002284: 697b ldr r3, [r7, #20]
- 8002286: edd3 7a08 vldr s15, [r3, #32]
- 800228a: ee27 7a27 vmul.f32 s14, s14, s15
- 800228e: 697b ldr r3, [r7, #20]
- 8002290: edd3 6a07 vldr s13, [r3, #28]
- 8002294: 697b ldr r3, [r7, #20]
- 8002296: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 800229a: ee66 7aa7 vmul.f32 s15, s13, s15
- 800229e: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfGravity = {.axis = {
- 80022a2: edc7 7a06 vstr s15, [r7, #24]
- .y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
- 80022a6: 697b ldr r3, [r7, #20]
- 80022a8: ed93 7a08 vldr s14, [r3, #32]
- 80022ac: 697b ldr r3, [r7, #20]
- 80022ae: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80022b2: ee27 7a27 vmul.f32 s14, s14, s15
- 80022b6: 697b ldr r3, [r7, #20]
- 80022b8: edd3 6a06 vldr s13, [r3, #24]
- 80022bc: 697b ldr r3, [r7, #20]
- 80022be: edd3 7a07 vldr s15, [r3, #28]
- 80022c2: ee66 7aa7 vmul.f32 s15, s13, s15
- 80022c6: ee77 7a27 vadd.f32 s15, s14, s15
- 80022ca: eef1 7a67 vneg.f32 s15, s15
- const FusionVector halfGravity = {.axis = {
- 80022ce: edc7 7a07 vstr s15, [r7, #28]
- .z = 0.5f - Q.w * Q.w - Q.z * Q.z,
- 80022d2: 697b ldr r3, [r7, #20]
- 80022d4: ed93 7a06 vldr s14, [r3, #24]
- 80022d8: 697b ldr r3, [r7, #20]
- 80022da: edd3 7a06 vldr s15, [r3, #24]
- 80022de: ee67 7a27 vmul.f32 s15, s14, s15
- 80022e2: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 80022e6: ee37 7a67 vsub.f32 s14, s14, s15
- 80022ea: 697b ldr r3, [r7, #20]
- 80022ec: edd3 6a09 vldr s13, [r3, #36] @ 0x24
- 80022f0: 697b ldr r3, [r7, #20]
- 80022f2: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80022f6: ee66 7aa7 vmul.f32 s15, s13, s15
- 80022fa: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfGravity = {.axis = {
- 80022fe: edc7 7a08 vstr s15, [r7, #32]
- }}; // third column of transposed rotation matrix scaled by -0.5
- return halfGravity;
- 8002302: f107 033c add.w r3, r7, #60 @ 0x3c
- 8002306: f107 0218 add.w r2, r7, #24
- 800230a: ca07 ldmia r2, {r0, r1, r2}
- 800230c: e883 0007 stmia.w r3, {r0, r1, r2}
- 8002310: e008 b.n 8002324
- }
- }
- return FUSION_VECTOR_ZERO; // avoid compiler warning
- 8002312: f04f 0300 mov.w r3, #0
- 8002316: 63fb str r3, [r7, #60] @ 0x3c
- 8002318: f04f 0300 mov.w r3, #0
- 800231c: 643b str r3, [r7, #64] @ 0x40
- 800231e: f04f 0300 mov.w r3, #0
- 8002322: 647b str r3, [r7, #68] @ 0x44
-#undef Q
-}
- 8002324: 6bf9 ldr r1, [r7, #60] @ 0x3c
- 8002326: 6c3a ldr r2, [r7, #64] @ 0x40
- 8002328: 6c7b ldr r3, [r7, #68] @ 0x44
- 800232a: ee06 1a90 vmov s13, r1
- 800232e: ee07 2a10 vmov s14, r2
- 8002332: ee07 3a90 vmov s15, r3
- 8002336: eeb0 0a66 vmov.f32 s0, s13
- 800233a: eef0 0a47 vmov.f32 s1, s14
- 800233e: eeb0 1a67 vmov.f32 s2, s15
- 8002342: 374c adds r7, #76 @ 0x4c
- 8002344: 46bd mov sp, r7
- 8002346: f85d 7b04 ldr.w r7, [sp], #4
- 800234a: 4770 bx lr
-
-0800234c :
-/**
- * @brief Returns the direction of the magnetic field scaled by 0.5.
- * @param ahrs AHRS algorithm structure.
- * @return Direction of the magnetic field scaled by 0.5.
- */
-static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) {
- 800234c: b480 push {r7}
- 800234e: b097 sub sp, #92 @ 0x5c
- 8002350: af00 add r7, sp, #0
- 8002352: 6178 str r0, [r7, #20]
-#define Q ahrs->quaternion.element
- switch (ahrs->settings.convention) {
- 8002354: 697b ldr r3, [r7, #20]
- 8002356: 781b ldrb r3, [r3, #0]
- 8002358: 2b02 cmp r3, #2
- 800235a: f000 809a beq.w 8002492
- 800235e: 2b02 cmp r3, #2
- 8002360: f300 80e1 bgt.w 8002526
- 8002364: 2b00 cmp r3, #0
- 8002366: d002 beq.n 800236e
- 8002368: 2b01 cmp r3, #1
- 800236a: d048 beq.n 80023fe
- 800236c: e0db b.n 8002526
- case FusionConventionNwu: {
- const FusionVector halfMagnetic = {.axis = {
- .x = Q.x * Q.y + Q.w * Q.z,
- 800236e: 697b ldr r3, [r7, #20]
- 8002370: ed93 7a07 vldr s14, [r3, #28]
- 8002374: 697b ldr r3, [r7, #20]
- 8002376: edd3 7a08 vldr s15, [r3, #32]
- 800237a: ee27 7a27 vmul.f32 s14, s14, s15
- 800237e: 697b ldr r3, [r7, #20]
- 8002380: edd3 6a06 vldr s13, [r3, #24]
- 8002384: 697b ldr r3, [r7, #20]
- 8002386: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 800238a: ee66 7aa7 vmul.f32 s15, s13, s15
- 800238e: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 8002392: edc7 7a0d vstr s15, [r7, #52] @ 0x34
- .y = Q.w * Q.w - 0.5f + Q.y * Q.y,
- 8002396: 697b ldr r3, [r7, #20]
- 8002398: ed93 7a06 vldr s14, [r3, #24]
- 800239c: 697b ldr r3, [r7, #20]
- 800239e: edd3 7a06 vldr s15, [r3, #24]
- 80023a2: ee67 7a27 vmul.f32 s15, s14, s15
- 80023a6: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 80023aa: ee37 7ac7 vsub.f32 s14, s15, s14
- 80023ae: 697b ldr r3, [r7, #20]
- 80023b0: edd3 6a08 vldr s13, [r3, #32]
- 80023b4: 697b ldr r3, [r7, #20]
- 80023b6: edd3 7a08 vldr s15, [r3, #32]
- 80023ba: ee66 7aa7 vmul.f32 s15, s13, s15
- 80023be: ee77 7a27 vadd.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 80023c2: edc7 7a0e vstr s15, [r7, #56] @ 0x38
- .z = Q.y * Q.z - Q.w * Q.x,
- 80023c6: 697b ldr r3, [r7, #20]
- 80023c8: ed93 7a08 vldr s14, [r3, #32]
- 80023cc: 697b ldr r3, [r7, #20]
- 80023ce: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80023d2: ee27 7a27 vmul.f32 s14, s14, s15
- 80023d6: 697b ldr r3, [r7, #20]
- 80023d8: edd3 6a06 vldr s13, [r3, #24]
- 80023dc: 697b ldr r3, [r7, #20]
- 80023de: edd3 7a07 vldr s15, [r3, #28]
- 80023e2: ee66 7aa7 vmul.f32 s15, s13, s15
- 80023e6: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 80023ea: edc7 7a0f vstr s15, [r7, #60] @ 0x3c
- }}; // second column of transposed rotation matrix scaled by 0.5
- return halfMagnetic;
- 80023ee: f107 034c add.w r3, r7, #76 @ 0x4c
- 80023f2: f107 0234 add.w r2, r7, #52 @ 0x34
- 80023f6: ca07 ldmia r2, {r0, r1, r2}
- 80023f8: e883 0007 stmia.w r3, {r0, r1, r2}
- 80023fc: e09c b.n 8002538
- }
- case FusionConventionEnu: {
- const FusionVector halfMagnetic = {.axis = {
- .x = 0.5f - Q.w * Q.w - Q.x * Q.x,
- 80023fe: 697b ldr r3, [r7, #20]
- 8002400: ed93 7a06 vldr s14, [r3, #24]
- 8002404: 697b ldr r3, [r7, #20]
- 8002406: edd3 7a06 vldr s15, [r3, #24]
- 800240a: ee67 7a27 vmul.f32 s15, s14, s15
- 800240e: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 8002412: ee37 7a67 vsub.f32 s14, s14, s15
- 8002416: 697b ldr r3, [r7, #20]
- 8002418: edd3 6a07 vldr s13, [r3, #28]
- 800241c: 697b ldr r3, [r7, #20]
- 800241e: edd3 7a07 vldr s15, [r3, #28]
- 8002422: ee66 7aa7 vmul.f32 s15, s13, s15
- 8002426: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 800242a: edc7 7a0a vstr s15, [r7, #40] @ 0x28
- .y = Q.w * Q.z - Q.x * Q.y,
- 800242e: 697b ldr r3, [r7, #20]
- 8002430: ed93 7a06 vldr s14, [r3, #24]
- 8002434: 697b ldr r3, [r7, #20]
- 8002436: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 800243a: ee27 7a27 vmul.f32 s14, s14, s15
- 800243e: 697b ldr r3, [r7, #20]
- 8002440: edd3 6a07 vldr s13, [r3, #28]
- 8002444: 697b ldr r3, [r7, #20]
- 8002446: edd3 7a08 vldr s15, [r3, #32]
- 800244a: ee66 7aa7 vmul.f32 s15, s13, s15
- 800244e: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 8002452: edc7 7a0b vstr s15, [r7, #44] @ 0x2c
- .z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
- 8002456: 697b ldr r3, [r7, #20]
- 8002458: ed93 7a07 vldr s14, [r3, #28]
- 800245c: 697b ldr r3, [r7, #20]
- 800245e: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 8002462: ee27 7a27 vmul.f32 s14, s14, s15
- 8002466: 697b ldr r3, [r7, #20]
- 8002468: edd3 6a06 vldr s13, [r3, #24]
- 800246c: 697b ldr r3, [r7, #20]
- 800246e: edd3 7a08 vldr s15, [r3, #32]
- 8002472: ee66 7aa7 vmul.f32 s15, s13, s15
- 8002476: ee77 7a27 vadd.f32 s15, s14, s15
- 800247a: eef1 7a67 vneg.f32 s15, s15
- const FusionVector halfMagnetic = {.axis = {
- 800247e: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- }}; // first column of transposed rotation matrix scaled by -0.5
- return halfMagnetic;
- 8002482: f107 034c add.w r3, r7, #76 @ 0x4c
- 8002486: f107 0228 add.w r2, r7, #40 @ 0x28
- 800248a: ca07 ldmia r2, {r0, r1, r2}
- 800248c: e883 0007 stmia.w r3, {r0, r1, r2}
- 8002490: e052 b.n 8002538
- }
- case FusionConventionNed: {
- const FusionVector halfMagnetic = {.axis = {
- .x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
- 8002492: 697b ldr r3, [r7, #20]
- 8002494: ed93 7a07 vldr s14, [r3, #28]
- 8002498: 697b ldr r3, [r7, #20]
- 800249a: edd3 7a08 vldr s15, [r3, #32]
- 800249e: ee27 7a27 vmul.f32 s14, s14, s15
- 80024a2: 697b ldr r3, [r7, #20]
- 80024a4: edd3 6a06 vldr s13, [r3, #24]
- 80024a8: 697b ldr r3, [r7, #20]
- 80024aa: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 80024ae: ee66 7aa7 vmul.f32 s15, s13, s15
- 80024b2: ee77 7a27 vadd.f32 s15, s14, s15
- 80024b6: eef1 7a67 vneg.f32 s15, s15
- const FusionVector halfMagnetic = {.axis = {
- 80024ba: edc7 7a07 vstr s15, [r7, #28]
- .y = 0.5f - Q.w * Q.w - Q.y * Q.y,
- 80024be: 697b ldr r3, [r7, #20]
- 80024c0: ed93 7a06 vldr s14, [r3, #24]
- 80024c4: 697b ldr r3, [r7, #20]
- 80024c6: edd3 7a06 vldr s15, [r3, #24]
- 80024ca: ee67 7a27 vmul.f32 s15, s14, s15
- 80024ce: eeb6 7a00 vmov.f32 s14, #96 @ 0x3f000000 0.5
- 80024d2: ee37 7a67 vsub.f32 s14, s14, s15
- 80024d6: 697b ldr r3, [r7, #20]
- 80024d8: edd3 6a08 vldr s13, [r3, #32]
- 80024dc: 697b ldr r3, [r7, #20]
- 80024de: edd3 7a08 vldr s15, [r3, #32]
- 80024e2: ee66 7aa7 vmul.f32 s15, s13, s15
- 80024e6: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 80024ea: edc7 7a08 vstr s15, [r7, #32]
- .z = Q.w * Q.x - Q.y * Q.z,
- 80024ee: 697b ldr r3, [r7, #20]
- 80024f0: ed93 7a06 vldr s14, [r3, #24]
- 80024f4: 697b ldr r3, [r7, #20]
- 80024f6: edd3 7a07 vldr s15, [r3, #28]
- 80024fa: ee27 7a27 vmul.f32 s14, s14, s15
- 80024fe: 697b ldr r3, [r7, #20]
- 8002500: edd3 6a08 vldr s13, [r3, #32]
- 8002504: 697b ldr r3, [r7, #20]
- 8002506: edd3 7a09 vldr s15, [r3, #36] @ 0x24
- 800250a: ee66 7aa7 vmul.f32 s15, s13, s15
- 800250e: ee77 7a67 vsub.f32 s15, s14, s15
- const FusionVector halfMagnetic = {.axis = {
- 8002512: edc7 7a09 vstr s15, [r7, #36] @ 0x24
- }}; // second column of transposed rotation matrix scaled by -0.5
- return halfMagnetic;
- 8002516: f107 034c add.w r3, r7, #76 @ 0x4c
- 800251a: f107 021c add.w r2, r7, #28
- 800251e: ca07 ldmia r2, {r0, r1, r2}
- 8002520: e883 0007 stmia.w r3, {r0, r1, r2}
- 8002524: e008 b.n 8002538
- }
- }
- return FUSION_VECTOR_ZERO; // avoid compiler warning
- 8002526: f04f 0300 mov.w r3, #0
- 800252a: 64fb str r3, [r7, #76] @ 0x4c
- 800252c: f04f 0300 mov.w r3, #0
- 8002530: 653b str r3, [r7, #80] @ 0x50
- 8002532: f04f 0300 mov.w r3, #0
- 8002536: 657b str r3, [r7, #84] @ 0x54
-#undef Q
-}
- 8002538: 6cf9 ldr r1, [r7, #76] @ 0x4c
- 800253a: 6d3a ldr r2, [r7, #80] @ 0x50
- 800253c: 6d7b ldr r3, [r7, #84] @ 0x54
- 800253e: ee06 1a90 vmov s13, r1
- 8002542: ee07 2a10 vmov s14, r2
- 8002546: ee07 3a90 vmov s15, r3
- 800254a: eeb0 0a66 vmov.f32 s0, s13
- 800254e: eef0 0a47 vmov.f32 s1, s14
- 8002552: eeb0 1a67 vmov.f32 s2, s15
- 8002556: 375c adds r7, #92 @ 0x5c
- 8002558: 46bd mov sp, r7
- 800255a: f85d 7b04 ldr.w r7, [sp], #4
- 800255e: 4770 bx lr
-
-08002560 :
- * @brief Returns the feedback.
- * @param sensor Sensor.
- * @param reference Reference.
- * @return Feedback.
- */
-static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) {
- 8002560: b580 push {r7, lr}
- 8002562: b090 sub sp, #64 @ 0x40
- 8002564: af00 add r7, sp, #0
- 8002566: eeb0 5a40 vmov.f32 s10, s0
- 800256a: eef0 5a60 vmov.f32 s11, s1
- 800256e: eeb0 6a41 vmov.f32 s12, s2
- 8002572: eef0 6a61 vmov.f32 s13, s3
- 8002576: eeb0 7a42 vmov.f32 s14, s4
- 800257a: eef0 7a62 vmov.f32 s15, s5
- 800257e: ed87 5a07 vstr s10, [r7, #28]
- 8002582: edc7 5a08 vstr s11, [r7, #32]
- 8002586: ed87 6a09 vstr s12, [r7, #36] @ 0x24
- 800258a: edc7 6a04 vstr s13, [r7, #16]
- 800258e: ed87 7a05 vstr s14, [r7, #20]
- 8002592: edc7 7a06 vstr s15, [r7, #24]
- if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
- 8002596: ed97 5a04 vldr s10, [r7, #16]
- 800259a: edd7 5a05 vldr s11, [r7, #20]
- 800259e: ed97 6a06 vldr s12, [r7, #24]
- 80025a2: edd7 6a07 vldr s13, [r7, #28]
- 80025a6: ed97 7a08 vldr s14, [r7, #32]
- 80025aa: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 80025ae: eef0 1a45 vmov.f32 s3, s10
- 80025b2: eeb0 2a65 vmov.f32 s4, s11
- 80025b6: eef0 2a46 vmov.f32 s5, s12
- 80025ba: eeb0 0a66 vmov.f32 s0, s13
- 80025be: eef0 0a47 vmov.f32 s1, s14
- 80025c2: eeb0 1a67 vmov.f32 s2, s15
- 80025c6: f7fe fea7 bl 8001318
- 80025ca: eef0 7a40 vmov.f32 s15, s0
- 80025ce: eef5 7ac0 vcmpe.f32 s15, #0.0
- 80025d2: eef1 fa10 vmrs APSR_nzcv, fpscr
- 80025d6: d540 bpl.n 800265a
- return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
- 80025d8: ed97 5a04 vldr s10, [r7, #16]
- 80025dc: edd7 5a05 vldr s11, [r7, #20]
- 80025e0: ed97 6a06 vldr s12, [r7, #24]
- 80025e4: edd7 6a07 vldr s13, [r7, #28]
- 80025e8: ed97 7a08 vldr s14, [r7, #32]
- 80025ec: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 80025f0: eef0 1a45 vmov.f32 s3, s10
- 80025f4: eeb0 2a65 vmov.f32 s4, s11
- 80025f8: eef0 2a46 vmov.f32 s5, s12
- 80025fc: eeb0 0a66 vmov.f32 s0, s13
- 8002600: eef0 0a47 vmov.f32 s1, s14
- 8002604: eeb0 1a67 vmov.f32 s2, s15
- 8002608: f7fe fe20 bl 800124c
- 800260c: eef0 6a40 vmov.f32 s13, s0
- 8002610: eeb0 7a60 vmov.f32 s14, s1
- 8002614: eef0 7a41 vmov.f32 s15, s2
- 8002618: edc7 6a0d vstr s13, [r7, #52] @ 0x34
- 800261c: ed87 7a0e vstr s14, [r7, #56] @ 0x38
- 8002620: edc7 7a0f vstr s15, [r7, #60] @ 0x3c
- 8002624: edd7 6a0d vldr s13, [r7, #52] @ 0x34
- 8002628: ed97 7a0e vldr s14, [r7, #56] @ 0x38
- 800262c: edd7 7a0f vldr s15, [r7, #60] @ 0x3c
- 8002630: eeb0 0a66 vmov.f32 s0, s13
- 8002634: eef0 0a47 vmov.f32 s1, s14
- 8002638: eeb0 1a67 vmov.f32 s2, s15
- 800263c: f7fe ff36 bl 80014ac
- 8002640: eef0 6a40 vmov.f32 s13, s0
- 8002644: eeb0 7a60 vmov.f32 s14, s1
- 8002648: eef0 7a41 vmov.f32 s15, s2
- 800264c: edc7 6a0a vstr s13, [r7, #40] @ 0x28
- 8002650: ed87 7a0b vstr s14, [r7, #44] @ 0x2c
- 8002654: edc7 7a0c vstr s15, [r7, #48] @ 0x30
- 8002658: e025 b.n 80026a6
- }
- return FusionVectorCrossProduct(sensor, reference);
- 800265a: ed97 5a04 vldr s10, [r7, #16]
- 800265e: edd7 5a05 vldr s11, [r7, #20]
- 8002662: ed97 6a06 vldr s12, [r7, #24]
- 8002666: edd7 6a07 vldr s13, [r7, #28]
- 800266a: ed97 7a08 vldr s14, [r7, #32]
- 800266e: edd7 7a09 vldr s15, [r7, #36] @ 0x24
- 8002672: eef0 1a45 vmov.f32 s3, s10
- 8002676: eeb0 2a65 vmov.f32 s4, s11
- 800267a: eef0 2a46 vmov.f32 s5, s12
- 800267e: eeb0 0a66 vmov.f32 s0, s13
- 8002682: eef0 0a47 vmov.f32 s1, s14
- 8002686: eeb0 1a67 vmov.f32 s2, s15
- 800268a: f7fe fddf bl 800124c
- 800268e: eef0 6a40 vmov.f32 s13, s0
- 8002692: eeb0 7a60 vmov.f32 s14, s1
- 8002696: eef0 7a41 vmov.f32 s15, s2
- 800269a: edc7 6a0a vstr s13, [r7, #40] @ 0x28
- 800269e: ed87 7a0b vstr s14, [r7, #44] @ 0x2c
- 80026a2: edc7 7a0c vstr s15, [r7, #48] @ 0x30
-}
- 80026a6: 6ab9 ldr r1, [r7, #40] @ 0x28
- 80026a8: 6afa ldr r2, [r7, #44] @ 0x2c
- 80026aa: 6b3b ldr r3, [r7, #48] @ 0x30
- 80026ac: ee06 1a90 vmov s13, r1
- 80026b0: ee07 2a10 vmov s14, r2
- 80026b4: ee07 3a90 vmov s15, r3
- 80026b8: eeb0 0a66 vmov.f32 s0, s13
- 80026bc: eef0 0a47 vmov.f32 s1, s14
- 80026c0: eeb0 1a67 vmov.f32 s2, s15
- 80026c4: 3740 adds r7, #64 @ 0x40
- 80026c6: 46bd mov sp, r7
- 80026c8: bd80 pop {r7, pc}
-
-080026ca :
- * @param value Value.
- * @param min Minimum value.
- * @param max Maximum value.
- * @return Value limited to maximum and minimum.
- */
-static inline int Clamp(const int value, const int min, const int max) {
- 80026ca: b480 push {r7}
- 80026cc: b085 sub sp, #20
- 80026ce: af00 add r7, sp, #0
- 80026d0: 60f8 str r0, [r7, #12]
- 80026d2: 60b9 str r1, [r7, #8]
- 80026d4: 607a str r2, [r7, #4]
- if (value < min) {
- 80026d6: 68fa ldr r2, [r7, #12]
- 80026d8: 68bb ldr r3, [r7, #8]
- 80026da: 429a cmp r2, r3
- 80026dc: da01 bge.n 80026e2
- return min;
- 80026de: 68bb ldr r3, [r7, #8]
- 80026e0: e006 b.n 80026f0
- }
- if (value > max) {
- 80026e2: 68fa ldr r2, [r7, #12]
- 80026e4: 687b ldr r3, [r7, #4]
- 80026e6: 429a cmp r2, r3
- 80026e8: dd01 ble.n 80026ee
- return max;
- 80026ea: 687b ldr r3, [r7, #4]
- 80026ec: e000 b.n 80026f0
- }
- return value;
- 80026ee: 68fb ldr r3, [r7, #12]
-}
- 80026f0: 4618 mov r0, r3
- 80026f2: 3714 adds r7, #20
- 80026f4: 46bd mov sp, r7
- 80026f6: f85d 7b04 ldr.w r7, [sp], #4
- 80026fa: 4770 bx lr
-
-080026fc :
-/**
- * @brief Returns the quaternion describing the sensor relative to the Earth.
- * @param ahrs AHRS algorithm structure.
- * @return Quaternion describing the sensor relative to the Earth.
- */
-FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) {
- 80026fc: b490 push {r4, r7}
- 80026fe: b08a sub sp, #40 @ 0x28
- 8002700: af00 add r7, sp, #0
- 8002702: 6178 str r0, [r7, #20]
- return ahrs->quaternion;
- 8002704: 697b ldr r3, [r7, #20]
- 8002706: f107 0418 add.w r4, r7, #24
- 800270a: 3318 adds r3, #24
- 800270c: cb0f ldmia r3, {r0, r1, r2, r3}
- 800270e: e884 000f stmia.w r4, {r0, r1, r2, r3}
- 8002712: 69b8 ldr r0, [r7, #24]
- 8002714: 69f9 ldr r1, [r7, #28]
- 8002716: 6a3a ldr r2, [r7, #32]
- 8002718: 6a7b ldr r3, [r7, #36] @ 0x24
- 800271a: ee06 0a10 vmov s12, r0
- 800271e: ee06 1a90 vmov s13, r1
- 8002722: ee07 2a10 vmov s14, r2
- 8002726: ee07 3a90 vmov s15, r3
-}
- 800272a: eeb0 0a46 vmov.f32 s0, s12
- 800272e: eef0 0a66 vmov.f32 s1, s13
- 8002732: eeb0 1a47 vmov.f32 s2, s14
- 8002736: eef0 1a67 vmov.f32 s3, s15
- 800273a: 3728 adds r7, #40 @ 0x28
- 800273c: 46bd mov sp, r7
- 800273e: bc90 pop {r4, r7}
- 8002740: 4770 bx lr
- ...
-
-08002744 :
-/**
- * @brief Returns the AHRS algorithm internal states.
- * @param ahrs AHRS algorithm structure.
- * @return AHRS algorithm internal states.
- */
-FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) {
- 8002744: b5b0 push {r4, r5, r7, lr}
- 8002746: b088 sub sp, #32
- 8002748: af00 add r7, sp, #0
- 800274a: 6078 str r0, [r7, #4]
- 800274c: 6039 str r1, [r7, #0]
- const FusionAhrsInternalStates internalStates = {
- .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
- 800274e: 683b ldr r3, [r7, #0]
- 8002750: edd3 6a11 vldr s13, [r3, #68] @ 0x44
- 8002754: ed93 7a12 vldr s14, [r3, #72] @ 0x48
- 8002758: edd3 7a13 vldr s15, [r3, #76] @ 0x4c
- 800275c: eeb0 0a66 vmov.f32 s0, s13
- 8002760: eef0 0a47 vmov.f32 s1, s14
- 8002764: eeb0 1a67 vmov.f32 s2, s15
- 8002768: f7fe fe76 bl 8001458
- 800276c: eef0 7a40 vmov.f32 s15, s0
- 8002770: ee77 7aa7 vadd.f32 s15, s15, s15
- 8002774: eeb0 0a67 vmov.f32 s0, s15
- 8002778: f7fe fbdc bl 8000f34
- 800277c: eef0 7a40 vmov.f32 s15, s0
- 8002780: eeb0 0a67 vmov.f32 s0, s15
- 8002784: f7fe fbc2 bl 8000f0c
- 8002788: eef0 7a40 vmov.f32 s15, s0
- const FusionAhrsInternalStates internalStates = {
- 800278c: edc7 7a02 vstr s15, [r7, #8]
- .accelerometerIgnored = ahrs->accelerometerIgnored,
- 8002790: 683b ldr r3, [r7, #0]
- 8002792: f893 305c ldrb.w r3, [r3, #92] @ 0x5c
- const FusionAhrsInternalStates internalStates = {
- 8002796: 733b strb r3, [r7, #12]
- .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->accelerationRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod,
- 8002798: 683b ldr r3, [r7, #0]
- 800279a: 695b ldr r3, [r3, #20]
- 800279c: 2b00 cmp r3, #0
- 800279e: d00e beq.n 80027be
- 80027a0: 683b ldr r3, [r7, #0]
- 80027a2: 6e1b ldr r3, [r3, #96] @ 0x60
- 80027a4: ee07 3a90 vmov s15, r3
- 80027a8: eef8 6ae7 vcvt.f32.s32 s13, s15
- 80027ac: 683b ldr r3, [r7, #0]
- 80027ae: 695b ldr r3, [r3, #20]
- 80027b0: ee07 3a90 vmov s15, r3
- 80027b4: eeb8 7a67 vcvt.f32.u32 s14, s15
- 80027b8: eec6 7a87 vdiv.f32 s15, s13, s14
- 80027bc: e001 b.n 80027c2
- 80027be: eddf 7a27 vldr s15, [pc, #156] @ 800285c
- const FusionAhrsInternalStates internalStates = {
- 80027c2: edc7 7a04 vstr s15, [r7, #16]
- .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
- 80027c6: 683b ldr r3, [r7, #0]
- 80027c8: edd3 6a14 vldr s13, [r3, #80] @ 0x50
- 80027cc: ed93 7a15 vldr s14, [r3, #84] @ 0x54
- 80027d0: edd3 7a16 vldr s15, [r3, #88] @ 0x58
- 80027d4: eeb0 0a66 vmov.f32 s0, s13
- 80027d8: eef0 0a47 vmov.f32 s1, s14
- 80027dc: eeb0 1a67 vmov.f32 s2, s15
- 80027e0: f7fe fe3a bl 8001458
- 80027e4: eef0 7a40 vmov.f32 s15, s0
- 80027e8: ee77 7aa7 vadd.f32 s15, s15, s15
- 80027ec: eeb0 0a67 vmov.f32 s0, s15
- 80027f0: f7fe fba0 bl 8000f34
- 80027f4: eef0 7a40 vmov.f32 s15, s0
- 80027f8: eeb0 0a67 vmov.f32 s0, s15
- 80027fc: f7fe fb86 bl 8000f0c
- 8002800: eef0 7a40 vmov.f32 s15, s0
- const FusionAhrsInternalStates internalStates = {
- 8002804: edc7 7a05 vstr s15, [r7, #20]
- .magnetometerIgnored = ahrs->magnetometerIgnored,
- 8002808: 683b ldr r3, [r7, #0]
- 800280a: f893 3068 ldrb.w r3, [r3, #104] @ 0x68
- const FusionAhrsInternalStates internalStates = {
- 800280e: 763b strb r3, [r7, #24]
- .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->magneticRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod,
- 8002810: 683b ldr r3, [r7, #0]
- 8002812: 695b ldr r3, [r3, #20]
- 8002814: 2b00 cmp r3, #0
- 8002816: d00e beq.n 8002836
- 8002818: 683b ldr r3, [r7, #0]
- 800281a: 6edb ldr r3, [r3, #108] @ 0x6c
- 800281c: ee07 3a90 vmov s15, r3
- 8002820: eef8 6ae7 vcvt.f32.s32 s13, s15
- 8002824: 683b ldr r3, [r7, #0]
- 8002826: 695b ldr r3, [r3, #20]
- 8002828: ee07 3a90 vmov s15, r3
- 800282c: eeb8 7a67 vcvt.f32.u32 s14, s15
- 8002830: eec6 7a87 vdiv.f32 s15, s13, s14
- 8002834: e001 b.n 800283a
- 8002836: eddf 7a09 vldr s15, [pc, #36] @ 800285c
- const FusionAhrsInternalStates internalStates = {
- 800283a: edc7 7a07 vstr s15, [r7, #28]
- };
- return internalStates;
- 800283e: 687b ldr r3, [r7, #4]
- 8002840: 461d mov r5, r3
- 8002842: f107 0408 add.w r4, r7, #8
- 8002846: cc0f ldmia r4!, {r0, r1, r2, r3}
- 8002848: c50f stmia r5!, {r0, r1, r2, r3}
- 800284a: e894 0003 ldmia.w r4, {r0, r1}
- 800284e: e885 0003 stmia.w r5, {r0, r1}
-}
- 8002852: 6878 ldr r0, [r7, #4]
- 8002854: 3720 adds r7, #32
- 8002856: 46bd mov sp, r7
- 8002858: bdb0 pop {r4, r5, r7, pc}
- 800285a: bf00 nop
- 800285c: 00000000 .word 0x00000000
-
-08002860 :
-/**
- * @brief Returns the AHRS algorithm flags.
- * @param ahrs AHRS algorithm structure.
- * @return AHRS algorithm flags.
- */
-FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) {
- 8002860: b480 push {r7}
- 8002862: b085 sub sp, #20
- 8002864: af00 add r7, sp, #0
- 8002866: 6078 str r0, [r7, #4]
- const FusionAhrsFlags flags = {
- .initialising = ahrs->initialising,
- 8002868: 687b ldr r3, [r7, #4]
- 800286a: f893 3034 ldrb.w r3, [r3, #52] @ 0x34
- const FusionAhrsFlags flags = {
- 800286e: 723b strb r3, [r7, #8]
- .angularRateRecovery = ahrs->angularRateRecovery,
- 8002870: 687b ldr r3, [r7, #4]
- 8002872: f893 3040 ldrb.w r3, [r3, #64] @ 0x40
- const FusionAhrsFlags flags = {
- 8002876: 727b strb r3, [r7, #9]
- .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
- 8002878: 687b ldr r3, [r7, #4]
- 800287a: 6e1a ldr r2, [r3, #96] @ 0x60
- 800287c: 687b ldr r3, [r7, #4]
- 800287e: 6e5b ldr r3, [r3, #100] @ 0x64
- 8002880: 429a cmp r2, r3
- 8002882: bfcc ite gt
- 8002884: 2301 movgt r3, #1
- 8002886: 2300 movle r3, #0
- 8002888: b2db uxtb r3, r3
- const FusionAhrsFlags flags = {
- 800288a: 72bb strb r3, [r7, #10]
- .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
- 800288c: 687b ldr r3, [r7, #4]
- 800288e: 6eda ldr r2, [r3, #108] @ 0x6c
- 8002890: 687b ldr r3, [r7, #4]
- 8002892: 6f1b ldr r3, [r3, #112] @ 0x70
- 8002894: 429a cmp r2, r3
- 8002896: bfcc ite gt
- 8002898: 2301 movgt r3, #1
- 800289a: 2300 movle r3, #0
- 800289c: b2db uxtb r3, r3
- const FusionAhrsFlags flags = {
- 800289e: 72fb strb r3, [r7, #11]
- };
- return flags;
- 80028a0: 68bb ldr r3, [r7, #8]
- 80028a2: 60fb str r3, [r7, #12]
- 80028a4: 2300 movs r3, #0
- 80028a6: 7b3a ldrb r2, [r7, #12]
- 80028a8: f362 0307 bfi r3, r2, #0, #8
- 80028ac: 7b7a ldrb r2, [r7, #13]
- 80028ae: f362 230f bfi r3, r2, #8, #8
- 80028b2: 7bba ldrb r2, [r7, #14]
- 80028b4: f362 4317 bfi r3, r2, #16, #8
- 80028b8: 7bfa ldrb r2, [r7, #15]
- 80028ba: f362 631f bfi r3, r2, #24, #8
-}
- 80028be: 4618 mov r0, r3
- 80028c0: 3714 adds r7, #20
- 80028c2: 46bd mov sp, r7
- 80028c4: f85d 7b04 ldr.w r7, [sp], #4
- 80028c8: 4770 bx lr
- ...
-
-080028cc :
- uint8_t val;
- HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
- return val;
-}
-
-void write_register(uint8_t reg, uint8_t val) {
- 80028cc: b580 push {r7, lr}
- 80028ce: b086 sub sp, #24
- 80028d0: af04 add r7, sp, #16
- 80028d2: 4603 mov r3, r0
- 80028d4: 460a mov r2, r1
- 80028d6: 71fb strb r3, [r7, #7]
- 80028d8: 4613 mov r3, r2
- 80028da: 71bb strb r3, [r7, #6]
- HAL_I2C_Mem_Write(&hi2c1, ICM20948_ADDR, reg, 1, &val, 1, HAL_MAX_DELAY);
- 80028dc: 79fb ldrb r3, [r7, #7]
- 80028de: b29a uxth r2, r3
- 80028e0: f04f 33ff mov.w r3, #4294967295
- 80028e4: 9302 str r3, [sp, #8]
- 80028e6: 2301 movs r3, #1
- 80028e8: 9301 str r3, [sp, #4]
- 80028ea: 1dbb adds r3, r7, #6
- 80028ec: 9300 str r3, [sp, #0]
- 80028ee: 2301 movs r3, #1
- 80028f0: 21d2 movs r1, #210 @ 0xd2
- 80028f2: 4803 ldr r0, [pc, #12] @ (8002900 )
- 80028f4: f002 f86e bl 80049d4
-}
- 80028f8: bf00 nop
- 80028fa: 3708 adds r7, #8
- 80028fc: 46bd mov sp, r7
- 80028fe: bd80 pop {r7, pc}
- 8002900: 200001f0 .word 0x200001f0
-
-08002904 :
-
-void icm20948_select_bank(uint8_t bank) {
- 8002904: b580 push {r7, lr}
- 8002906: b082 sub sp, #8
- 8002908: af00 add r7, sp, #0
- 800290a: 4603 mov r3, r0
- 800290c: 71fb strb r3, [r7, #7]
- write_register(0x7F, bank << 4); // REG_BANK_SEL (0x7F)
- 800290e: 79fb ldrb r3, [r7, #7]
- 8002910: 011b lsls r3, r3, #4
- 8002912: b2db uxtb r3, r3
- 8002914: 4619 mov r1, r3
- 8002916: 207f movs r0, #127 @ 0x7f
- 8002918: f7ff ffd8 bl 80028cc
-}
- 800291c: bf00 nop
- 800291e: 3708 adds r7, #8
- 8002920: 46bd mov sp, r7
- 8002922: bd80 pop {r7, pc}
-
-08002924 :
-
-void icm20948_init(void) {
- 8002924: b580 push {r7, lr}
- 8002926: af00 add r7, sp, #0
- icm20948_select_bank(0); // BANK 0
- 8002928: 2000 movs r0, #0
- 800292a: f7ff ffeb bl 8002904
- write_register(0x06, 0x01); // PWR_MGMT_1: reset
- 800292e: 2101 movs r1, #1
- 8002930: 2006 movs r0, #6
- 8002932: f7ff ffcb bl 80028cc
+ // Format pixel RGB565 16-bit
+ GC9A01_WriteCommand(GC9A01_COLMOD);
+ 8000d7c: 203a movs r0, #58 @ 0x3a
+ 8000d7e: f7ff ff4b bl 8000c18
+ GC9A01_WriteData(0x55); // 16 bits/pixel
+ 8000d82: 2055 movs r0, #85 @ 0x55
+ 8000d84: f7ff ff62 bl 8000c4c
HAL_Delay(10);
- 8002936: 200a movs r0, #10
- 8002938: f001 fbfc bl 8004134
- write_register(0x06, 0x01); // Clock source auto
- 800293c: 2101 movs r1, #1
- 800293e: 2006 movs r0, #6
- 8002940: f7ff ffc4 bl 80028cc
+ 8000d88: 200a movs r0, #10
+ 8000d8a: f000 fba7 bl 80014dc
- icm20948_select_bank(2); // BANK 2
- 8002944: 2002 movs r0, #2
- 8002946: f7ff ffdd bl 8002904
- // write_register(0x14, 0x00); // ACCEL_CONFIG: ±2g
- write_register(0x14, 0x04); // ACCEL_CONFIG : ±8g (bits 2:1 = 10)
- 800294a: 2104 movs r1, #4
- 800294c: 2014 movs r0, #20
- 800294e: f7ff ffbd bl 80028cc
+ // Memory Access Control : rotation / orientation
+ GC9A01_WriteCommand(GC9A01_MADCTL);
+ 8000d8e: 2036 movs r0, #54 @ 0x36
+ 8000d90: f7ff ff42 bl 8000c18
+ GC9A01_WriteData(0x00); // rotation normale, ajuste si besoin
+ 8000d94: 2000 movs r0, #0
+ 8000d96: f7ff ff59 bl 8000c4c
- icm20948_select_bank(0); // Revenir en BANK 0 pour lecture
- 8002952: 2000 movs r0, #0
- 8002954: f7ff ffd6 bl 8002904
-}
- 8002958: bf00 nop
- 800295a: bd80 pop {r7, pc}
+ // Activation de l'écran
+ GC9A01_WriteCommand(GC9A01_INVON); // inversion couleurs (souvent requis)
+ 8000d9a: 2021 movs r0, #33 @ 0x21
+ 8000d9c: f7ff ff3c bl 8000c18
+ HAL_Delay(10);
+ 8000da0: 200a movs r0, #10
+ 8000da2: f000 fb9b bl 80014dc
-0800295c :
+ GC9A01_WriteCommand(GC9A01_NORON); // mode normal on
+ 8000da6: 2013 movs r0, #19
+ 8000da8: f7ff ff36 bl 8000c18
+ HAL_Delay(10);
+ 8000dac: 200a movs r0, #10
+ 8000dae: f000 fb95 bl 80014dc
-void icm20948_read_accel(float *ax, float *ay, float *az) {
- 800295c: b580 push {r7, lr}
- 800295e: b08c sub sp, #48 @ 0x30
- 8002960: af04 add r7, sp, #16
- 8002962: 60f8 str r0, [r7, #12]
- 8002964: 60b9 str r1, [r7, #8]
- 8002966: 607a str r2, [r7, #4]
- uint8_t data[6];
- HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x2D, 1, data, 6, HAL_MAX_DELAY);
- 8002968: f04f 33ff mov.w r3, #4294967295
- 800296c: 9302 str r3, [sp, #8]
- 800296e: 2306 movs r3, #6
- 8002970: 9301 str r3, [sp, #4]
- 8002972: f107 0314 add.w r3, r7, #20
- 8002976: 9300 str r3, [sp, #0]
- 8002978: 2301 movs r3, #1
- 800297a: 222d movs r2, #45 @ 0x2d
- 800297c: 21d2 movs r1, #210 @ 0xd2
- 800297e: 4823 ldr r0, [pc, #140] @ (8002a0c )
- 8002980: f002 f93c bl 8004bfc
- int16_t x = (data[0] << 8) | data[1];
- 8002984: 7d3b ldrb r3, [r7, #20]
- 8002986: b21b sxth r3, r3
- 8002988: 021b lsls r3, r3, #8
- 800298a: b21a sxth r2, r3
- 800298c: 7d7b ldrb r3, [r7, #21]
- 800298e: b21b sxth r3, r3
- 8002990: 4313 orrs r3, r2
- 8002992: 83fb strh r3, [r7, #30]
- int16_t y = (data[2] << 8) | data[3];
- 8002994: 7dbb ldrb r3, [r7, #22]
- 8002996: b21b sxth r3, r3
- 8002998: 021b lsls r3, r3, #8
- 800299a: b21a sxth r2, r3
- 800299c: 7dfb ldrb r3, [r7, #23]
- 800299e: b21b sxth r3, r3
- 80029a0: 4313 orrs r3, r2
- 80029a2: 83bb strh r3, [r7, #28]
- int16_t z = (data[4] << 8) | data[5];
- 80029a4: 7e3b ldrb r3, [r7, #24]
- 80029a6: b21b sxth r3, r3
- 80029a8: 021b lsls r3, r3, #8
- 80029aa: b21a sxth r2, r3
- 80029ac: 7e7b ldrb r3, [r7, #25]
- 80029ae: b21b sxth r3, r3
- 80029b0: 4313 orrs r3, r2
- 80029b2: 837b strh r3, [r7, #26]
- //*ax = (float)x / 16384.0f;
- //*ay = (float)y / 16384.0f;
- //*az = (float)z / 16384.0f;
- *ax = (float)x / 4096.0f;
- 80029b4: f9b7 301e ldrsh.w r3, [r7, #30]
- 80029b8: ee07 3a90 vmov s15, r3
- 80029bc: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 80029c0: eddf 6a13 vldr s13, [pc, #76] @ 8002a10
- 80029c4: eec7 7a26 vdiv.f32 s15, s14, s13
- 80029c8: 68fb ldr r3, [r7, #12]
- 80029ca: edc3 7a00 vstr s15, [r3]
- *ay = (float)y / 4096.0f;
- 80029ce: f9b7 301c ldrsh.w r3, [r7, #28]
- 80029d2: ee07 3a90 vmov s15, r3
- 80029d6: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 80029da: eddf 6a0d vldr s13, [pc, #52] @ 8002a10
- 80029de: eec7 7a26 vdiv.f32 s15, s14, s13
- 80029e2: 68bb ldr r3, [r7, #8]
- 80029e4: edc3 7a00 vstr s15, [r3]
- *az = (float)z / 4096.0f;
- 80029e8: f9b7 301a ldrsh.w r3, [r7, #26]
- 80029ec: ee07 3a90 vmov s15, r3
- 80029f0: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 80029f4: eddf 6a06 vldr s13, [pc, #24] @ 8002a10
- 80029f8: eec7 7a26 vdiv.f32 s15, s14, s13
- 80029fc: 687b ldr r3, [r7, #4]
- 80029fe: edc3 7a00 vstr s15, [r3]
+ GC9A01_WriteCommand(GC9A01_DISPON); // allume l'écran
+ 8000db2: 2029 movs r0, #41 @ 0x29
+ 8000db4: f7ff ff30 bl 8000c18
+ HAL_Delay(100);
+ 8000db8: 2064 movs r0, #100 @ 0x64
+ 8000dba: f000 fb8f bl 80014dc
}
- 8002a02: bf00 nop
- 8002a04: 3720 adds r7, #32
- 8002a06: 46bd mov sp, r7
- 8002a08: bd80 pop {r7, pc}
- 8002a0a: bf00 nop
- 8002a0c: 200001f0 .word 0x200001f0
- 8002a10: 45800000 .word 0x45800000
+ 8000dbe: bf00 nop
+ 8000dc0: 3708 adds r7, #8
+ 8000dc2: 46bd mov sp, r7
+ 8000dc4: bd80 pop {r7, pc}
+ 8000dc6: bf00 nop
+ 8000dc8: 200001f0 .word 0x200001f0
-08002a14 :
+08000dcc :
-void icm20948_read_gyro(float *gx, float *gy, float *gz) {
- 8002a14: b580 push {r7, lr}
- 8002a16: b08c sub sp, #48 @ 0x30
- 8002a18: af04 add r7, sp, #16
- 8002a1a: 60f8 str r0, [r7, #12]
- 8002a1c: 60b9 str r1, [r7, #8]
- 8002a1e: 607a str r2, [r7, #4]
- uint8_t data[6];
- icm20948_select_bank(0);
- 8002a20: 2000 movs r0, #0
- 8002a22: f7ff ff6f bl 8002904
- HAL_I2C_Mem_Read(&hi2c1, ICM20948_ADDR, 0x33, 1, data, 6, HAL_MAX_DELAY);
- 8002a26: f04f 33ff mov.w r3, #4294967295
- 8002a2a: 9302 str r3, [sp, #8]
- 8002a2c: 2306 movs r3, #6
- 8002a2e: 9301 str r3, [sp, #4]
- 8002a30: f107 0314 add.w r3, r7, #20
- 8002a34: 9300 str r3, [sp, #0]
- 8002a36: 2301 movs r3, #1
- 8002a38: 2233 movs r2, #51 @ 0x33
- 8002a3a: 21d2 movs r1, #210 @ 0xd2
- 8002a3c: 4822 ldr r0, [pc, #136] @ (8002ac8 )
- 8002a3e: f002 f8dd bl 8004bfc
- int16_t x = (data[0] << 8) | data[1];
- 8002a42: 7d3b ldrb r3, [r7, #20]
- 8002a44: b21b sxth r3, r3
- 8002a46: 021b lsls r3, r3, #8
- 8002a48: b21a sxth r2, r3
- 8002a4a: 7d7b ldrb r3, [r7, #21]
- 8002a4c: b21b sxth r3, r3
- 8002a4e: 4313 orrs r3, r2
- 8002a50: 83fb strh r3, [r7, #30]
- int16_t y = (data[2] << 8) | data[3];
- 8002a52: 7dbb ldrb r3, [r7, #22]
- 8002a54: b21b sxth r3, r3
- 8002a56: 021b lsls r3, r3, #8
- 8002a58: b21a sxth r2, r3
- 8002a5a: 7dfb ldrb r3, [r7, #23]
- 8002a5c: b21b sxth r3, r3
- 8002a5e: 4313 orrs r3, r2
- 8002a60: 83bb strh r3, [r7, #28]
- int16_t z = (data[4] << 8) | data[5];
- 8002a62: 7e3b ldrb r3, [r7, #24]
- 8002a64: b21b sxth r3, r3
- 8002a66: 021b lsls r3, r3, #8
- 8002a68: b21a sxth r2, r3
- 8002a6a: 7e7b ldrb r3, [r7, #25]
- 8002a6c: b21b sxth r3, r3
- 8002a6e: 4313 orrs r3, r2
- 8002a70: 837b strh r3, [r7, #26]
- *gx = (float)x / 131.0f; // sensibilité typique ±250 dps
- 8002a72: f9b7 301e ldrsh.w r3, [r7, #30]
- 8002a76: ee07 3a90 vmov s15, r3
- 8002a7a: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 8002a7e: eddf 6a13 vldr s13, [pc, #76] @ 8002acc
- 8002a82: eec7 7a26 vdiv.f32 s15, s14, s13
- 8002a86: 68fb ldr r3, [r7, #12]
- 8002a88: edc3 7a00 vstr s15, [r3]
- *gy = (float)y / 131.0f;
- 8002a8c: f9b7 301c ldrsh.w r3, [r7, #28]
- 8002a90: ee07 3a90 vmov s15, r3
- 8002a94: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 8002a98: eddf 6a0c vldr s13, [pc, #48] @ 8002acc
- 8002a9c: eec7 7a26 vdiv.f32 s15, s14, s13
- 8002aa0: 68bb ldr r3, [r7, #8]
- 8002aa2: edc3 7a00 vstr s15, [r3]
- *gz = (float)z / 131.0f;
- 8002aa6: f9b7 301a ldrsh.w r3, [r7, #26]
- 8002aaa: ee07 3a90 vmov s15, r3
- 8002aae: eeb8 7ae7 vcvt.f32.s32 s14, s15
- 8002ab2: eddf 6a06 vldr s13, [pc, #24] @ 8002acc
- 8002ab6: eec7 7a26 vdiv.f32 s15, s14, s13
- 8002aba: 687b ldr r3, [r7, #4]
- 8002abc: edc3 7a00 vstr s15, [r3]
+
+void GC9A01_Reset(void) {
+ 8000dcc: b580 push {r7, lr}
+ 8000dce: af00 add r7, sp, #0
+ HAL_GPIO_WritePin(GC9A01_RST_GPIO_Port, GC9A01_RST_Pin, GPIO_PIN_RESET);
+ 8000dd0: 2200 movs r2, #0
+ 8000dd2: 2104 movs r1, #4
+ 8000dd4: 4807 ldr r0, [pc, #28] @ (8000df4 )
+ 8000dd6: f000 fe05 bl 80019e4