From 345b69493106e2ca8d8d95d1520af5364202e3c4 Mon Sep 17 00:00:00 2001 From: Max <45385195+MasterPlayer@users.noreply.github.com> Date: Mon, 3 Oct 2022 02:19:57 +0300 Subject: [PATCH] fix issue #54 Add output rules for printing text to display: --- src_sw/axi_adxl.c | 120 ++++++++++++++++++++++++++++++++++++++++++ src_sw/axi_adxl.h | 35 +++++++++--- src_sw/axi_adxl_api.c | 16 +++--- src_sw/axi_adxl_cfg.c | 2 + src_sw/axi_adxl_dev.c | 13 +++-- src_sw/selector.c | 77 +++++++++++++++++++++++++-- src_sw/selector.h | 4 ++ 7 files changed, 248 insertions(+), 19 deletions(-) diff --git a/src_sw/axi_adxl.c b/src_sw/axi_adxl.c index c12418d..43c97a3 100644 --- a/src_sw/axi_adxl.c +++ b/src_sw/axi_adxl.c @@ -1,6 +1,126 @@ #include "axi_adxl.h" +#include +// +//enum output_rule_enum{ +// XYZ_INTEGER, +// XYZ_GRAVITY, +// ROLL_PITCH +//}; +int axi_adxl_set_output_rule(axi_adxl *ptr, enum output_rule_enum output_rule){ + if (!axi_adxl_has_init(ptr)){ +#ifdef AXI_ADXL_LOGGING_SW + textcolor(DEFAULT, RED, STD); + printf("\t\t[ADXL_SET_OUTPUT_RULE] : unitialized software structure"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); +#endif + return ADXL_UNINIT; + } + if ((output_rule != XYZ_INTEGER) && (output_rule != XYZ_GRAVITY) && (output_rule != ROLL_PITCH)){ +#ifdef AXI_ADXL_LOGGING_SW + textcolor(DEFAULT, RED, STD); + printf("\t\t[ADXL_SET_OUTPUT_RULE] : uncorrect selection : %d", output_rule); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); +#endif + return ADXL_UNCORRECT_VALUE; + } + + ptr->output_rule = output_rule; + + return ADXL_OK; +} + + + +int axi_adxl_get_output_rule(axi_adxl *ptr, int *output_rule){ + + if (!axi_adxl_has_init(ptr)){ +#ifdef AXI_ADXL_LOGGING_SW + textcolor(DEFAULT, RED, STD); + printf("\t\t[ADXL_GET_OUTPUT_RULE] : unitialized software structure"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); +#endif + return ADXL_UNINIT; + } + + *output_rule = ptr->output_rule; + + return ADXL_OK; +} + + + +int axi_adxl_is_output_rule(axi_adxl *ptr, enum output_rule_enum output_rule){ + return ((ptr->output_rule == output_rule) ? TRUE : FALSE); +} + + +int axi_adxl_print(axi_adxl *ptr){ + int output_rule; + adxl_data data; + adxl_data_float data_float; + adxl_data_pitch_roll pitch_roll; + + int status = axi_adxl_get_output_rule(ptr, &output_rule); + + + switch(output_rule){ + case XYZ_INTEGER: + status = axi_adxl_get_data(ptr, &data); + if (status != ADXL_OK){ + return status; + } + printf("X : %5d \tY : %5d \tZ : %d\r\n", data.x, data.y, data.z); + break; + + case XYZ_GRAVITY: + status = axi_adxl_get_data_float(ptr, &data_float); + if (status != ADXL_OK){ + return status; + } + printf("X : %4.6f \tY : %4.6f \tZ : %4.6f\r\n", data_float.x, data_float.y, data_float.z); + break; + + case ROLL_PITCH : + status = axi_adxl_get_pitch_roll(ptr, &pitch_roll); + if (status != ADXL_OK){ + return status; + } + printf("\t%3.2f \t%3.2f\r\n", pitch_roll.pitch, pitch_roll.roll); + break; + + default : + return ADXL_UNCORRECT_VALUE; + } + + + return status; +} + + + +int axi_adxl_get_pitch_roll(axi_adxl *ptr, adxl_data_pitch_roll *pitch_roll_ptr){ + + if (!axi_adxl_has_init(ptr)){ +#ifdef AXI_ADXL_LOGGING_SW + textcolor(DEFAULT, RED, STD); + printf("\t\t[ADXL_GET_PITCH_ROLL] : unitialized software structure"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); +#endif + return ADXL_UNINIT; + } + + adxl_data_float gravity; + int status = axi_adxl_get_data_float(ptr, &gravity); + pitch_roll_ptr->roll = atan(gravity.y/ sqrt(pow(gravity.x, 2) + pow(gravity.z, 2))) * 180 / PI; + pitch_roll_ptr->pitch = atan(-1 * gravity.x / sqrt(pow(gravity.y, 2) + pow(gravity.z, 2))) * 180 / PI; + return status; +} diff --git a/src_sw/axi_adxl.h b/src_sw/axi_adxl.h index 2602799..2c8e7a7 100644 --- a/src_sw/axi_adxl.h +++ b/src_sw/axi_adxl.h @@ -5,9 +5,11 @@ #include #include "text_color.h" +#define PI 3.14159265358979323846 -//#define AXI_ADXL_LOGGING_CFG -//#define AXI_ADXL_LOGGING_DEV +//#define AXI_ADXL_LOGGING_CFG 0 +//#define AXI_ADXL_LOGGING_DEV 0 +#define AXI_ADXL_LOGGING_SW 1 #define ADXL_OK 0 #define ADXL_UNINIT -1 @@ -36,7 +38,7 @@ static int rw_address_const[ADXL_DEV_RW_COUNT] = {29, 30, 31, 32, 33, 34, 35, 36 static int ro_address_const[ADXL_DEV_RO_COUNT] = {0, 43, 48, 50, 51, 52, 53, 54, 55, 57}; static int reserved_address_const[ADXL_DEV_RESERVED_COUNT] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,58,59,60,61,62,63}; -#define FUNCTIONS_COUNT 89 +#define FUNCTIONS_COUNT 91 static int function_index_list[FUNCTIONS_COUNT] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, @@ -48,7 +50,8 @@ static int function_index_list[FUNCTIONS_COUNT] = { 451, 452, 453, 454, 455, 456, 457, 458, 459, 461, 460, 470, 471, 480, 490, 491, 492, 493, 494, 495, 496, 497, 498, 500, 501, 560, 561, - 562, 563, 564, 565, 570, 571, 100, 120 + 562, 563, 564, 565, 570, 571, 100, 120, 121, + 122 }; typedef struct { @@ -70,12 +73,25 @@ typedef struct { float z; }adxl_data_float; + +typedef struct { + float roll; + float pitch; +}adxl_data_pitch_roll; + +enum output_rule_enum{ + XYZ_INTEGER, + XYZ_GRAVITY, + ROLL_PITCH +}; + typedef struct { adxl_cfg *cfg; adxl_dev *dev; int init_flaq; adxl_offset offset; adxl_data data; + int output_rule; } axi_adxl; @@ -220,8 +236,7 @@ enum spi_enum { #define axi_adxl_get_datay(ptr) (int16_t)(((uint16_t)adxl_dev_get_datay1((ptr)->dev)<<8) + ((uint16_t)adxl_dev_get_datay0((ptr)->dev))) #define axi_adxl_get_dataz(ptr) (int16_t)(((uint16_t)adxl_dev_get_dataz1((ptr)->dev)<<8) + ((uint16_t)adxl_dev_get_dataz0((ptr)->dev))) - -void axi_adxl_dev_debug_register_space(adxl_dev *ptr); +int axi_adxl_dev_debug_register_space(axi_adxl *ptr); int axi_adxl_init(axi_adxl *ptr, uint32_t baseaddr_cfg, uint32_t baseaddr_dev, uint8_t iic_address); @@ -419,4 +434,12 @@ int axi_adxl_has_fifo_sts_trigger(axi_adxl *ptr); int axi_adxl_get_data(axi_adxl *ptr, adxl_data *data); int axi_adxl_get_data_float(axi_adxl *ptr, adxl_data_float *data_float); +int axi_adxl_get_pitch_roll(axi_adxl *ptr, adxl_data_pitch_roll *data_pitch_roll); + + +int axi_adxl_set_output_rule(axi_adxl *ptr, enum output_rule_enum output_rule); +int axi_adxl_get_output_rule(axi_adxl *ptr, int *output_rule); +int axi_adxl_is_output_rule(axi_adxl *ptr, enum output_rule_enum output_rule); +int axi_adxl_print(axi_adxl *ptr); + diff --git a/src_sw/axi_adxl_api.c b/src_sw/axi_adxl_api.c index 4f5165d..63cced5 100644 --- a/src_sw/axi_adxl_api.c +++ b/src_sw/axi_adxl_api.c @@ -113,7 +113,9 @@ const char* function_list[] = { "GET_FIFO_ENTRIES", // 570 "GET_FIFO_TRIGGER", // 571 "DUMP_DEVICE_REGISTER_SPACE", // 100 - "DEBUG_MODE" // 120 + "DEBUG_MODE", // 120 + "SET_OUTPUT_RULES", // 121 + "GET_OUTPUT_RULES" //122 }; @@ -280,6 +282,8 @@ void print_menu(){ printf("\r\n"); printf("\t100. Dump device register space\r\n"); printf("\t120. Debug mode\r\n"); + printf("\t121. Set output rule\r\n"); + printf("\t122. Get output rule\r\n"); @@ -470,7 +474,10 @@ int menu(axi_adxl *ptr, int mode){ case 571 : status = selector_axi_adxl_has_fifo_sts_trigger(ptr); break; case 100 : status = selector_axi_adxl_dev_debug_register_space(ptr); break; + case 120 : status = dbg_set_reg(ptr); break; + case 121 : status = selector_axi_adxl_set_output_rules(ptr); break; + case 122 : status = selector_axi_adxl_get_output_rules(ptr); break; default : printf("[MENU] : incorrect selection : 0x%02x\r\n", mode); @@ -529,10 +536,7 @@ void adxl_intr_handler(void *callback){ printf("[IRQ] : bad returning status : %d", status); } - adxl_data_float data; - axi_adxl_get_data_float(ptr, &data); - - if ((axi_adxl_is_int_source(ptr, DATA_READY)) && (axi_adxl_is_int_enable(ptr, DATA_READY))){ + if ((axi_adxl_is_int_source(ptr, DATA_READY)) && (axi_adxl_is_int_enable(ptr, DATA_READY))){ printf("[DR] "); } @@ -564,7 +568,7 @@ void adxl_intr_handler(void *callback){ printf("[OV] "); } - printf("X : %4.6f \tY : %4.6f \tZ : %4.6f\r\n", data.x, data.y, data.z); + axi_adxl_print(ptr); axi_adxl_irq_ack(ptr); diff --git a/src_sw/axi_adxl_cfg.c b/src_sw/axi_adxl_cfg.c index 0dfce00..c2677b5 100644 --- a/src_sw/axi_adxl_cfg.c +++ b/src_sw/axi_adxl_cfg.c @@ -21,6 +21,8 @@ int axi_adxl_init(axi_adxl* ptr, uint32_t baseaddr_cfg, uint32_t baseaddr_dev, u printf("\t\t\tconfiguration address space : 0x%08x\r\n", baseaddr_dev); #endif + ptr->output_rule = 0; + int timer = TIMER_LIMIT; diff --git a/src_sw/axi_adxl_dev.c b/src_sw/axi_adxl_dev.c index b9b30c8..ef239be 100644 --- a/src_sw/axi_adxl_dev.c +++ b/src_sw/axi_adxl_dev.c @@ -2,10 +2,13 @@ - -void axi_adxl_dev_debug_register_space(adxl_dev* ptr) { +int axi_adxl_dev_debug_register_space(axi_adxl* ptr) { int byte_cnt = 0; + if (!axi_adxl_has_init(ptr)){ + return ADXL_UNINIT; + } + printf("\t|| [0] \t| [1] \t| [2] \t| [3] \t|\r\n"); printf("========================================\r\n"); @@ -34,7 +37,7 @@ void axi_adxl_dev_debug_register_space(adxl_dev* ptr) { } - printf(" 0x%02x\t", *((uint8_t*)ptr + i)); + printf(" 0x%02x\t", *((uint8_t*)(ptr->dev) + i)); textcolor(DEFAULT, STD, STD); @@ -49,6 +52,8 @@ void axi_adxl_dev_debug_register_space(adxl_dev* ptr) { } } + + return ADXL_OK; } /// @@ -3859,7 +3864,7 @@ int axi_adxl_get_data_float(axi_adxl* ptr, adxl_data_float* data_float) { int16_t x = (int16_t)axi_adxl_get_datax(ptr); int16_t y = (int16_t)axi_adxl_get_datay(ptr); - int16_t z = (int16_t)axi_adxl_get_datax(ptr); + int16_t z = (int16_t)axi_adxl_get_dataz(ptr); if (adxl_dev_get_data_format(ptr->dev) & DATA_FORMAT_FULL_RES) { data_float->x = (float)x / SENSITIVITY_FULL_RES; diff --git a/src_sw/selector.c b/src_sw/selector.c index ad8a795..aec7058 100644 --- a/src_sw/selector.c +++ b/src_sw/selector.c @@ -1071,11 +1071,11 @@ int selector_axi_adxl_has_interrupt_enabled(axi_adxl* ptr) { //100 int selector_axi_adxl_dev_debug_register_space(axi_adxl *ptr){ - axi_adxl_dev_debug_register_space(ptr->dev); - return ADXL_OK; + return axi_adxl_dev_debug_register_space(ptr); } + int selector_axi_adxl_change_range(axi_adxl *ptr){ int status = ADXL_OK; @@ -1968,7 +1968,7 @@ int selector_axi_adxl_get_ofsx(axi_adxl* ptr) { int ofs = 0; int status = axi_adxl_get_ofsx(ptr, &ofs); - printf("\t[MENU] : OFSY : 0x%02x [%3.6f g]\r\n", ofs, (float)ofs * SCALE_OFS); + printf("\t[MENU] : OFSX : 0x%02x [%3.6f g]\r\n", ofs, (float)ofs * SCALE_OFS); return status; @@ -3105,3 +3105,74 @@ int dbg_set_reg(axi_adxl *ptr){ *((uint8_t*)ptr->dev + (uint8_t)reg_address) = value; return ADXL_OK; } + + + +int selector_axi_adxl_set_output_rules(axi_adxl *ptr){ + printf("\t[MENU] : Switch output rules \r\n"); + int status = ADXL_OK; + int xyz_int = axi_adxl_is_output_rule(ptr, XYZ_INTEGER); + int xyz_float = axi_adxl_is_output_rule(ptr, XYZ_GRAVITY); + int roll_pitch = axi_adxl_is_output_rule(ptr, ROLL_PITCH); + + if (xyz_int){ + textcolor(DEFAULT, BLACK, GREEN); + }else{ + textcolor(DEFAULT, BLACK, RED); + } + printf("\t0. raw X, Y, Z"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); + + if (xyz_float){ + textcolor(DEFAULT, BLACK, GREEN); + }else{ + textcolor(DEFAULT, BLACK, RED); + } + printf("\t1. gravity X, Y, Z"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); + + if (roll_pitch){ + textcolor(DEFAULT, BLACK, GREEN); + }else{ + textcolor(DEFAULT, BLACK, RED); + } + printf("\t2. Roll and pitch"); + textcolor(DEFAULT, STD, STD); + printf("\r\n"); + + + int value = 0; + scanf("%d", &value); + + switch (value){ + case 0 : status = axi_adxl_set_output_rule(ptr, XYZ_INTEGER); break; + case 1 : status = axi_adxl_set_output_rule(ptr, XYZ_GRAVITY); break; + case 2 : status = axi_adxl_set_output_rule(ptr, ROLL_PITCH); break; + default : return ADXL_UNCORRECT_VALUE; + } + + return status; +} + + + +int selector_axi_adxl_get_output_rules(axi_adxl *ptr){ + + int status = 0; + int rule = 0; + status = axi_adxl_get_output_rule(ptr, &rule); + + printf("\t[MENU] : output rule is : "); + + switch (rule) { + case XYZ_INTEGER: printf("RAW X, Y, Z\r\n"); break; + case XYZ_GRAVITY: printf("GRAVITY X, Y, Z\r\n"); break; + case ROLL_PITCH: printf("ROLL and PITCH\r\n"); break; + default : printf("\r\n"); break; + } + + return status; +} + diff --git a/src_sw/selector.h b/src_sw/selector.h index ccb21c0..293ef9b 100644 --- a/src_sw/selector.h +++ b/src_sw/selector.h @@ -161,3 +161,7 @@ int selector_axi_adxl_has_inverted(axi_adxl *ptr); int selector_axi_adxl_justify(axi_adxl *ptr); int selector_axi_adxl_has_justify(axi_adxl *ptr); + + +int selector_axi_adxl_set_output_rules(axi_adxl *ptr); +int selector_axi_adxl_get_output_rules(axi_adxl *ptr);