diff --git a/builder/main.py b/builder/main.py index 64ef97d..5a7ee50 100644 --- a/builder/main.py +++ b/builder/main.py @@ -36,8 +36,19 @@ ARFLAGS=["rc"], - ASFLAGS=[], + SIZEPROGREGEXP=r"^(?:\.text|\.data|\.rodata|\.text.align|\.ARM.exidx)\s+(\d+).*", + SIZEDATAREGEXP=r"^(?:\.data|\.bss|\.noinit)\s+(\d+).*", + SIZECHECKCMD="$SIZETOOL -A -d $SOURCES", + SIZEPRINTCMD='$SIZETOOL -B -d $SOURCES', + + PROGSUFFIX=".elf" +) + +# Allow user to override via pre:script +if env.get("PROGNAME", "program") == "program": + env.Replace(PROGNAME="firmware") +env.Append( CCFLAGS=[ "-O0", "-ffunction-sections", # place each function in its own section @@ -65,32 +76,6 @@ LIBS=["c", "gcc", "m", "stdc++", "nosys"], - SIZEPROGREGEXP=r"^(?:\.text|\.data|\.rodata|\.text.align|\.ARM.exidx)\s+(\d+).*", - SIZEDATAREGEXP=r"^(?:\.data|\.bss|\.noinit)\s+(\d+).*", - SIZECHECKCMD="$SIZETOOL -A -d $SOURCES", - SIZEPRINTCMD='$SIZETOOL -B -d $SOURCES', - - PROGSUFFIX=".elf" -) - -# Allow user to override via pre:script -if env.get("PROGNAME", "program") == "program": - env.Replace(PROGNAME="firmware") - - -if "BOARD" in env: - env.Append( - CCFLAGS=[ - "-mcpu=%s" % env.BoardConfig().get("build.cpu") - ], - LINKFLAGS=[ - "-mcpu=%s" % env.BoardConfig().get("build.cpu") - ] - ) - -env.Append( - # ASFLAGS=env.get("CCFLAGS", [])[:], - BUILDERS=dict( ElfToBin=Builder( action=env.VerboseAction(" ".join([ @@ -117,6 +102,16 @@ ) ) +if "BOARD" in env: + env.Append( + CCFLAGS=[ + "-mcpu=%s" % env.BoardConfig().get("build.cpu") + ], + LINKFLAGS=[ + "-mcpu=%s" % env.BoardConfig().get("build.cpu") + ] + ) + # # Target: Build executable and linkable firmware # diff --git a/examples/LEVELER/.gitignore b/examples/Compass/.gitignore similarity index 100% rename from examples/LEVELER/.gitignore rename to examples/Compass/.gitignore diff --git a/examples/LEVELER/.travis.yml b/examples/Compass/.travis.yml similarity index 100% rename from examples/LEVELER/.travis.yml rename to examples/Compass/.travis.yml diff --git a/examples/LEVELER/include/API/userAPI.h b/examples/Compass/include/API/userAPI.h similarity index 78% rename from examples/LEVELER/include/API/userAPI.h rename to examples/Compass/include/API/userAPI.h index 986a71f..88b2ad4 100644 --- a/examples/LEVELER/include/API/userAPI.h +++ b/examples/Compass/include/API/userAPI.h @@ -26,12 +26,25 @@ limitations under the License. #ifndef _USER_API_H #define _USER_API_H +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + #include #include "gpsAPI.h" -void inertialAndPositionDataProcessing(int dacqRate); +void inertialAndPositionDataProcessing(uint16_t dacqRate); -void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, int dacqRate); +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); void WriteResultsIntoOutputStream(void *results) ; void InitUserDataStructures(); void InitUserFilters(); @@ -39,5 +52,4 @@ void InitUserAlgorithm(); void initUserDataProcessingEngine(); void userInitConfigureUnit(); - #endif diff --git a/examples/LEVELER/include/FreeRTOSConfig.h b/examples/Compass/include/FreeRTOSConfig.h similarity index 98% rename from examples/LEVELER/include/FreeRTOSConfig.h rename to examples/Compass/include/FreeRTOSConfig.h index 690eca8..80ca6b9 100644 --- a/examples/LEVELER/include/FreeRTOSConfig.h +++ b/examples/Compass/include/FreeRTOSConfig.h @@ -96,7 +96,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES (7) #define configMINIMAL_STACK_SIZE ((uint16_t)2048) -#define configTOTAL_HEAP_SIZE ((size_t)(30 * 2048)) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) #define configMAX_TASK_NAME_LEN (30) #define configUSE_TRACE_FACILITY 1 #define configUSE_16_BIT_TICKS 0 @@ -109,6 +109,7 @@ #define configUSE_APPLICATION_TASK_TAG 0 #define configUSE_COUNTING_SEMAPHORES 1 #define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 /* Co-routine definitions. */ #define configUSE_CO_ROUTINES 0 diff --git a/examples/LEVELER/include/osresources.h b/examples/Compass/include/osresources.h similarity index 100% rename from examples/LEVELER/include/osresources.h rename to examples/Compass/include/osresources.h diff --git a/examples/LEVELER/include/taskDataAcquisition.h b/examples/Compass/include/taskDataAcquisition.h similarity index 89% rename from examples/LEVELER/include/taskDataAcquisition.h rename to examples/Compass/include/taskDataAcquisition.h index a02b74a..891fb4e 100644 --- a/examples/LEVELER/include/taskDataAcquisition.h +++ b/examples/Compass/include/taskDataAcquisition.h @@ -27,9 +27,7 @@ limitations under the License. #ifndef _TASK_DATA_ACQUISITION_H_ #define _TASK_DATA_ACQUISITION_H_ -// Specify the limit used in the int16-limiter -#define INT16_LIMIT 32765 -#define INT12_LIMIT 2045 + #include "stdint.h" #include "GlobalConstants.h" extern void TaskDataAcquisition(void const *argument); @@ -40,8 +38,4 @@ extern void EnterMainAlgLoop(void); extern void DataAquisitionStart(void); extern BOOL isOneHundredHertzFlag(void); -int16_t LimitInt16Value( int16_t value, int16_t limit ); -extern uint32_t dacqTimer; - - -#endif \ No newline at end of file +#endif diff --git a/examples/Compass/ldscripts/stm32f40x.ld b/examples/Compass/ldscripts/stm32f40x.ld new file mode 100644 index 0000000..1c0c9f6 --- /dev/null +++ b/examples/Compass/ldscripts/stm32f40x.ld @@ -0,0 +1,202 @@ +/* +***************************************************************************** +** + +** File : LinkerScript.ld +** +** Abstract : Linker script for STM32F405RGx Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +**

© COPYRIGHT(c) 2014 Ac6

+** +** Redistribution and use in source and binary forms, with or without modification, +** are permitted provided that the following conditions are met: +** 1. Redistributions of source code must retain the above copyright notice, +** this list of conditions and the following disclaimer. +** 2. Redistributions in binary form must reproduce the above copyright notice, +** this list of conditions and the following disclaimer in the documentation +** and/or other materials provided with the distribution. +** 3. Neither the name of Ac6 nor the names of its contributors +** may be used to endorse or promote products derived from this software +** without specific prior written permission. +** +** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = 0x2001ff00; /* end of RAM */ +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x2000; /* required amount of heap */ +_Min_Stack_Size = 0x2000; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ +FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 448K +EEPROM (rw) : ORIGIN = 0x08008000, LENGTH = 16K +USER_EEPROM(rw) : ORIGIN = 0x0800C000, LENGTH = 16K +RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data goes into FLASH */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH + + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + .eeprom 0x08008000: + { + . = ALIGN(4); + KEEP(*(.xbowEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eeeprom = .; /* define a global symbols at end of code */ + } >EEPROM + + .usereeprom 0x0800C000: + { + . = ALIGN(4); + KEEP(*(.userEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eusereeprom = .; /* define a global symbols at end of code */ + } >USER_EEPROM + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/examples/Compass/lib/CLI/include/commandLine.h b/examples/Compass/lib/CLI/include/commandLine.h new file mode 100644 index 0000000..33535f0 --- /dev/null +++ b/examples/Compass/lib/CLI/include/commandLine.h @@ -0,0 +1,54 @@ +/** *************************************************************************** + * @file commandLine.h DEBUG parser functions + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef COMMAND_LINE_H +#define COMMAND_LINE_H + +#include + + +typedef void(*tShellCallback)(uint32_t); + +/// command token, callback and help table +typedef struct { + char const *name; + tShellCallback callback; + uint32_t callbackData; + char const *help; +} tCommand; + +extern const tCommand gCommands[]; + +void CmdPrintPrompt(); +void CmdLineLookup(tCommand const *cmd_table); +int CmdLineGetArgString(uint8_t **s); +int CmdLineGetArgInt(int32_t *i); +int CmdLineGetArgUInt(uint32_t *i); +int CmdLineGetArgFloat(float *f); +void CmdLineExec(char const *cmdline, const tCommand *cmd_table); +void TaskCommandLine(void const *argument); + + +#endif //COMMAND_LINE_H \ No newline at end of file diff --git a/examples/Compass/lib/CLI/include/commandTable.h b/examples/Compass/lib/CLI/include/commandTable.h new file mode 100644 index 0000000..14a2ee0 --- /dev/null +++ b/examples/Compass/lib/CLI/include/commandTable.h @@ -0,0 +1,44 @@ +/** *************************************************************************** + * @file commandTable.h table of commands, calbacks, and help strings + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Table of commands available to be sent from the commandLine.c shell + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + + +//#include "commandLine.h" +#include "commands.h" + +#define COMMAND_TABLE_END {"",0,0,""} + +//Command table +// {char *name, tShellCallback callback, uint32_t callbackData, const *help } +const tCommand gCommands[] = +{ + {"ver", &CmdVersion, 0, "Display firmware version"}, + {"raccel", &CmdReadAccelerometer, 0, "Read accelerometer"}, + {"rgyro", &CmdReadGyro, 0, "Read Gyro"}, + {"rmag", &CmdReadMagnetometer, 0, "Read magnetometer"}, +// {"rGPS", &CmdGpsRead, 0, "Read current GPS value" }, + COMMAND_TABLE_END //MUST BE LAST!!! +}; + diff --git a/examples/Compass/lib/CLI/include/commands.h b/examples/Compass/lib/CLI/include/commands.h new file mode 100644 index 0000000..3c4e563 --- /dev/null +++ b/examples/Compass/lib/CLI/include/commands.h @@ -0,0 +1,33 @@ +/****************************************************************************** + * @file commands.h +*******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef COMMANDS_H +#define _COMMANDS_H + +void CmdVersion(uint32_t data); +void CmdUsartBaudRate( uint32_t data ); +void CmdReadAccelerometer(uint32_t data); +void CmdReadAccelTemp(uint32_t data); +void CmdReadGyro(uint32_t data); +void CmdReadGyroTemp(uint32_t data); +void CmdGpsRead(uint32_t data); +void CmdReadMagnetometer(uint32_t data); + + +#endif /* COMMANDS_H */ \ No newline at end of file diff --git a/examples/Compass/lib/CLI/src/commandLine.c b/examples/Compass/lib/CLI/src/commandLine.c new file mode 100644 index 0000000..3e0bf34 --- /dev/null +++ b/examples/Compass/lib/CLI/src/commandLine.c @@ -0,0 +1,250 @@ +/** *************************************************************************** + * @file commandLine.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Simple interactive serial console shell - line input + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include +#include +#include +#include "commandLine.h" +#include "debug.h" +#include "debug_usart.h" +#include "utilities.h" +#include "osapi.h" +#include "osresources.h" + +static void _ExecLine(tCommand const *cmd_table); +static void _Dispatch(char const *token, tCommand const *table); +static void _CmdHelp(tCommand const *table); + +static char gCmdLine[80]; +static char *gCmdLineIndex; +#define PARSE_SEPARATOR ' ' + +/** *************************************************************************** + * @name CmdPrintPrompt() + * @brief send prompt characters out serial to the debug serial console + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void CmdPrintPrompt() +{ + static uint16_t lineno; + DebugPrintInt("", lineno); + lineno++; + DebugPrintString("> "); +} + +/** *************************************************************************** + * @name CmdLineLookup() + * @brief at startup call send prompt characters out serial to console + * the debug serial + * + * @param [in] cmd_table - table entry to parse + * @param [in] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void CmdLineLookup(tCommand const *cmd_table) +{ + static uint32_t index = 0; + if (DebugSerialReadLine((uint8_t*) gCmdLine, &index, 80)) { + strrep(gCmdLine, '\r', 0); + strrep(gCmdLine, '\n', 0); + if (gCmdLine[0]) {//don't process empty string + //Ignore lines that start with # character + if (gCmdLine[0] != '#') { + _ExecLine(cmd_table); + } + } + CmdPrintPrompt(); + } // else wait until prompt is ready +} + +/** *************************************************************************** + * @name _ExecLine() LOCAL + * @brief extract tokens from strings with ' ' as the delimiter + * + * @param [in] cmd_table - table entry to parse + * @param [in] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +static void _ExecLine(tCommand const *cmd_table) { + char *token = strtok_r1(gCmdLine, PARSE_SEPARATOR, &gCmdLineIndex); + _Dispatch(token, cmd_table); +} + +void CmdLinexec(char const *cmdline, + tCommand const *cmd_table) { + char *dst; + if (cmdline && *cmdline) { + dst = gCmdLine; + while ((*dst++ = *cmdline++)){;} + _ExecLine(cmd_table); + } +} + +/** *************************************************************************** + * @name CmdLineGetArgString() + * @brief get string from command line arguments + * + * @param [out] s - pointer to a string + * @retval N/A + ******************************************************************************/ +int CmdLineGetArgString(uint8_t **s) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (s) { + *s = (uint8_t*)token; + } + return 1; + } else { + return 0; + } +} + +int CmdLineGetArgInt(int32_t *i) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (i) { + *i = atoi(token); + } + return 1; + } else { + return 0; + } +} + +/** *************************************************************************** + * @name CmdLineGetArgUInt() + * @brief get unsigned integer from command line arguments + * + * @param [out] t - ingteger to return + * @retval N/A + ******************************************************************************/ +int CmdLineGetArgUInt(uint32_t *i) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (i) { + *i = atoi(token); + } + return 1; + } else { + return 0; + } +} + +int CmdLineGetArgFloat(float *f) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (f) { + *f = atof(token); + } + return 1; + } else { + return 0; + } +} + + +/** *************************************************************************** + * @name _Dispatch() + * @brief Traverses a command table and dispatches the appropriate callback + * + * @param [in] token - command text from the console + * @param [in] table - command table + * @retval N/A + ******************************************************************************/ +static void _Dispatch(char const *token, + tCommand const *table) +{ + tCommand *cmd = (tCommand*) table; + if (0 == strcmpi(token, "help")) { + _CmdHelp(table); + } else { + while (cmd->callback != 0) { + if (0 == strcmpi(token, cmd->name)) { + (*(cmd->callback))(cmd->callbackData); + return; + } else { + cmd++; + } + } + //If we get here, the command wasn't found + DebugPrintString("Unknown command ("); + DebugPrintString(token); + DebugPrintString(") Type 'help' for list.\r\n"); + } +} + +/** *************************************************************************** + * @name _CmdHelp() LOCAL + * @brief traverses command table and prints the help strings + * + * @param [in] token - command text from the console + * @param [in] table - command table + * @retval N/A + ******************************************************************************/ +static void _CmdHelp(tCommand const *table) { + char command[30]; + + tCommand *cmd = (tCommand*) table; + + while (cmd->callback != 0) { + DebugPrintString(" "); + sprintf(command, "%-10s", cmd->name); +// DebugPrintString(cmd->name); + DebugPrintString(command); +// DebugPrintString(" "); + DebugPrintString(cmd->help); + DebugPrintEndline(); + DebugPrintEndline(); + cmd++; + while (!IsDebugSerialIdle()) + {/*spin*/;} + } +} + +/** **************************************************************************** + * @name TaskCommandLine() LOCAL task callback function + * @brief handles debug serial console command line messaging + * When the USART interrupt handler indicates a RX on the USART line, + * toggle the LED and parse the received message using the commandTable + * + * @param [in] gCommands - command line command and arguments + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void TaskCommandLine(void const *argument) +{ + CmdPrintPrompt(); // out to serial console + + while (1) { + /// context switch and wait for the semaphore, BINSEM_SERIAL_RX, to + /// change state: occurs in the USART interrupt handler (DEBUG_USART_IRQ) + osSemaphoreWait( cliSem, OS_INFINITE_TIMEOUT ); + CmdLineLookup( gCommands ); // commandLine.c + } +} diff --git a/examples/Compass/lib/CLI/src/commands.c b/examples/Compass/lib/CLI/src/commands.c new file mode 100644 index 0000000..915b60c --- /dev/null +++ b/examples/Compass/lib/CLI/src/commands.c @@ -0,0 +1,126 @@ +/** *************************************************************************** + * @file commands.c callback functions from the commandTable + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Commands available to the commandLine.c shell + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + + +#include +#include +#include +#include "commandLine.h" +#include "osapi.h" +#include "sensorsAPI.h" +#include "platformAPI.h" + + +#define LOGGING_LEVEL LEVEL_INFO +#include "debug.h" + +char str[100]; +// Display firmware version +void CmdVersion(uint32_t data) +{ + uint8_t *model = (uint8_t*)unitVersionString(); + uint8_t *rev = (uint8_t*)platformBuildInfo(); + unsigned int serialNum = unitSerialNumber(); + snprintf(str, sizeof(str), "%s %s SN:%u", model, rev, serialNum ); + + DEBUG_STRING(str); + DEBUG_ENDLINE(); +} // End of display firmware version + +/** *************************************************************************** + * @name CmdReadAccelerometer() Read accelerometer + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadAccelerometer(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetAccelData_mPerSecSq(readings); + OSEnableHook(); + + DEBUG_STRING("Reading accelerometer data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + +/** *************************************************************************** + * @name CmdReadGyro() Read gyro data + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadGyro(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetRateData_degPerSec(readings); + OSEnableHook(); + + DEBUG_STRING("Reading gyro data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + +/** *************************************************************************** + * @name CmdReadMagnetometer. Read magnetometer data + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadMagnetometer(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetMagData_G(readings); + OSEnableHook(); + + DEBUG_STRING("Reading magnetometer data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + + +#include "commandTable.h" diff --git a/examples/LEVELER/lib/readme.txt b/examples/Compass/lib/readme.txt similarity index 100% rename from examples/LEVELER/lib/readme.txt rename to examples/Compass/lib/readme.txt diff --git a/examples/Compass/platformio.ini b/examples/Compass/platformio.ini new file mode 100644 index 0000000..a955859 --- /dev/null +++ b/examples/Compass/platformio.ini @@ -0,0 +1,47 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html +[platformio] +description = + The compass app uses calibrated accelerometer readings to measure the local gravity-field, + compute the two-axis attitude (roll and pitch angles) of a body relative to the local-level + frame, and ‘level’ the magnetometer reading (project the reading into the plane perpendicular + to the gravity field). Once ‘level’ the magnetometer reading is used to compute heading. + +[env:OpenIMU300ZA] +platform = aceinna_imu +lib_archive=false +board = OpenIMU300ZA +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps = ../../../openimu-core +lib_deps = openimu-core-libraries@^1.0.22 +build_flags = + -D CLI + -D __FPU_PRESENT + -D ARM_MATH_CM4 + -D __VFP_FP + -I include + -I include/API + -I src/user +; -L ldscripts + -O0 +; -Wno-comment + -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld + -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 + +;upload_protocol = jlink +;debug_tool = jlink + + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/Compass/src/main.c b/examples/Compass/src/main.c new file mode 100644 index 0000000..c4bbfb1 --- /dev/null +++ b/examples/Compass/src/main.c @@ -0,0 +1,228 @@ +/** *************************************************************************** + * @file main.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * main is the center of the system universe, it all starts here. And never ends. + * entry point for system (pins, clocks, interrupts), data and task initialization. + * contains the main processing loop. - this is a standard implementation + * which has mainly os functionality in the main loop + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ +#define __MAIN + +#include + +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "debug.h" + +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" + +#include "osapi.h" +#include "osresources.h" + +#ifdef CLI +#include "commandLine.h" +#endif + +#ifdef GPS +#include "gpsAPI.h" +#endif + +#ifdef MEMSIC_CAN +#include "canAPI.h" +#endif + +char buildInfo[] = {__DATE__"," __TIME__}; + +/** *************************************************************************** + * @name getBuildInfo - provides the pointer to the build date and time string + * @brief + * + * @param [in] N/A + * @param [out] pointer to build info string + * @retval N/A + ******************************************************************************/ +char *getBuildInfo() +{ + return buildInfo; +} + +/** *************************************************************************** + * @name initDebugInterface Debug serial interface initialization + * @brief Initializes debug serial console messaging support + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void DebugInterfaceInit(void) +{ + char status[100]; + + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + + if(debugChannel == UART_CHANNEL_NONE){ + //nothing to do + return; + } + + // Add a delay to allow the system to stabilize after the reset line (nRst) + // is released + for(int i = 0; i < 4000000; i++) ; // TODO - calculate more precisely + //DelayMs(300); //todo - calculate + + // Initialize the DEBUG USART (serial) port + InitDebugSerialCommunication(230400); // debug_usart.c + //DEBUG_STRING("\r\nDMU380 System\r\n"); + + BoardGetResetStatus(status, sizeof(status)); + + ERROR_STRING(status); +} + +void CreateTasks(void) +{ + osThreadId iD; + + /// Create RTOS tasks: + // dacq task + osThreadDef(DACQ_TASK, TaskDataAcquisition, osPriorityAboveNormal, 0, 2048); + iD = osThreadCreate(osThread(DACQ_TASK), NULL); + if(iD == NULL){ + while(1); + } + + //user communication task + osThreadDef(USER_COMM_TASK, TaskUserCommunication, osPriorityNormal, 0, 2048); + iD = osThreadCreate(osThread(USER_COMM_TASK), NULL); + if(iD == NULL){ + while(1); + } + + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); + accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); + magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); + tempReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); + navDataReadySem = osSemaphoreCreate(osSemaphore(NAV_DATA_READY_SEM), 1); + dataAcqSem = osSemaphoreCreate(osSemaphore(DATA_ACQ_SEM), 1); + +#ifdef GPS + osThreadDef(GPS_TASK, TaskGps, osPriorityBelowNormal, 0, 1024); + iD = osThreadCreate(osThread(GPS_TASK), NULL); + if(iD == NULL){ + while(1); + } + osThreadDef(WMM_TASK, TaskWorldMagneticModel, osPriorityLow, 0, 512); + iD = osThreadCreate(osThread(WMM_TASK), NULL); + if(iD == NULL){ + while(1); + } +#endif + +#ifdef CLI + osThreadDef(CLI_TASK, TaskCommandLine, osPriorityLow, 0, 1024); + iD = osThreadCreate(osThread(CLI_TASK), NULL); + if(iD == NULL){ + while(1); + } + cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); +#endif +} + +/** *************************************************************************** + * @name main() The DMU380 firmware initialization and entry point + * @brief creates tasks for data collection SPI1 and user communications SPI3 + * or USART serial stream (Nav-View) debug serial console messaging + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +int main(void) +{ + // Initialize processor and board-related signals + BoardInit(); + + platformSetUnitCommunicationType(UART_COMM); + + // Un-assign and reassign ports +#ifdef GPS + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_1); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid +#else + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid + //res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_1); + //while(!res){}; // check if valid +#endif + + // Apply factory configuration + platformInitConfigureUnit(); + + // Apply user-chosen configuration + userInitConfigureUnit(); + + // Initialize OS and create required tasks + CreateTasks(); + + // Initialize the DEBUG USART (serial) port +#ifndef GPS_ON_DEBUG_PORT + DebugInterfaceInit(); +#endif + + //InitTimer_Watchdog( ENABLE ); + + // Start Running the tasks... + // Start scheduler + osKernelStart(); + + // We should never get here as control is now taken by the scheduler + for (;;); + +} + + +void vApplicationStackOverflowHook() +{ + while(1); +} +void vApplicationMallocFailedHook() +{ + while(1); +} diff --git a/examples/Compass/src/taskDataAcquisition.c b/examples/Compass/src/taskDataAcquisition.c new file mode 100644 index 0000000..9bed1ca --- /dev/null +++ b/examples/Compass/src/taskDataAcquisition.c @@ -0,0 +1,174 @@ +/***************************************************************************** + * @file taskDataAcquisition.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "algorithmAPI.h" +#include "bitAPI.h" +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" + +#include "osapi.h" +#include "osresources.h" + +/** *************************************************************************** + * @name TaskDataAcquisition() CALLBACK main loop + * @brief Get the sensor data at the specified frequency (based on the + * configuration of the accelerometer rate-sensor). Process and provide + * information to the user via the UART or SPI. + * @param N/A + * @retval N/A + ******************************************************************************/ +void TaskDataAcquisition(void const *argument) +{ + // + int res; + uint16_t dacqRate; + +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" + BOOL overRange = FALSE; //uncomment this line if overrange processing required +#pragma GCC diagnostic warning "-Wunused-but-set-variable" + + // This routine sets up the timer. Can use the structure below this point. + TaskDataAcquisition_Init(); + // Initialize user algorithm parameters, if needed + initUserDataProcessingEngine(); + // set the mag align scale factors + InitMagAlignParams(); + // Start sensors data acquisition + DataAquisitionStart(); + + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; + + //************** Add user initialization here, if needed **************** + + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. + + while( 1 ) + { + // ***************************************************************** + // NOTE: This task loop runs at 100 or 200 Hz (default 200 Hz) + // user can choose period of this task by + // ***************************************************************** + // Handle Timing vard, watchdog and BIT + PrepareToNewDacqTick(); + // Wait for next tick + // Upon timeout of TIM5 (or user sync), let the process continue + + res = osSemaphoreWait(dataAcqSem, 1000); + if(res != osOK){ + // Wait timeout expired. Something wrong wit the dacq system + // Process timeout here + } + + // inform user, that new data set is being prepared (if required) + // in case of UART communication interface sets pin IO2 high + setIO2Pin (1); + + // Get calibrated sensor data: + // Inside this function the sensor data is filtered by a second-order low-pass + // Butterworth filter, with a cutoff frequency selected by the user (or zero to + // disable). The cutoff is selected using the following: + // + // Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) + // + // Refer to UserConfiguration.c for implementation and to the enumerator structure + // 'eFilterType' in file filter.h for available selections. + // + // Low pass filtering is followed by application of the unit calibration parameters + // - scaling, temperature compensation, bias removal, and misalignment. + // + // Results are placed in the structure of type double. The pointer to this + // structure is 'pScaledSensors'. Ordering of sensors data in this structure + // defined by 'rawSensor_e' enumerator in the file indices.h + GetSensorsData(); + + // Check if sensors data over range + // If overRange is TRUE - limit values are put into sensors data based + // on chosen sensors sensitivity + overRange = handleOverRange(); + + // ***************************************************************** + // At this point sensors data is in next units + // Acceleration - g's + // Rates - rad/s + // Magnetometer - Gauss + // Temperature - degrees C + //****************************************************************** + + // BIT status. May have inadvertently changed this during an update. + updateBITandStatus(); + + // ********************** Algorithm ************************ + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. + inertialAndPositionDataProcessing(dacqRate); + + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output + //***************************************************************** + // applyNewScaledSensorsData(); + //***************************************************************** + + // Inform user, that new data set is ready (if required) + // in case of UART communication interface clears pin IO2 + // Pin IO2 can be used for timing of data processing + setIO2Pin (0); + + if(platformHasMag()) { + // Mag Alignment (follows Kalman filter or user algorithm as the + // innovation routine calculates the euler angles and the magnetic + // vector in the NED-frame) + MagAlign(); // only does this on align + } + + if(platformGetUnitCommunicationType() != UART_COMM){ + // Perform interface - specific processing here + }else { + // Process commands and output continuous packets to UART + // Processing of user commands always goes first + ProcessUserCommands (); + SendContinuousPacket(dacqRate); + } + } +} diff --git a/examples/Compass/src/user/Compass.c b/examples/Compass/src/user/Compass.c new file mode 100644 index 0000000..bc63030 --- /dev/null +++ b/examples/Compass/src/user/Compass.c @@ -0,0 +1,172 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "Compass.h" + +#include "VectorMath.h" +#include "math.h" + +#include "gpsAPI.h" + +#include "MagAlign.h" + +static void _Compass_IncrementTimer(void); + +// SETTERS: for compass structures +CompassDataStruct gCompass; + +// Extract the attitude (expressed in Euler-angles) of the body-frame (B) +// in the NED-frame (N) in degrees. +void Compass_GetAttitude_EA(real *EulerAngles) +{ + EulerAngles[ROLL] = gCompass.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + EulerAngles[PITCH] = gCompass.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; + EulerAngles[YAW] = gCompass.measuredEulerAngles_BinN[YAW] * RAD_TO_DEG; +} + + +// Replace this with algorithm_setExeFreq +// Setter used to specify the leveler execution frequency at initialization +void Compass_SetExeFreq(uint16_t freq) +{ + gCompass.callingFreq = freq; +} + + +// Initialization routine used to set up the timing output variables +void Compass_InitializeAlgorithmStruct(uint16_t callingFreq) +{ + // Compute dt from the calling frequency of the function, compute the + // timer delta-value from dt (account for rounding), and initialize + // the timer. + gCompass.callingFreq = callingFreq; + + gCompass.dt = 1.0 / (real)callingFreq; + gCompass.dTimerCntr = (uint32_t)( 1000.0 * gCompass.dt + 0.5 ); + gCompass.timerCntr = 0; +} + + +// +void Compass_Algorithm(void) +{ + // Measurement variables + float accelsFloat_B[NUM_AXIS], aHat_B[NUM_AXIS]; + volatile float gHat_B[NUM_AXIS]; + + // + _Compass_IncrementTimer(); + + // -------- Compute the roll/pitch values from accelerometer readings -------- + // Compute the acceleration unit-vector + accelsFloat_B[X_AXIS] = (float)gCompass.input.accel_B[X_AXIS]; + accelsFloat_B[Y_AXIS] = (float)gCompass.input.accel_B[Y_AXIS]; + accelsFloat_B[Z_AXIS] = (float)gCompass.input.accel_B[Z_AXIS]; + + VectorNormalize( accelsFloat_B, aHat_B ); + + // Form the gravity vector in the body-frame (negative of the + // accelerometer measurement) + gHat_B[X_AXIS] = -aHat_B[X_AXIS]; + gHat_B[Y_AXIS] = -aHat_B[Y_AXIS]; + gHat_B[Z_AXIS] = -aHat_B[Z_AXIS]; + + // Form the roll and pitch angles (in radians) from the gravity + // unit-vector. Formulation is based on a 321-rotation sequence + // and assumption that the gravity-vector is constant. + gCompass.measuredEulerAngles_BinN[ROLL] = (real)( atan2( gHat_B[Y_AXIS], + gHat_B[Z_AXIS] ) ); + gCompass.measuredEulerAngles_BinN[PITCH] = (real)( -asin( gHat_B[X_AXIS] ) ); + + // -------- Compute the heading (yaw) values from magnetometer readings -------- + // Form R_BinPerp from roll and pitch and level the magnetometer measurement + real sinRoll, cosRoll; + real sinPitch, cosPitch; + + // + sinRoll = (real)(sin( gCompass.measuredEulerAngles_BinN[ROLL] )); + cosRoll = (real)(cos( gCompass.measuredEulerAngles_BinN[ROLL] )); + sinPitch = (real)(sin( gCompass.measuredEulerAngles_BinN[PITCH] )); + cosPitch = (real)(cos( gCompass.measuredEulerAngles_BinN[PITCH] )); + + real temp; + temp = sinRoll * (float)gCompass.input.magField_B[Y_AXIS] + + cosRoll * (float)gCompass.input.magField_B[Z_AXIS]; + + float mags_Perp[NUM_AXIS]; + mags_Perp[X_AXIS] = cosPitch * (float)gCompass.input.magField_B[X_AXIS] + sinPitch * temp; + mags_Perp[Y_AXIS] = cosRoll * (float)gCompass.input.magField_B[Y_AXIS] - sinRoll * (float)gCompass.input.magField_B[Z_AXIS]; + mags_Perp[Z_AXIS] = -sinPitch * (float)gCompass.input.magField_B[X_AXIS] + cosPitch * temp; + + // Correct for hard/soft-iron effects in the body-frame + float magCorr[2], tmp[2]; + tmp[X_AXIS] = mags_Perp[X_AXIS] - gMagAlign.hardIronBias[X_AXIS]; + tmp[Y_AXIS] = mags_Perp[Y_AXIS] - gMagAlign.hardIronBias[Y_AXIS]; + + magCorr[X_AXIS] = gMagAlign.SF[0] * tmp[X_AXIS] + gMagAlign.SF[1] * tmp[Y_AXIS]; + magCorr[Y_AXIS] = gMagAlign.SF[2] * tmp[X_AXIS] + gMagAlign.SF[3] * tmp[Y_AXIS]; + + // Form the heading from the leveled magnetometer readings + // The negative of the angle the vector makes with the unrotated (psi = 0) + // frame is the yaw-angle of the initial frame. + gCompass.measuredEulerAngles_BinN[YAW] = (real)( -atan2( magCorr[Y_AXIS], magCorr[X_AXIS] ) ); +} + + +static void _Compass_IncrementTimer(void) +{ + // Increment the timerCntr + gCompass.timerCntr = gCompass.timerCntr + gCompass.dTimerCntr; +} + + +// Populate the EKF input structure with sensor and GPS data (if used) +void Compass_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) +{ + // Accelerometer signal is in [g] + gCompass.input.accel_B[X_AXIS] = accels[X_AXIS]; + gCompass.input.accel_B[Y_AXIS] = accels[Y_AXIS]; + gCompass.input.accel_B[Z_AXIS] = accels[Z_AXIS]; + + // Angular-rate signal is in [rad/s] + gCompass.input.angRate_B[X_AXIS] = rates[X_AXIS]; + gCompass.input.angRate_B[Y_AXIS] = rates[Y_AXIS]; + gCompass.input.angRate_B[Z_AXIS] = rates[Z_AXIS]; + + // Magnetometer signal is in [G] + gCompass.input.magField_B[X_AXIS] = mags[X_AXIS]; + gCompass.input.magField_B[Y_AXIS] = mags[Y_AXIS]; + gCompass.input.magField_B[Z_AXIS] = mags[Z_AXIS]; +} + + +// Populate the compass output structure with algorithm results +void Compass_SetOutputStruct(void) +{ + // Euler-angles in [deg] + gCompass.output.measuredEulerAngles_BinN[ROLL] = gCompass.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + gCompass.output.measuredEulerAngles_BinN[PITCH] = gCompass.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; + gCompass.output.measuredEulerAngles_BinN[YAW] = gCompass.measuredEulerAngles_BinN[YAW] * RAD_TO_DEG; +} diff --git a/examples/Compass/src/user/Compass.h b/examples/Compass/src/user/Compass.h new file mode 100644 index 0000000..e6f24f0 --- /dev/null +++ b/examples/Compass/src/user/Compass.h @@ -0,0 +1,60 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _COMPASS_H_ +#define _COMPASS_H_ + +//#include "GlobalConstants.h" +#include "Indices.h" + +#include "gpsAPI.h" // for gpsDataStruct_t + +// Compass related functions +void Compass_GetAttitude_EA(real *EulerAngles); +void Compass_SetExeFreq(uint16_t freq); +void Compass_InitializeAlgorithmStruct(uint16_t callingFreq); + +void Compass_Algorithm(void); +void Compass_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps); +void Compass_SetOutputStruct(void); + +// Compass input data structure +typedef struct { + // Sensor readings + double accel_B[NUM_AXIS]; + double angRate_B[NUM_AXIS]; + double magField_B[NUM_AXIS]; +} CompassInputDataStruct; + + +// Compass output data structure +typedef struct { + // Angles converted to degrees + double measuredEulerAngles_BinN[NUM_AXIS]; +} CompassOutputDataStruct; + + +// Compass data structure +typedef struct { + // Compass calling-frequency and derived + uint16_t callingFreq; + real dt; + + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + real measuredEulerAngles_BinN[NUM_AXIS]; + + // Input/output data items + CompassInputDataStruct input; + CompassOutputDataStruct output; +} CompassDataStruct; + +extern CompassDataStruct gCompass; + +#endif /* _COMPASS_H_ */ diff --git a/examples/Compass/src/user/UserAlgorithm.c b/examples/Compass/src/user/UserAlgorithm.c new file mode 100644 index 0000000..2b307e0 --- /dev/null +++ b/examples/Compass/src/user/UserAlgorithm.c @@ -0,0 +1,216 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include + +//#include "algorithmAPI.h" +#include "gpsAPI.h" +//#include "platformAPI.h" +#include "userAPI.h" + +#include "Indices.h" +#include "GlobalConstants.h" + +#include "math.h" +#include "VectorMath.h" + + +// +#include "algorithm.h" +#include "UserAlgorithm.h" + +#include "bsp.h" +#include "debug.h" + +#include "MagAlign.h" + + +// Declare the compass data structure +#include "Compass.h" + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _InitAlgo(uint8_t algoType); + +// Initialize compass algorithm variables +void InitUserAlgorithm() +{ + // Initialize built-in algorithm structure + Compass_InitializeAlgorithmStruct(FREQ_200_HZ); + + // place additional required initialization here +} + + +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) +{ + // This can be set at startup based on the packet type selected + static uint8_t algoType = IMU; + + // Initialize variable related to the UserNavAlgorithm + _InitAlgo(algoType); + + // Populate the compass input data structure. Load the GPS data + // structure as NULL. + Compass_SetInputStruct(accels, rates, mags, NULL); + + // Call the desired algorithm based on the EKF with different + // calling rates and different settings. + _Algorithm(dacqRate, algoType); + + // Fill the output data structure with the algorithm states and other + // desired information + Compass_SetOutputStruct(); + + // Generate a debug message that provides algorithm output to verify the + // algorithm is generating the proper output. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // The returned value from this function is unused by external functions. The + // NULL pointer is returned instead of a data structure. + return NULL; +} + + +// +static void _InitAlgo(uint8_t algoType) +{ + // Initialize the timer variables + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the configuration variables for a VG-type solution + // (useMags = 0 forces the VG solution) + gAlgorithm.Behavior.bit.freeIntegrate = 0; + gAlgorithm.Behavior.bit.useMag = 0; + gAlgorithm.Behavior.bit.useGPS = 0; + gAlgorithm.Behavior.bit.stationaryLockYaw = 0; + gAlgorithm.Behavior.bit.restartOnOverRange = 0; + gAlgorithm.Behavior.bit.dynamicMotion = 1; + + // Set the system configuration based on system type + switch( algoType ) { + case VG: + // Nothing additional to do (already configured for a VG + // solution) + break; + case AHRS: + // Set the configuration variables for AHRS solution + // (useMags = 1 and enable mags) + enableMagInAlgorithm(TRUE); + gAlgorithm.Behavior.bit.useMag = 1; + break; + case INS: + // Nothing additional to do (already configured for a VG + // solution) + enableMagInAlgorithm(TRUE); + gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above + gAlgorithm.Behavior.bit.useMag = 1; + gAlgorithm.Behavior.bit.useGPS = 1; + break; + default: + // Nothing to do + break; + } + } +} + + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType) +{ + // + static uint8_t algoCntr = 0, algoCntrLimit = 0; + + // Initialize the configuration variables needed to make the system + // generate a VG-type solution. + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the variables that control the algorithm execution rate + algoCntrLimit = (uint8_t)( (float)dacqRate / (float)gCompass.callingFreq + 0.5 ); + if( algoCntrLimit < 1 ) { + // If this logic is reached, also need to adjust the algorithm + // parameters to match the modified calling freq (or stop the + // program to indicate that the user must adjust the program) + algoCntrLimit = 1; + } + algoCntr = algoCntrLimit; + } + + // Increment the counter. If greater than or equal to the limit, reset + // the counter to cause the algorithm to run on the next pass through. + algoCntr++; + if(algoCntr >= algoCntrLimit) { + // Reset counter + algoCntr = 0; + + // Aceinna VG/AHRS/INS algorithm + Compass_Algorithm(); + } +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the output of the EKF algorithm + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; + if( gMagAlign.state != 0xC) { + DebugPrintFloat("Time: ", gCompass.timerCntr * 0.001, 3); + DebugPrintFloat(", Roll: ", gCompass.output.measuredEulerAngles_BinN[ROLL], 5); + DebugPrintFloat(", Pitch: ", gCompass.output.measuredEulerAngles_BinN[PITCH], 5); + DebugPrintFloat(", Heading: ", gCompass.output.measuredEulerAngles_BinN[YAW], 5); + DebugPrintEndline(); + } + } + } +} diff --git a/examples/Compass/src/user/UserAlgorithm.h b/examples/Compass/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/Compass/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/Compass/src/user/UserConfiguration.c b/examples/Compass/src/user/UserConfiguration.c new file mode 100644 index 0000000..e42992b --- /dev/null +++ b/examples/Compass/src/user/UserConfiguration.c @@ -0,0 +1,567 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "string.h" + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" +#include "platformAPI.h" + +#include "UserConfiguration.h" +#include "UserMessaging.h" +#include "Indices.h" + +// Default user configuration structure +// Applied to unit upon reception of "zR" command +// Do Not remove - just add extra parameters if needed +// Change default settings if desired +const UserConfigurationStruct gDefaultUserConfig = { + .dataCRC = 0, + .dataSize = sizeof(UserConfigurationStruct), + .userUartBaudRate = 230400, + .userPacketType = "c1", + .userPacketRate = 10, + .lpfAccelFilterFreq = 25, + .lpfRateFilterFreq = 25, + .orientation = "+X+Y+Z", + // add default parameter values here, if desired + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 +}; + +UserConfigurationStruct gUserConfiguration; +UserConfigurationStruct gTmpUserConfiguration; + +uint8_t UserDataBuffer[4096]; +volatile char *info; +BOOL configValid = FALSE; + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} + + +void userInitConfigureUnit() +{ + uint64_t *ptr = (uint64_t*)&gUserConfiguration; + int size = sizeof(gUserConfiguration); // total size in bytes + + // sanity check for maximum size of user config structure; + if(size >= 0x4000){ + while(1); + } + + if(appStartedFirstTime()){ + // comment next line if want to keep previously stored in EEPROM parameters + // after rebuilding and/or reloading new application + RestoreDefaultUserConfig(); // JSM - Commented out so the mag-align values + // won't be overwritten each time the + // firmware is reloaded + } + + // Validate checksum of user configuration structure + configValid = validateUserConfigInEeprom(&size); + + if(configValid == TRUE){ + // Here we have validated User configuration image. + // Load it from eeprom into ram on top of the default configuration + loadUserConfigFromEeprom((void*)&gUserConfiguration, &size); + }else{ + memset((void*)&gUserConfiguration, 0xff, sizeof(gUserConfiguration)); + } + + // assign new actual size + gUserConfiguration.dataSize = sizeof(UserConfigurationStruct); + + // apply parameters to the platform + for(int i = USER_USER_BAUD_RATE; i <= USER_LAST_SYSTEM_PARAM && configValid; i++){ + UpdateSystemParameter(i, ptr[i], TRUE); + } + + // validate and apply own parameters if desired + for(int i = USER_LAST_SYSTEM_PARAM+1; i < USER_MAX_PARAM && configValid; i++){ + UpdateUserParameter(i, ptr[i], TRUE); + } + + info = getBuildInfo(); +} + + + +/** *************************************************************************** + * @name UpdateSystemParameter - updating of system configuration parameter based of user preferences + * @brief + * + * @param [in] number - parameter number in user configuration structure + * @param [in] data - value of the parameter in little endian format + * @retval error (1), no error (0) + ******************************************************************************/ +// NEEDS TO BE CHECKED +BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) +{ + BOOL result = TRUE; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + if(number < USER_CRC || number >= USER_MAX_PARAM ){ + return FALSE; + } + + switch (number) { + case USER_USER_BAUD_RATE: + result = platformSetBaudRate((int)data, fApply); + break; + case USER_USER_PACKET_TYPE: + result = setUserPacketType((uint8_t*)&data, fApply); + break; + case USER_USER_PACKET_RATE: + result = platformSetPacketRate((int)data, fApply); + break; + case USER_LPF_ACCEL_TYPE: + result = platformSelectLPFilter(ACCEL_SENSOR, (uint32_t)data, fApply); + break; + case USER_LPF_RATE_TYPE: + result = platformSelectLPFilter(RATE_SENSOR, (uint32_t)data, fApply); + break; + case USER_ORIENTATION: + result = platformSetOrientation((uint16_t*)&data, fApply); + break; + case USER_CRC: + case USER_DATA_SIZE: + return TRUE; + + // case USER_XXX: add function calls here if parameter XXXX + // required be updated on the fly + // break; + default: + // by default result should be FALSE for system parameter + result = FALSE; + break; + } + + if(result == TRUE){ + ptr[number] = data; + } + + return result; +} + + +/** *************************************************************************** + * @name UpdateUserParameter - updating user configuration parameter based of preferences + * @brief + * + * @param [in] number - parameter number in user configuration structure + * @param [in] data - value of the parameter in little endian format + * @retval error (1), no error (0) + ******************************************************************************/ +// NEEDS TO BE CHECKED +BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply) +{ + BOOL result; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + if(number <= USER_LAST_SYSTEM_PARAM || number >= USER_MAX_PARAM ){ + return FALSE; + } + + switch (number) { + // case USER_XXX_OFFSET: add function calls here if parameter XXXX + // required be updated on the fly and/or validated + // break; + default: + // by default result should be true if there is no special + // consideration or criteria for parameter validity + result = TRUE; + break; + } + + if(result == TRUE){ + ptr[number] = data; + } + + return result; +} + +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL offsetValid = TRUE; + BOOL lenValid = TRUE; + BOOL numValid = TRUE; + BOOL ret = FALSE; + int32_t result = 0; + + maxParam = sizeof(UserConfigurationStruct)/8; + + // Validate parameters numbers and quantity + if(pld->numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ + lenValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + if(pld->paramOffset >= maxParam){ + offsetValid = FALSE; + result = INVALID_PARAM; + } + + if((pld->numParams + pld->paramOffset) > maxParam){ + numValid = FALSE; + result = INVALID_PARAM; + } + + if(offsetValid && numValid && lenValid){ + // Validate parameters values first + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + result = INVALID_VALUE; + break; + } + } + offset++; + } + if(ret == TRUE){ + // Apply parameters values here + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], TRUE); + } + offset++; + } + } + } + + pld->numParams = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateUserParam + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + uint32_t maxParam; + BOOL offsetValid; + BOOL ret = TRUE; + int32_t result = 0; + + maxParam = sizeof(UserConfigurationStruct)/8; + offsetValid = pld->paramNum < maxParam; + + if(offsetValid){ + // Validate parameter first + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(pld->paramNum, pld->parameter, FALSE); + } + if(ret == TRUE){ + // Apply parameter if valid + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, TRUE); + if (ret != TRUE){ + ret = UpdateUserParameter(pld->paramNum, pld->parameter, TRUE); + } + }else{ + result = INVALID_VALUE; + } + } else { + result = INVALID_PARAM; + } + + pld->paramNum = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateAllUserParams + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL lenValid = TRUE; + BOOL numValid = TRUE; + BOOL ret = FALSE; + int32_t result = 0; + + int numParams = (*payloadLen)/8; + maxParam = sizeof(UserConfigurationStruct)/8; + + if(numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ + lenValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + if(numParams > maxParam){ + numValid = FALSE; + result = INVALID_PARAM; + } + + if(numValid && lenValid){ + // Validate parameters here + offset = 0; + for (i = 0; i < numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + result = INVALID_VALUE; + break; + } + } + offset++; + } + if(ret == TRUE){ + // Apply parameters here + offset = 0; + for (i = 0; i < numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], TRUE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], TRUE); + } + offset++; + } + } + } + + pld->parameters[0] = result; + *payloadLen = 4; // return error code + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserConfig + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL offsetValid = TRUE; + BOOL lenValid = TRUE; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + maxParam = sizeof(UserConfigurationStruct)/8; + + offsetValid = pld->paramOffset < maxParam; + + lenValid = ((pld->numParams + pld->paramOffset) <= maxParam) && + (pld->numParams <= MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET); + + if(offsetValid && lenValid){ + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++, offset++) + { + pld->parameters[i] = ptr[offset]; + } + *payloadLen = (pld->numParams + 1) * 8; + } else { + *payloadLen = 4; + pld->numParams = INVALID_PARAM; + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserParam + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, maxParam; + BOOL offsetValid; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + maxParam = sizeof(UserConfigurationStruct)/8; + offsetValid = pld->paramNum < maxParam; + + if(offsetValid){ + offset = pld->paramNum; + pld->parameter = ptr[offset]; + *payloadLen = 8 + 4; // parameter + number + } else { + *payloadLen = 4; // number + pld->paramNum = INVALID_PARAM; // invalid + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetAllUserParams + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + numParams = sizeof(UserConfigurationStruct)/8; + + offset = 0; + for (i = 0; i < numParams; i++, offset++){ + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + +/** **************************************************************************** + * @name SetMagAlignCmds + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL SetMagAlignCmds(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + numParams = sizeof(UserConfigurationStruct)/8; + + offset = 0; + for (i = 0; i < numParams; i++, offset++) { + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + +/** *************************************************************************** + * @name SaveUserConfig - saving of user configuration structure un the + * predefined flash sector + * @brief + * + * @param [in] N/A + * @retval error (0), no error (1) + ******************************************************************************/ +BOOL SaveUserConfig(void) +{ + int size; + BOOL status; + + size = sizeof(UserConfigurationStruct); + status = saveUserConfigInEeprom((uint8_t *)&gUserConfiguration, size); + + if(status){ + return TRUE; + } + + return FALSE; + +} + + +BOOL RestoreDefaultUserConfig(void) +{ + BOOL valid = TRUE; + // Load default user configuration + memcpy((void*)&gUserConfiguration, (void*)&gDefaultUserConfig, sizeof(UserConfigurationStruct)); + if(!SaveUserConfig()){ + valid = FALSE; + } + return valid; +} diff --git a/examples/Compass/src/user/UserConfiguration.h b/examples/Compass/src/user/UserConfiguration.h new file mode 100644 index 0000000..8cd4283 --- /dev/null +++ b/examples/Compass/src/user/UserConfiguration.h @@ -0,0 +1,147 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on JAn 25, 2017 + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef USER_CONFIGURATION_H +#define USER_CONFIGURATION_H + +#include + +#include "GlobalConstants.h" +#include "UserMessaging.h" +#include "filter.h" + +/// User defined configuration strucrture +///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types + +typedef struct { + uint64_t dataCRC; /// CRC of user configuration structure CRC-16 + uint64_t dataSize; /// Size of the user configuration structure + + int64_t userUartBaudRate; /// baudrate of user UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + /// 460800 + uint8_t userPacketType[8]; /// User packet to be continiously sent by unit + /// Packet types defined in structure UserOutPacketType + /// in file UserMessaging.h + + int64_t userPacketRate; /// Packet rate for continiously output packet, Hz. + /// Valid settings are: 0 ,2, 5, 10, 20, 25, 50, 100, 200 + + int64_t lpfAccelFilterFreq; /// built-in lpf filter cutoff frequency selection for accelerometers + int64_t lpfRateFilterFreq; /// built-in lpf filter cutoff frequency selection for rate sensors + /// Options are: + /// 0 - Filter turned off + /// 50 - Butterworth LPF 50HZ + /// 20 - Butterworth LPF 20HZ + /// 10 - Butterworth LPF 10HZ + /// 05 - Butterworth LPF 5HZ + /// 02 - Butterworth LPF 2HZ + /// 25 - Butterworth LPF 25HZ + /// 40 - Butterworth LPF 40HZ + + uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff + /// where dd - down axis, rr - right axis, ff - forward axis + /// next axis values a valid : + /// 'X' (0x58) -> plus X, 'Y' (0x59) -> plus Y, 'Z' (0x5a) -> plus Z + /// 'x' (0x78) -> minus X, 'y' (0x79) -> minus Y, 'z' (0x7a) ->minusZ + + //*************************************************************************************** + // here is the border between arbitrary parameters and platform configuration parameters + //*************************************************************************************** + + // place new arbitrary configuration parameters here + // parameter size should even to 4 bytes + // Add parameter offset in UserConfigParamOffset structure if validation or + // special processing required + + float hardIron_X; + float hardIron_Y; + float softIron_Ratio; + float softIron_Angle; +} UserConfigurationStruct; + +typedef enum{ +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) + USER_CRC = 0, + USER_DATA_SIZE , // 1 + USER_USER_BAUD_RATE , // 2 order of next 4 parameters + USER_USER_PACKET_TYPE , // 3 of required unit output bandwidth + USER_USER_PACKET_RATE , // 4 + USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer + USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor + USER_ORIENTATION , // 7 unit orientation + USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** +// add parameter enumerator here while adding new parameter in user UserConfigurationStruct + USER_HARD_IRON_X , + USER_HARD_IRON_Y , + USER_SOFT_IRON_RATIO , + USER_SOFT_IRON_ANGLE , + USER_MAX_PARAM +} UserConfigParamNumber; + +#define MAX_SYSTEM_PARAM USER_ORIENTATION + +extern int userPacketOut; + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +#define INVALID_PARAM -1 +#define INVALID_VALUE -2 +#define INVALID_PAYLOAD_SIZE -3 + + + +extern UserConfigurationStruct gUserConfiguration; + +extern void InitializeUserAlgorithmParams(void); +extern BOOL validateUserConfigInEeprom(int *numParams); +extern uint32_t getUserParamFromEeprom(uint32_t offset); +extern BOOL saveUserConfigInEeprom(uint8_t *ptrToUserConfigStruct, int userConfigStructLen); +extern int getUserPayloadLength(void); +extern BOOL checkIfUserEEPROMErased(void); +extern BOOL SaveUserData(void); +extern BOOL loadUserConfigFromEeprom(uint8_t *ptrToUserConfigInRam, int *userConfigSize); +//extern UcbPacketType checkPacketType(UcbPacketCodeType receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); +extern BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern BOOL SaveUserConfig(void); +extern BOOL RestoreDefaultUserConfig(void); +extern BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply); +extern BOOL UpdateSystemParameter(uint32_t offset, uint64_t data, BOOL fApply); + +#endif /* USER_CONFIGURATION_H */ + + diff --git a/examples/Compass/src/user/UserMessaging.c b/examples/Compass/src/user/UserMessaging.c new file mode 100644 index 0000000..5f50863 --- /dev/null +++ b/examples/Compass/src/user/UserMessaging.c @@ -0,0 +1,484 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include +#include +#include + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "sensorsAPI.h" +#include "userAPI.h" + +#include "UserMessaging.h" +#include "UserConfiguration.h" + +#include "Indices.h" // For X_AXIS, etc + +// Declare the IMU data structure +IMUDataStruct gIMU; +gpsDataStruct_t gGPS; + +// Version string +char userVersionString[] = "Compass 1.0.0"; + +// for EKFOutputDataStruct +#include "UserAlgorithm.h" + +#include "Compass.h" +CompassDataStruct *algo_res; // <-- define in useralgo + +// Example inputs: +// Ping: 55 55 70 47 00 5D 5F +// Mag Align: 55 55 6D 61 00 F0 2D +// 55 55 6D 61 01 00 F1 2E +// 55 55 6D 61 01 01 E1 0F +// 55 55 6D 61 01 02 D1 6C +// 55 55 6D 61 01 03 C1 4D +// 55 55 6D 61 01 04 B1 AA +// 55 55 6D 61 01 05 A1 8B +// 55 55 6D 61 01 06 91 E8 +// 55 55 6D 61 01 07 81 C9 // Get stored values +// 55 55 6D 61 01 08 70 26 +// 55 55 6D 61 01 09 60 07 +// 55 55 6D 61 01 0A 50 64 +// 55 55 6D 61 01 0B +// 55 55 6D 61 01 0C +// 55 55 6D 61 01 0D +// 55 55 6D 61 01 0E 10 E0 +// 55 55 6D 61 01 0F + +// other: 55 55 ... + +/// List of allowed packet codes +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, + {USR_IN_PING, "pG"}, + {USR_IN_UPDATE_CONFIG, "uC"}, + {USR_IN_UPDATE_PARAM, "uP"}, + {USR_IN_UPDATE_ALL, "uA"}, + {USR_IN_SAVE_CONFIG, "sC"}, + {USR_IN_RESTORE_DEFAULTS, "rD"}, + {USR_IN_GET_CONFIG, "gC"}, + {USR_IN_GET_PARAM, "gP"}, + {USR_IN_GET_ALL, "gA"}, + {USR_IN_GET_VERSION, "gV"}, + {USR_IN_RESET, "rS"}, +// place new input packet code here, before USR_IN_MAX + {USR_IN_MAG_ALIGN, "ma"}, // 0x6D 0x61 + {USR_IN_MAX, {0xff, 0xff}}, // "" +}; + + +// packet codes here should be unique - +// should not overlap codes for input packets and system packets +// First byte of Packet code should have value >= 0x61 +usr_packet_t userOutputPackets[] = { +// Packet Type Packet Code + {USR_OUT_NONE, {0x00, 0x00}}, + {USR_OUT_TEST, "zT"}, + {USR_OUT_DATA1, "z1"}, +// place new type and code here + {USR_OUT_SCALED1, "s1"}, + {USR_OUT_COM1, "c1"}, + {USR_OUT_MAX, {0xff, 0xff}}, // "" +}; + +volatile char *info; +static int _userPayloadLen = 0; +static int _outputPacketType = USR_OUT_MAX; +static int _inputPacketType = USR_IN_MAX; + + +int checkUserPacketType(uint16_t receivedCode) +{ + int res = UCB_ERROR_INVALID_TYPE; + usr_packet_t *packet = &userInputPackets[1]; + uint16_t code; + + // validate packet code here and memorise for further processing + while(packet->packetType != USR_IN_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _inputPacketType = packet->packetType; + return UCB_USER_IN; + } + packet++; + } + + packet = &userOutputPackets[1]; + + // validate packet code here and memorize for further processing + while(packet->packetType != USR_OUT_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _outputPacketType = packet->packetType; + return UCB_USER_OUT; + } + packet++; + } + + return res; +} + + +void userPacketTypeToBytes(uint8_t bytes[]) +{ + if(_inputPacketType && _inputPacketType < USR_IN_MAX) { + // response to request. Return same packet code + bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; + bytes[1] = userInputPackets[_inputPacketType].packetCode[1]; + _inputPacketType = USR_IN_MAX; // wait for next input packet + return; + } + + if(_outputPacketType && _outputPacketType < USR_OUT_MAX) { + // continuous packet + bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; + bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; + } else { + bytes[0] = 0; + bytes[1] = 0; + } + +} + + +/** *************************************************************************** + * @name setUserPacketType - set user output packet type + * @brief + * @param [in] packet type + * @retval - TRUE if success, FALSE otherwise + ******************************************************************************/ +BOOL setUserPacketType(uint8_t *data, BOOL fApply) +{ + int type = -1; + uint16_t *code = (uint16_t*)data; + uint16_t tmp; + BOOL result = TRUE; + + usr_packet_t *packet = &userOutputPackets[1]; + for(int i = 0; i < USR_OUT_MAX; i++, packet++){ + if(*code == *((uint16_t*)packet->packetCode)){ + type = packet->packetType; + break; + } + } + + switch(type){ + case USR_OUT_TEST: // simple test packet to check communication + _outputPacketType = type; + _userPayloadLen = USR_OUT_TEST_PAYLOAD_LEN; + break; + case USR_OUT_DATA1: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_DATA1_PAYLOAD_LEN; + break; + case USR_OUT_SCALED1: // packet with arbitrary data + _outputPacketType = type; + _userPayloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + break; + case USR_OUT_COM1: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_COM1_PAYLOAD_LEN; + break; + default: + result = FALSE; + break; + } + + if(result == FALSE){ + return FALSE; + } + + tmp = (data[0] << 8) | data[1]; + + result = platformSetOutputPacketCode(tmp, fApply); + + return result; +} + + +/** *************************************************************************** + * @name getUserPayloadLength - get user payload length for sanity check + * @brief + * + * @retval - user payload length + ******************************************************************************/ +int getUserPayloadLength(void) +{ + // ATTENTION: return actual user payload length, if user packet used + return _userPayloadLen; +} + +/****************************************************************************** + * @name HandleUserInputPacket - API + * @brief general handler + * @param [out] packetPtr - filled in packet from the mapped physical port + * @retval N/A + ******************************************************************************/ +int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) +{ + BOOL valid = TRUE; + int ret = USER_PACKET_OK; +// userPacket *pkt = (userPacket *)ptrUcbPacket->payload; + + /// call appropriate function based on packet type + switch (_inputPacketType) { + case USR_IN_RESET: + Reset(); + break; + case USR_IN_PING: + { + int len; + uint8_t *model = (uint8_t*)unitVersionString(); + uint8_t *rev = (uint8_t*)platformBuildInfo(); + unsigned int serialNum = unitSerialNumber(); + len = snprintf((char*)ptrUcbPacket->payload, 250, "%s%s SN:%u", model, rev, serialNum ); + ptrUcbPacket->payloadLength = len; + } + // leave all the same - it will be bounced back unchanged + break; + case USR_IN_GET_VERSION: + { + int len = snprintf((char*)ptrUcbPacket->payload, 250, "%s", userVersionString ); + ptrUcbPacket->payloadLength = len; + } + break; + case USR_IN_SAVE_CONFIG: + // payload length does not change + if(!SaveUserConfig()){ + valid = FALSE; + } + break; + case USR_IN_UPDATE_CONFIG: + UpdateUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_PARAM: + UpdateUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_ALL: + UpdateAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_RESTORE_DEFAULTS: + valid = RestoreDefaultUserConfig(); + break; + case USR_IN_GET_CONFIG: + if(!GetUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_PARAM: + if(!GetUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_ALL: + if(!GetAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_MAG_ALIGN: + // + if(!ProcessMagAlignCmds((magAlignCmdPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + default: + /// default handler - unknown packet + valid = FALSE; + break; + } + + if(!valid){ + ptrUcbPacket->payloadLength = 0; + ret = USER_PACKET_ERROR; + } + + ptrUcbPacket->packetType = UCB_USER_OUT; // do not remove - done for proper packet routing + + return ret; +} + + +/****************************************************************************** + * @name HandleUserOutputPacket - API call ro prepare continuous user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) +{ + static uint32_t _testVal = 0; + BOOL ret = TRUE; + + switch (_outputPacketType) { + case USR_OUT_TEST: + { + uint32_t *testParam = (uint32_t*)(payload); + *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; + *testParam = _testVal++; + } + break; + + case USR_OUT_DATA1: + { + int n = 0; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; + data1_payload_t *pld = (data1_payload_t *)payload; + + pld->timer = platformGetDacqTime(); + GetAccelData_mPerSecSq(accels); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)accels[i]; + } + GetRateData_degPerSec(rates); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)rates[i]; + } + GetMagData_G(mags); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)mags[i]; + } + *payloadLen = sizeof(data1_payload_t); + } + break; + + case USR_OUT_SCALED1: + { + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 1 floats (4 bytes) = 4 bytes + // ================================= + // NumOfBytes = 52 bytes + *payloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + + // Output time as represented by gIMU.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Output a double representation of time generated from + // gLeveler.itow + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gIMU.timerCntr); + + // Set the pointer of the sensor array to the payload + float *algoData_3 = (float*)(algoData_2); + *algoData_3++ = (float)gIMU.accel_g[X_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Y_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Z_AXIS]; + + *algoData_3++ = (float)gIMU.rate_degPerSec[X_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Y_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Z_AXIS]; + + *algoData_3++ = (float)gIMU.mag_G[X_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Y_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Z_AXIS]; + + *algoData_3++ = (float)gIMU.temp_C; + } + break; + + case USR_OUT_COM1: + { + // Variables used to hold the EKF values + real EulerAngles[NUM_AXIS]; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // ================================= + // NumOfBytes = 48 bytes + *payloadLen = USR_OUT_COM1_PAYLOAD_LEN; + + // Output time as represented by gLeveler.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gCompass.timerCntr; + + // Output a double representation of time generated from + // gLeveler.timerCntr + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gCompass.timerCntr); + + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); + + Compass_GetAttitude_EA(EulerAngles); + *algoData_3++ = (float)EulerAngles[ROLL]; + *algoData_3++ = (float)EulerAngles[PITCH]; + *algoData_3++ = (float)EulerAngles[YAW]; + + GetAccelData_mPerSecSq(accels); + *algoData_3++ = (float)accels[X_AXIS]; + *algoData_3++ = (float)accels[Y_AXIS]; + *algoData_3++ = (float)accels[Z_AXIS]; + + GetMagData_G(mags); + *algoData_3++ = (float)mags[X_AXIS]; + *algoData_3++ = (float)mags[Y_AXIS]; + *algoData_3++ = (float)mags[Z_AXIS]; + } + break; + + // place additional user packet preparing calls here + // case USR_OUT_XXXX: + // *payloadLen = YYYY; // total user payload length, including user packet type + // payload[0] = ZZZZ; // user packet type + // prepare dada here + // break; + default: + { + *payloadLen = 0; + ret = FALSE; + } + break; /// unknown user packet, will send error in response + } + + return ret; +} + + +void WriteResultsIntoOutputStream(void *results) +{ +// implement specific data processing/saving here + algo_res = results; +} diff --git a/examples/Compass/src/user/UserMessaging.h b/examples/Compass/src/user/UserMessaging.h new file mode 100644 index 0000000..0aa976e --- /dev/null +++ b/examples/Compass/src/user/UserMessaging.h @@ -0,0 +1,156 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on Jan 25, 2017 + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef USER_MESSAGING_H +#define USER_MESSAGING_H + +#include + +#include "GlobalConstants.h" +#include "ucb_packet_struct.h" + +#define USER_PACKET_OK 0 +#define UNKNOWN_USER_PACKET 1 +#define USER_PACKET_ERROR 2 + +// here is definition for packet rate divider +// considering that data acquisition task runs at 200 Hz +typedef enum { + PACKET_RATE_INVALID = -1, + PACKET_RATE_QUIET = 0, // quiet mode + PACKET_RATE_200HZ = 200, // packet rate 200 Hz + PACKET_RATE_100HZ = 100, // packet rate 100 Hz + PACKET_RATE_50HZ = 50, // packet rate 50 Hz + PACKET_RATE_25HZ = 25, // packet rate 25 Hz + PACKET_RATE_20HZ = 20, // packet rate 20 Hz + PACKET_RATE_10HZ = 10, // packet rate 10 Hz + PACKET_RATE_5HZ = 5, // packet rate 5 Hz + PACKET_RATE_2HZ = 2, // packet rate 2 Hz + PACKET_RATE_1HZ = 1, // packet rate 1 Hz +}packet_rate_t; + + +// User Input packet payload has next structure: +// number offset +// of of first +// parameters parameter +// U2 U2 U4/I4/F +// XXXX YYYY [parameters] +// User input packet codes, change at will +typedef enum { + USR_IN_NONE = 0 , + USR_IN_PING , + USR_IN_UPDATE_CONFIG , + USR_IN_UPDATE_PARAM , + USR_IN_UPDATE_ALL , + USR_IN_SAVE_CONFIG , + USR_IN_RESTORE_DEFAULTS , + USR_IN_GET_CONFIG , + USR_IN_GET_PARAM , + USR_IN_GET_ALL , + USR_IN_GET_VERSION , + USR_IN_RESET , + // add new packet type here, before USR_IN_MAX + USR_IN_MAG_ALIGN , + USR_IN_MAX +} UserInPacketType; + +// User output packet codes, change at will +typedef enum { + USR_OUT_NONE = 0, + USR_OUT_TEST, + USR_OUT_DATA1, +// add new output packet type here, before USR_OUT_MAX + USR_OUT_SCALED1, + USR_OUT_COM1, + USR_OUT_MAX +} UserOutPacketType; + + +// total size of user packet structure should not exceed 255 bytes +#pragma pack(1) +typedef struct { + uint8_t packetPayload[252]; // maximum 252 bytes +}userPacket; +#define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 +#define FIRST_30_PARAMS 0xFFFFFFFF + +// example of user payload structure +typedef struct { + uint32_t numParams; // number of consecutive parameters to update (little endian) + uint32_t paramOffset; // parameter number in parameters structure (little endian) + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}userConfigPayload; + +#pragma pack(1) +// example of user payload structure +typedef struct { + uint32_t paramNum; // parameter number in parameters structure (little endian) + uint64_t parameter; // up to 30 64-bit parameters (little endian) +}userParamPayload; +#pragma pack() + +// example of user payload structure +typedef struct { + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}allUserParamsPayload; + +typedef struct { + uint32_t timer; + float sensorsData[9]; +}data1_payload_t; + + +#pragma pack() + + +#define USR_OUT_TEST_PAYLOAD_LEN (4) // test parameter (uint32_t) +#define USR_OUT_DATA1_PAYLOAD_LEN (4*9) // 3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) +#define USR_OUT_SCALED1_PAYLOAD_LEN (52) +#define USR_OUT_COM1_PAYLOAD_LEN (48) + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +extern int userPacketOut; + +extern int getUserPayloadLength(void); +extern int checkUserPacketType(uint16_t receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); +extern void WriteResultsIntoOutputStream(void *results); +BOOL setUserPacketType(uint8_t* type, BOOL fApply); + +// IMU data structure +typedef struct { + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + double accel_g[3]; + double rate_radPerSec[3]; + double rate_degPerSec[3]; + double mag_G[3]; + double temp_C; +} IMUDataStruct; + +extern IMUDataStruct gIMU; + +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/Compass/src/user/dataProcessingAndPresentation.c b/examples/Compass/src/user/dataProcessingAndPresentation.c new file mode 100644 index 0000000..6a5fb89 --- /dev/null +++ b/examples/Compass/src/user/dataProcessingAndPresentation.c @@ -0,0 +1,187 @@ +/***************************************************************************** + * @file dataProcessingAndPresentation.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * contains of data post-processing framework + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "userAPI.h" +#include "sensorsAPI.h" +#include "gpsAPI.h" +#include "UserMessaging.h" + +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); + +/* * + ****************** + * + Sensors data processing flow (from left to right) + +Raw *************** Filtered * ************ Calibrated **************** ********************* Data +Data * Built-in * Raw Data * * Data * User Filter s* * Algorithm * Output +****** LP Filter ************* Calibration ************* (if desired) ***** (Kalman Filter ******** + * * * * * * * or user Algorithm)* + ************* * ************** **************** ********************* + ^^^^^^^ + Can be selected during + initialization or turned + OFF +*///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> + + +// Next function is common for all platforms, but implementation of the methods inside is platform-dependent +// Call to this function made from DataAcquisitionTask during initialization phase +// All user algorithm and data structures should be initialized here, if used +void initUserDataProcessingEngine() +{ + InitUserAlgorithm(); // default implementation located in file user_algorithm.c +} + + +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +void inertialAndPositionDataProcessing(uint16_t dacqRate) +{ + // + void *results; + + // Increment the IMU timer by the calling rate of the data-acquisition task + _IncrementIMUTimer(dacqRate); + + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + GetBoardTempData(&gIMU.temp_C); + + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // Execute user algorithm (default implementation located in file user_algorithm.c) + results = RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, NULL, dacqRate); + + // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure + WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c +} + + +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ + // Initialize timer variables (used to control the output of the debug + // messages and the IMU timer value) + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the IMU output delta-counter value + gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); + } + + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called + gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the loading of the GPS data into the GPS data structure + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + + case 2: + // + break; + } + } + } +} + + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} diff --git a/examples/Framework/include/README b/examples/Framework/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/examples/Framework/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/examples/Framework/include/taskDataAcquisition.h b/examples/Framework/include/taskDataAcquisition.h index a02b74a..9951af6 100644 --- a/examples/Framework/include/taskDataAcquisition.h +++ b/examples/Framework/include/taskDataAcquisition.h @@ -28,8 +28,6 @@ limitations under the License. #ifndef _TASK_DATA_ACQUISITION_H_ #define _TASK_DATA_ACQUISITION_H_ // Specify the limit used in the int16-limiter -#define INT16_LIMIT 32765 -#define INT12_LIMIT 2045 #include "stdint.h" #include "GlobalConstants.h" extern void TaskDataAcquisition(void const *argument); diff --git a/examples/Framework/ldscripts/stm32f40x.ld1 b/examples/Framework/ldscripts/stm32f40x.ld1 new file mode 100644 index 0000000..1c0c9f6 --- /dev/null +++ b/examples/Framework/ldscripts/stm32f40x.ld1 @@ -0,0 +1,202 @@ +/* +***************************************************************************** +** + +** File : LinkerScript.ld +** +** Abstract : Linker script for STM32F405RGx Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +**

© COPYRIGHT(c) 2014 Ac6

+** +** Redistribution and use in source and binary forms, with or without modification, +** are permitted provided that the following conditions are met: +** 1. Redistributions of source code must retain the above copyright notice, +** this list of conditions and the following disclaimer. +** 2. Redistributions in binary form must reproduce the above copyright notice, +** this list of conditions and the following disclaimer in the documentation +** and/or other materials provided with the distribution. +** 3. Neither the name of Ac6 nor the names of its contributors +** may be used to endorse or promote products derived from this software +** without specific prior written permission. +** +** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = 0x2001ff00; /* end of RAM */ +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x2000; /* required amount of heap */ +_Min_Stack_Size = 0x2000; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ +FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 448K +EEPROM (rw) : ORIGIN = 0x08008000, LENGTH = 16K +USER_EEPROM(rw) : ORIGIN = 0x0800C000, LENGTH = 16K +RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data goes into FLASH */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH + + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + .eeprom 0x08008000: + { + . = ALIGN(4); + KEEP(*(.xbowEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eeeprom = .; /* define a global symbols at end of code */ + } >EEPROM + + .usereeprom 0x0800C000: + { + . = ALIGN(4); + KEEP(*(.userEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eusereeprom = .; /* define a global symbols at end of code */ + } >USER_EEPROM + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/examples/Framework/lib/README b/examples/Framework/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/examples/Framework/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/Framework/platformio.ini b/examples/Framework/platformio.ini index 94354db..20f7c5e 100644 --- a/examples/Framework/platformio.ini +++ b/examples/Framework/platformio.ini @@ -14,15 +14,29 @@ description = OpenIMU application framework. Provides base IMU functionality platform = aceinna_imu lib_archive = false board = OpenIMU300ZA -lib_deps = openimu-core-libraries@^1.0.2 +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps = ..\..\..\openimu-core +lib_deps = openimu-core-libraries@^1.0.22 build_flags = -D CLI + -D ARM_MATH_CM4 + -D __FPU_PRESENT -I include -I include/API -I src/user +; -L ldscripts + -O0 +; -Wno-comment -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 ;upload_protocol = jlink ;debug_tool = jlink + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/Framework/src/main.c b/examples/Framework/src/main.c index d7dfbb2..a2124f8 100644 --- a/examples/Framework/src/main.c +++ b/examples/Framework/src/main.c @@ -84,7 +84,7 @@ void DebugInterfaceInit(void) char status[100]; // Initialize the DEBUG USART (serial) port - InitDebugSerialCommunication( 9600 ); // debug_usart.c + InitDebugSerialCommunication(38400 ); // debug_usart.c DEBUG_STRING("\r\nOpenIMU System\r\n"); // Add a delay to allow the system to stabilize after the reset line (nRst) @@ -129,6 +129,7 @@ void CreateTasks(void) while(1); } cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); #endif } @@ -144,8 +145,7 @@ void CreateTasks(void) ******************************************************************************/ int main(void) { - - + // Initialize processor and board-related signals BoardInit(); @@ -155,7 +155,7 @@ int main(void) // Apply user-chosen configuration userInitConfigureUnit(); - + // Initialize OS and create required tasks CreateTasks(); diff --git a/examples/Framework/src/taskDataAcquisition.c b/examples/Framework/src/taskDataAcquisition.c index a7fe2ed..845fef4 100644 --- a/examples/Framework/src/taskDataAcquisition.c +++ b/examples/Framework/src/taskDataAcquisition.c @@ -169,7 +169,7 @@ void TaskDataAcquisition(void const *argument) MagAlign(); // only does this on align } - if(getUnitCommunicationType() != UART_COMM){ + if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here }else { // Process commands and output continuous packets to UART diff --git a/examples/Framework/src/user/UserConfiguration.c b/examples/Framework/src/user/UserConfiguration.c index a075879..a2fefbb 100644 --- a/examples/Framework/src/user/UserConfiguration.c +++ b/examples/Framework/src/user/UserConfiguration.c @@ -29,6 +29,7 @@ limitations under the License. #include "Indices.h" #include "algorithmAPI.h" #include "platformAPI.h" +#include "magAPI.h" // Default user configuration structure @@ -48,6 +49,12 @@ const UserConfigurationStruct gDefaultUserConfig = { // add default parameter values here, if desired } ; +magAlignUserParams_t magAlignParams = { + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 +}; UserConfigurationStruct gUserConfiguration; UserConfigurationStruct gTmpUserConfiguration; @@ -57,6 +64,22 @@ volatile char *info; BOOL configValid = FALSE; //extern BOOL setUserPacketType(int type, B); +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + magAlignParams.hardIron_X = params->hardIron_X; + magAlignParams.hardIron_Y = params->hardIron_Y; + magAlignParams.softIron_Ratio = params->softIron_Ratio; + magAlignParams.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = magAlignParams.hardIron_X; + params->hardIron_Y = magAlignParams.hardIron_Y; + params->softIron_Ratio = magAlignParams.softIron_Ratio; + params->softIron_Angle = magAlignParams.softIron_Angle; +} + void userInitConfigureUnit() { @@ -99,7 +122,6 @@ void userInitConfigureUnit() } info = getBuildInfo(); - } @@ -160,6 +182,8 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) return result; } + + /** *************************************************************************** * @name UpdateUserParameter - updating user configuration parameter based of preferences * @brief @@ -262,9 +286,9 @@ BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateUserParam * @brief writes user data into user configuration structure, validates data if @@ -306,9 +330,9 @@ BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateAllUserParams * @brief writes user data into user configuration structure, validates data if @@ -381,8 +405,6 @@ BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) } - - /** **************************************************************************** * @name GetUserConfig * @brief Retrieves specified number of user configuration parameters data for @@ -422,6 +444,7 @@ BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetUserParam * @brief Retrieves specified number of user configuration parameters data for @@ -453,6 +476,7 @@ BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetAllUserParams * @brief Retrieves specified number of user configuration parameters data for @@ -505,6 +529,7 @@ BOOL SaveUserConfig(void) } + BOOL RestoreDefaultUserConfig(void) { BOOL valid = TRUE; diff --git a/examples/Framework/src/user/UserConfiguration.h b/examples/Framework/src/user/UserConfiguration.h index 69e5c08..e2d5ee9 100644 --- a/examples/Framework/src/user/UserConfiguration.h +++ b/examples/Framework/src/user/UserConfiguration.h @@ -84,6 +84,8 @@ typedef struct { } UserConfigurationStruct; typedef enum{ +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) USER_CRC = 0, USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters @@ -92,15 +94,14 @@ typedef enum{ USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation -// add system parameters here and reassign USER_LAST_SYSTEM_PARAM USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct USER_MAX_PARAM } UserConfigParamNumber; #define MAX_SYSTEM_PARAM USER_ORIENTATION - extern int userPacketOut; #define USER_OK 0x00 @@ -111,8 +112,6 @@ extern int userPacketOut; #define INVALID_VALUE -2 #define INVALID_PAYLOAD_SIZE -3 - - extern UserConfigurationStruct gUserConfiguration; extern void InitializeUserAlgorithmParams(void); diff --git a/examples/Framework/src/user/UserMessaging.c b/examples/Framework/src/user/UserMessaging.c index 39cf60a..a93c487 100644 --- a/examples/Framework/src/user/UserMessaging.c +++ b/examples/Framework/src/user/UserMessaging.c @@ -56,7 +56,6 @@ usr_packet_t userInputPackets[] = { // }; - // packet codes here should be unique - // should not overlap codes for input packets and system packets // First byte of Packet code should have value >= 0x61 @@ -71,7 +70,6 @@ usr_packet_t userOutputPackets[] = { }; - volatile char *info; static int _userPayloadLen = 0; static int _outputPacketType = USR_OUT_MAX; @@ -112,7 +110,6 @@ int checkUserPacketType(uint16_t receivedCode) void userPacketTypeToBytes(uint8_t bytes[]) { - if(_inputPacketType && _inputPacketType < USR_IN_MAX){ // response to request. Return same packet code bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; @@ -122,7 +119,7 @@ void userPacketTypeToBytes(uint8_t bytes[]) } if(_outputPacketType && _outputPacketType < USR_OUT_MAX){ - // continious packet + // continuous packet bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; }else { @@ -209,8 +206,6 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) // userPacket *pkt = (userPacket *)ptrUcbPacket->payload; /// call appropriate function based on packet type - - switch (_inputPacketType) { case USR_IN_RESET: Reset(); @@ -281,6 +276,7 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) return ret; } + /****************************************************************************** * @name HandleUserOutputPacket - API call ro prepare continuous user output packet * @brief general handler @@ -295,13 +291,15 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) switch (_outputPacketType) { case USR_OUT_TEST: - { uint32_t *testParam = (uint32_t*)(payload); - *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; - *testParam = _testVal++; + { + uint32_t *testParam = (uint32_t*)(payload); + *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; + *testParam = _testVal++; } break; case USR_OUT_DATA1: - { int n = 0; + { + int n = 0; double accels[3]; double mags[3]; double rates[3]; @@ -354,5 +352,5 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) void WriteResultsIntoOutputStream(void *results) { // implement specific data processing/saving here - } + diff --git a/examples/Framework/src/user/UserMessaging.h b/examples/Framework/src/user/UserMessaging.h index 459d745..27e0e72 100644 --- a/examples/Framework/src/user/UserMessaging.h +++ b/examples/Framework/src/user/UserMessaging.h @@ -139,7 +139,6 @@ extern int userPacketOut; #define USER_INVALID 0x81 - extern int userPacketOut; diff --git a/examples/IMU/include/API/userAPI.h b/examples/IMU/include/API/userAPI.h index 986a71f..88b2ad4 100644 --- a/examples/IMU/include/API/userAPI.h +++ b/examples/IMU/include/API/userAPI.h @@ -26,12 +26,25 @@ limitations under the License. #ifndef _USER_API_H #define _USER_API_H +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + #include #include "gpsAPI.h" -void inertialAndPositionDataProcessing(int dacqRate); +void inertialAndPositionDataProcessing(uint16_t dacqRate); -void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, int dacqRate); +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); void WriteResultsIntoOutputStream(void *results) ; void InitUserDataStructures(); void InitUserFilters(); @@ -39,5 +52,4 @@ void InitUserAlgorithm(); void initUserDataProcessingEngine(); void userInitConfigureUnit(); - #endif diff --git a/examples/IMU/include/FreeRTOSConfig.h b/examples/IMU/include/FreeRTOSConfig.h index 690eca8..80ca6b9 100644 --- a/examples/IMU/include/FreeRTOSConfig.h +++ b/examples/IMU/include/FreeRTOSConfig.h @@ -96,7 +96,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES (7) #define configMINIMAL_STACK_SIZE ((uint16_t)2048) -#define configTOTAL_HEAP_SIZE ((size_t)(30 * 2048)) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) #define configMAX_TASK_NAME_LEN (30) #define configUSE_TRACE_FACILITY 1 #define configUSE_16_BIT_TICKS 0 @@ -109,6 +109,7 @@ #define configUSE_APPLICATION_TASK_TAG 0 #define configUSE_COUNTING_SEMAPHORES 1 #define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 /* Co-routine definitions. */ #define configUSE_CO_ROUTINES 0 diff --git a/examples/IMU/include/README b/examples/IMU/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/examples/IMU/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/examples/IMU/include/taskDataAcquisition.h b/examples/IMU/include/taskDataAcquisition.h index a02b74a..f29a4be 100644 --- a/examples/IMU/include/taskDataAcquisition.h +++ b/examples/IMU/include/taskDataAcquisition.h @@ -27,9 +27,7 @@ limitations under the License. #ifndef _TASK_DATA_ACQUISITION_H_ #define _TASK_DATA_ACQUISITION_H_ -// Specify the limit used in the int16-limiter -#define INT16_LIMIT 32765 -#define INT12_LIMIT 2045 + #include "stdint.h" #include "GlobalConstants.h" extern void TaskDataAcquisition(void const *argument); @@ -40,8 +38,4 @@ extern void EnterMainAlgLoop(void); extern void DataAquisitionStart(void); extern BOOL isOneHundredHertzFlag(void); -int16_t LimitInt16Value( int16_t value, int16_t limit ); -extern uint32_t dacqTimer; - - #endif \ No newline at end of file diff --git a/examples/IMU/lib/README b/examples/IMU/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/examples/IMU/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/IMU/platformio.ini b/examples/IMU/platformio.ini index 07f6a68..7beab24 100644 --- a/examples/IMU/platformio.ini +++ b/examples/IMU/platformio.ini @@ -7,7 +7,6 @@ ; ; Please visit documentation for the other options and examples ; http://docs.platformio.org/page/projectconf.html - [platformio] description = An IMU provide calibrated inertial sensor data to the user. @@ -15,14 +14,30 @@ description = An IMU provide calibrated inertial sensor data to the user. platform = aceinna_imu lib_archive = false board = OpenIMU300ZA -lib_deps = openimu-core-libraries@^1.0.2 +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps= ../../../openimu-core +lib_deps = openimu-core-libraries@^1.0.22 build_flags = -D CLI + -D __FPU_PRESENT + -D ARM_MATH_CM4 -I include -I include/API -I src/user + -I lib/CMSIS/include +; -L ldscripts + -O0 +; -Wno-comment -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 ;upload_protocol = jlink ;debug_tool = jlink + + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/IMU/src/main.c b/examples/IMU/src/main.c index 540e459..8837a12 100644 --- a/examples/IMU/src/main.c +++ b/examples/IMU/src/main.c @@ -29,25 +29,24 @@ limitations under the License. #define __MAIN #include + #include "boardAPI.h" +#include "magAPI.h" #include "platformAPI.h" #include "userAPI.h" + #include "debug.h" + #include "taskDataAcquisition.h" #include "taskUserCommunication.h" -#include "magAPI.h" + #include "osapi.h" #include "osresources.h" - - - - #ifdef CLI #include "commandLine.h" #endif - #ifdef GPS #include "gpsAPI.h" #endif @@ -83,23 +82,29 @@ void DebugInterfaceInit(void) { char status[100]; - // Initialize the DEBUG USART (serial) port - InitDebugSerialCommunication(115200); // debug_usart.c - DEBUG_STRING("\r\nOpenIMU System\r\n"); + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + + if(debugChannel == UART_CHANNEL_NONE){ + //nothing to do + return; + } // Add a delay to allow the system to stabilize after the reset line (nRst) // is released - for(int i = 0; i < 400000; i++) ; // TODO - calculate more precisely + for(int i = 0; i < 4000000; i++) ; // TODO - calculate more precisely + //DelayMs(300); //todo - calculate + + // Initialize the DEBUG USART (serial) port + InitDebugSerialCommunication(115200); // debug_usart.c + //DEBUG_STRING("\r\nDMU380 System\r\n"); BoardGetResetStatus(status, sizeof(status)); ERROR_STRING(status); - } void CreateTasks(void) { - osThreadId iD; /// Create RTOS tasks: @@ -109,12 +114,14 @@ void CreateTasks(void) if(iD == NULL){ while(1); } + //user communication task osThreadDef(USER_COMM_TASK, TaskUserCommunication, osPriorityNormal, 0, 2048); iD = osThreadCreate(osThread(USER_COMM_TASK), NULL); if(iD == NULL){ while(1); } + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); @@ -122,6 +129,7 @@ void CreateTasks(void) navDataReadySem = osSemaphoreCreate(osSemaphore(NAV_DATA_READY_SEM), 1); dataAcqSem = osSemaphoreCreate(osSemaphore(DATA_ACQ_SEM), 1); + #ifdef CLI osThreadDef(CLI_TASK, TaskCommandLine, osPriorityLow, 0, 1024); iD = osThreadCreate(osThread(CLI_TASK), NULL); @@ -129,8 +137,8 @@ void CreateTasks(void) while(1); } cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); #endif - } /** *************************************************************************** @@ -144,18 +152,17 @@ void CreateTasks(void) ******************************************************************************/ int main(void) { - - // Initialize processor and board-related signals BoardInit(); + platformSetUnitCommunicationType(UART_COMM); + // Apply factory configuration platformInitConfigureUnit(); // Apply user-chosen configuration userInitConfigureUnit(); - // Initialize OS and create required tasks CreateTasks(); @@ -183,4 +190,4 @@ void vApplicationStackOverflowHook() void vApplicationMallocFailedHook() { while(1); -} \ No newline at end of file +} diff --git a/examples/IMU/src/taskDataAcquisition.c b/examples/IMU/src/taskDataAcquisition.c index a7fe2ed..908163d 100644 --- a/examples/IMU/src/taskDataAcquisition.c +++ b/examples/IMU/src/taskDataAcquisition.c @@ -25,27 +25,18 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#include "taskDataAcquisition.h" -#include "taskUserCommunication.h" -#include "UserConfiguration.h" -#include "magAPI.h" +#include "algorithmAPI.h" #include "bitAPI.h" #include "boardAPI.h" -#include "algorithmAPI.h" -#include "Indices.h" -#include "qmath.h" -#include "osapi.h" -#include "osresources.h" -#include "userAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "userAPI.h" -uint32_t dacqTimer = 0; - -uint32_t getDacqTime() -{ - return dacqTimer; -} +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" +#include "osapi.h" +#include "osresources.h" /** *************************************************************************** * @name TaskDataAcquisition() CALLBACK main loop @@ -57,13 +48,14 @@ uint32_t getDacqTime() ******************************************************************************/ void TaskDataAcquisition(void const *argument) { - // uint8_t functionStatus; - int res, dacqRate; + // + int res; + uint16_t dacqRate; + #pragma GCC diagnostic ignored "-Wunused-but-set-variable" BOOL overRange = FALSE; //uncomment this line if overrange processing required #pragma GCC diagnostic warning "-Wunused-but-set-variable" - // This routine sets up the timer. Can use the structure below this point. TaskDataAcquisition_Init(); // Initialize user algorithm parameters, if needed @@ -72,25 +64,22 @@ void TaskDataAcquisition(void const *argument) InitMagAlignParams(); // Start sensors data acquisition DataAquisitionStart(); - // determine the period of data acquisition task - dacqRate = DACQ_200_HZ; - + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; //************** Add user initialization here, if needed **************** - - /// Data is provided by the primary sensors (accelerometer and rate-sensor) - /// as fast as possible. Data is obtained from secondary sensors at a - /// lower rate. The rate-sensor data read is synced to TIM5 or an - /// external signal. When the read is commanded and in the data buffer, - /// The data-event flag is set and the wait is bypassed. Data is obtained - /// from the buffer, calibrated, filtered and provided to the user for - /// subsequent processing. + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. while( 1 ) { - // ***************************************************************** // NOTE: This task loop runs at 100 or 200 Hz (default 200 Hz) // user can choose period of this task by @@ -104,25 +93,28 @@ void TaskDataAcquisition(void const *argument) if(res != osOK){ // Wait timeout expired. Something wrong wit the dacq system // Process timeout here - } // inform user, that new data set is being prepared (if required) // in case of UART communication interface sets pin IO2 high setIO2Pin (1); - // Get calibrated sensors data - // Inside this function done initial low pass data filtering (second order batterworth - // filter). User can choose cutoff frequency of the filter or turn filtering off. - // Use Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) function to choose - // filter type. Refer to UserConfiguration.c for implementation and to the enumerator structure - // eFilterType in file filter.h for available selections. - // Low pass filtering followed by applying of unit calibration parameters - scaling, temperature - // compensation, bias removal. - // Results are placed in the structure of type double. Pointer to this structure - // is pScaledSensors. Ordering of sensors data in this structure defined by rawSensor_e - // enumerator in the file indices.h - + // Get calibrated sensor data: + // Inside this function the sensor data is filtered by a second-order low-pass + // Butterworth filter, with a cutoff frequency selected by the user (or zero to + // disable). The cutoff is selected using the following: + // + // Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) + // + // Refer to UserConfiguration.c for implementation and to the enumerator structure + // 'eFilterType' in file filter.h for available selections. + // + // Low pass filtering is followed by application of the unit calibration parameters + // - scaling, temperature compensation, bias removal, and misalignment. + // + // Results are placed in the structure of type double. The pointer to this + // structure is 'pScaledSensors'. Ordering of sensors data in this structure + // defined by 'rawSensor_e' enumerator in the file indices.h GetSensorsData(); // Check if sensors data over range @@ -138,21 +130,22 @@ void TaskDataAcquisition(void const *argument) // Temperature - degrees C //****************************************************************** - // BIT status. May have inadvertently changed this during an update. updateBITandStatus(); // ********************** Algorithm ************************ - // In AHRS or INS mode due to CPU performance concerns algorithm better be - // performed at 100 or less Hz based on complexity and number calculations - // involved. Incorporate timing logic inside algorithm function, if desired - // For the initial simplicity use pScaledSensors array as input - // and output of algorithm. - // Use is100Hz variable to determine the rate of this task loop + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. inertialAndPositionDataProcessing(dacqRate); - //Uncomment next line if there is intention of using S0 or S1 xbow packets - //for continuous data output + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output //***************************************************************** // applyNewScaledSensorsData(); //***************************************************************** @@ -163,13 +156,13 @@ void TaskDataAcquisition(void const *argument) setIO2Pin (0); if(platformHasMag() ) { - // Mag Alignment (follows Kalman filter or user algorithm as the innovation routine - // calculates the euler angles and the magnetic vector in the - // NED-frame) + // Mag Alignment (follows Kalman filter or user algorithm as the + // innovation routine calculates the euler angles and the magnetic + // vector in the NED-frame) MagAlign(); // only does this on align } - if(getUnitCommunicationType() != UART_COMM){ + if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here }else { // Process commands and output continuous packets to UART @@ -179,6 +172,3 @@ void TaskDataAcquisition(void const *argument) } } } - - - diff --git a/examples/IMU/src/user/UserAlgorithm.c b/examples/IMU/src/user/UserAlgorithm.c index 600f750..9140437 100644 --- a/examples/IMU/src/user/UserAlgorithm.c +++ b/examples/IMU/src/user/UserAlgorithm.c @@ -23,19 +23,23 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ + +#include "platformAPI.h" #include "userAPI.h" -#include "gpsAPI.h" + #include "stddef.h" -extern void InitializeAlgorithmStruct(void); + +// Initialize GPS algorithm variables void InitUserAlgorithm() { // place additional required initialization here } -void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, int dacqRate) + +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) { // so far no results diff --git a/examples/IMU/src/user/UserAlgorithm.h b/examples/IMU/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/IMU/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/IMU/src/user/UserConfiguration.c b/examples/IMU/src/user/UserConfiguration.c index 251f11f..24f7baa 100644 --- a/examples/IMU/src/user/UserConfiguration.c +++ b/examples/IMU/src/user/UserConfiguration.c @@ -24,12 +24,14 @@ limitations under the License. *******************************************************************************/ #include "string.h" -#include "UserConfiguration.h" -#include "UserMessaging.h" -#include "Indices.h" + #include "algorithmAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "UserConfiguration.h" +#include "UserMessaging.h" +#include "Indices.h" // Default user configuration structure // Saved into EEPROM of first startup after reloading the code @@ -41,21 +43,39 @@ const UserConfigurationStruct gDefaultUserConfig = { .dataSize = sizeof(UserConfigurationStruct), .userUartBaudRate = 115200, .userPacketType = "s1", - .userPacketRate = 100, + .userPacketRate = 10, .lpfAccelFilterFreq = 25, .lpfRateFilterFreq = 25, - .orientation = "+X+Y+Z" + .orientation = "+X+Y+Z", // add default parameter values here, if desired + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 }; - UserConfigurationStruct gUserConfiguration; UserConfigurationStruct gTmpUserConfiguration; uint8_t UserDataBuffer[4096]; volatile char *info; BOOL configValid = FALSE; -//extern BOOL setUserPacketType(int type, B); + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} void userInitConfigureUnit() @@ -68,7 +88,7 @@ void userInitConfigureUnit() while(1); } - if(appStartedFirstTime()){ + if(appStartedFirstTime()) { // comment next line if want to keep previously stored in EEPROM parameters // after rebuilding and/or reloading new application RestoreDefaultUserConfig(); @@ -77,11 +97,11 @@ void userInitConfigureUnit() // Validate checksum of user configuration structure configValid = validateUserConfigInEeprom(&size); - if(configValid == TRUE){ + if(configValid == TRUE) { // Here we have validated User configuration image. // Load it from eeprom into ram on top of the default configuration loadUserConfigFromEeprom((void*)&gUserConfiguration, &size); - }else{ + } else { memset((void*)&gUserConfiguration, 0xff, sizeof(gUserConfiguration)); } @@ -99,7 +119,6 @@ void userInitConfigureUnit() } info = getBuildInfo(); - } @@ -160,6 +179,8 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) return result; } + + /** *************************************************************************** * @name UpdateUserParameter - updating user configuration parameter based of preferences * @brief @@ -262,9 +283,9 @@ BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateUserParam * @brief writes user data into user configuration structure, validates data if @@ -306,9 +327,9 @@ BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateAllUserParams * @brief writes user data into user configuration structure, validates data if @@ -381,8 +402,6 @@ BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) } - - /** **************************************************************************** * @name GetUserConfig * @brief Retrieves specified number of user configuration parameters data for @@ -422,6 +441,7 @@ BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetUserParam * @brief Retrieves specified number of user configuration parameters data for @@ -453,6 +473,7 @@ BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetAllUserParams * @brief Retrieves specified number of user configuration parameters data for @@ -505,6 +526,7 @@ BOOL SaveUserConfig(void) } + BOOL RestoreDefaultUserConfig(void) { BOOL valid = TRUE; diff --git a/examples/IMU/src/user/UserConfiguration.h b/examples/IMU/src/user/UserConfiguration.h index 2f7350d..75bfe49 100644 --- a/examples/IMU/src/user/UserConfiguration.h +++ b/examples/IMU/src/user/UserConfiguration.h @@ -20,14 +20,13 @@ limitations under the License. #ifndef USER_CONFIGURATION_H #define USER_CONFIGURATION_H + #include + #include "GlobalConstants.h" #include "UserMessaging.h" #include "filter.h" - -#include - /// User defined configuration strucrture ///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types @@ -64,26 +63,30 @@ typedef struct { /// 25 - Butterworth LPF 25HZ /// 40 - Butterworth LPF 40HZ - uint8_t orientation[8]; /// unit orientation as string - /// "SFSRSD" - /// Where S is sign (+ or -) - /// F - forward axis (X ot Y or Z) - /// R - right axis (X ot Y or Z) - /// D - down axis (X ot Y or Z) - /// For example "+X+Y+Z" + uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff + /// where dd - down axis, rr - right axis, ff - forward axis + /// next axis values a valid : + /// 'X' (0x58) -> plus X, 'Y' (0x59) -> plus Y, 'Z' (0x5a) -> plus Z + /// 'x' (0x78) -> minus X, 'y' (0x79) -> minus Y, 'z' (0x7a) ->minusZ //*************************************************************************************** // here is the border between arbitrary parameters and platform configuration parameters //*************************************************************************************** // place new arbitrary configuration parameters here - // parameter size should even to 8 bytes + // parameter size should even to 4 bytes // Add parameter offset in UserConfigParamOffset structure if validation or // special processing required + float hardIron_X; + float hardIron_Y; + float softIron_Ratio; + float softIron_Angle; } UserConfigurationStruct; typedef enum{ +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) USER_CRC = 0, USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters @@ -92,15 +95,18 @@ typedef enum{ USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation -// add system parameters here and reassign USER_LAST_SYSTEM_PARAM USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct + USER_HARD_IRON_X , + USER_HARD_IRON_Y , + USER_SOFT_IRON_RATIO , + USER_SOFT_IRON_ANGLE , USER_MAX_PARAM } UserConfigParamNumber; #define MAX_SYSTEM_PARAM USER_ORIENTATION - extern int userPacketOut; #define USER_OK 0x00 diff --git a/examples/IMU/src/user/UserMessaging.c b/examples/IMU/src/user/UserMessaging.c index 71d02ae..78c77d8 100644 --- a/examples/IMU/src/user/UserMessaging.c +++ b/examples/IMU/src/user/UserMessaging.c @@ -26,22 +26,30 @@ limitations under the License. #include #include #include -#include "UserMessaging.h" -#include "UserConfiguration.h" + #include "algorithmAPI.h" -#include "userAPI.h" #include "platformAPI.h" #include "sensorsAPI.h" +#include "userAPI.h" + +#include "UserMessaging.h" +#include "UserConfiguration.h" #include "Indices.h" // For X_AXIS, etc -// provided as example +// Declare the IMU data structure +IMUDataStruct gIMU; + +// Version string char userVersionString[] = "IMU 1.0.0"; + + + /// List of allowed packet codes -usr_packet_t userInputPackets[] = { // - {USR_IN_NONE, {0,0}}, // " " +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, {USR_IN_PING, "pG"}, {USR_IN_UPDATE_CONFIG, "uC"}, {USR_IN_UPDATE_PARAM, "uP"}, @@ -58,7 +66,6 @@ usr_packet_t userInputPackets[] = { // }; - // packet codes here should be unique - // should not overlap codes for input packets and system packets // First byte of Packet code should have value >= 0x61 @@ -73,8 +80,6 @@ usr_packet_t userOutputPackets[] = { {USR_OUT_MAX, {0xff, 0xff}}, // "" }; - - volatile char *info; static int _userPayloadLen = 0; static int _outputPacketType = USR_OUT_MAX; @@ -115,7 +120,6 @@ int checkUserPacketType(uint16_t receivedCode) void userPacketTypeToBytes(uint8_t bytes[]) { - if(_inputPacketType && _inputPacketType < USR_IN_MAX){ // response to request. Return same packet code bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; @@ -216,8 +220,6 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) // userPacket *pkt = (userPacket *)ptrUcbPacket->payload; /// call appropriate function based on packet type - - switch (_inputPacketType) { case USR_IN_RESET: Reset(); @@ -289,9 +291,6 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) } -// Declare the leveler data structure -IMUDataStruct gIMU; - /****************************************************************************** * @name HandleUserOutputPacket - API call ro prepare continuous user output packet * @brief general handler @@ -306,29 +305,32 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) switch (_outputPacketType) { case USR_OUT_TEST: - { uint32_t *testParam = (uint32_t*)(payload); + { + uint32_t *testParam = (uint32_t*)(payload); *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; *testParam = _testVal++; } break; + case USR_OUT_DATA1: - { int n = 0; - double accels[3]; - double mags[3]; - double rates[3]; + { + int n = 0; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; data1_payload_t *pld = (data1_payload_t *)payload; - pld->timer = getDacqTime(); + pld->timer = platformGetDacqTime(); GetAccelData_mPerSecSq(accels); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)accels[i]; } GetRateData_degPerSec(rates); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)rates[i]; } GetMagData_G(mags); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)mags[i]; } *payloadLen = sizeof(data1_payload_t); @@ -336,14 +338,14 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) break; case USR_OUT_DATA2: { - data2_payload_t *pld = (data2_payload_t *)payload; - pld->timer = getDacqTime(); - pld->c = 'A'; - pld->s = 1234; - pld->i = -5; - pld->ll = 1122334455667788LL; - pld->d = 1.23456789; - *payloadLen = sizeof(data2_payload_t); + //data2_payload_t *pld = (data2_payload_t *)payload; + //pld->timer = getDacqTime(); + //pld->c = 'A'; + //pld->s = 1234; + //pld->i = -5; + //pld->ll = 1122334455667788LL; + //pld->d = 1.23456789; + //*payloadLen = sizeof(data2_payload_t); } break; // place additional user packet preparing calls here @@ -393,8 +395,10 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) } break; default: + { *payloadLen = 0; ret = FALSE; + } break; /// unknown user packet, will send error in response } diff --git a/examples/IMU/src/user/UserMessaging.h b/examples/IMU/src/user/UserMessaging.h index 00b5fa4..04928bd 100644 --- a/examples/IMU/src/user/UserMessaging.h +++ b/examples/IMU/src/user/UserMessaging.h @@ -1,6 +1,6 @@ /******************************************************************************* * File: UserConfiguration.h - * Created on JAn 25, 2017 + * Created on Jan 25, 2017 ******************************************************************************/ /******************************************************************************* Copyright 2018 ACEINNA, INC @@ -20,17 +20,16 @@ limitations under the License. #ifndef USER_MESSAGING_H #define USER_MESSAGING_H + #include + #include "GlobalConstants.h" #include "ucb_packet_struct.h" - -#include #define USER_PACKET_OK 0 #define UNKNOWN_USER_PACKET 1 #define USER_PACKET_ERROR 2 - // here is definition for packet rate divider // considering that data acquisition task runs at 200 Hz typedef enum { @@ -45,7 +44,7 @@ typedef enum { PACKET_RATE_5HZ = 5, // packet rate 5 Hz PACKET_RATE_2HZ = 2, // packet rate 2 Hz PACKET_RATE_1HZ = 1, // packet rate 1 Hz -}packet_rate_t; +} packet_rate_t; // User Input packet payload has next structure: @@ -88,7 +87,7 @@ typedef enum { #pragma pack(1) typedef struct { uint8_t packetPayload[252]; // maximum 252 bytes -}userPacket; +} userPacket; #define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 #define FIRST_30_PARAMS 0xFFFFFFFF @@ -134,17 +133,13 @@ typedef struct { #define USR_OUT_DATA1_PAYLOAD_LEN (4*10) // 1 integer +3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) #define USR_OUT_SCALED1_PAYLOAD_LEN (52) // See UserMessaging.c for make-up of Scaled1 message -extern int userPacketOut; #define USER_OK 0x00 #define USER_NAK 0x80 #define USER_INVALID 0x81 - - extern int userPacketOut; - extern int getUserPayloadLength(void); extern int checkUserPacketType(uint16_t receivedCode); extern void userPacketTypeToBytes(uint8_t bytes[]); @@ -158,12 +153,12 @@ typedef struct { // Algorithm states double accel_g[3]; + double rate_radPerSec[3]; double rate_degPerSec[3]; double mag_G[3]; double temp_C; } IMUDataStruct; extern IMUDataStruct gIMU; -#endif /* USER_CONFIGURATION_H */ - +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/IMU/src/user/dataProcessingAndPresentation.c b/examples/IMU/src/user/dataProcessingAndPresentation.c index dc1a0ff..34fd8ca 100644 --- a/examples/IMU/src/user/dataProcessingAndPresentation.c +++ b/examples/IMU/src/user/dataProcessingAndPresentation.c @@ -27,10 +27,15 @@ limitations under the License. #include "userAPI.h" #include "sensorsAPI.h" #include "gpsAPI.h" -#include "gps.h" #include "UserMessaging.h" -#include "bsp.h" // For setIO3Pin +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); /* * ****************** @@ -49,7 +54,6 @@ Data * Built-in * Raw Data * * Data * User Filter s* * Al *///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> - // Next function is common for all platforms, but implementation of the methods inside is platform-dependent // Call to this function made from DataAcquisitionTask during initialization phase // All user algorithm and data structures should be initialized here, if used @@ -59,74 +63,121 @@ void initUserDataProcessingEngine() } -#include "Indices.h" // For X_AXIS and Z_AXIS -#include "debug.h" // For debug commands - -// Next function is common for all platforms, but implementation of the methods inside is platform and user-dependent -// Call to this function made from DataAcquisitionTask after retrieving samples of current sensors data and -// applying corresponding calibration -// dacqRate is rate with which new set of sensors data arrives -void inertialAndPositionDataProcessing(int dacqRate) +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +void inertialAndPositionDataProcessing(uint16_t dacqRate) { - // Define variables to hold sensor data - gpsDataStruct_t gps; + // Increment the IMU timer by the calling rate of the data-acquisition task + _IncrementIMUTimer(dacqRate); - // Initialization variable - static int initFlag = 1; + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + GetBoardTempData(&gIMU.temp_C); + + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); +} - // Variables that control the output frequency of the debug statement - static int debugFlag = 0; - static uint8_t debugOutputCntr, debugOutputCntrLimit; +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ // Initialize timer variables (used to control the output of the debug // messages and the IMU timer value) + static int initFlag = 1; if(initFlag) { // Reset 'initFlag' so this section is not executed more than once. initFlag = 0; - // Set the variables that control the debug-message output-rate (based on - // the desired calling frequency of the debug output) - debugOutputCntr = 0; - - uint16_t debugOutputFreq = 4; // [Hz] - debugOutputCntrLimit = dacqRate / debugOutputFreq; - // Set the IMU output delta-counter value gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); } - // Increment the timer counter by the sampling period equivalent to the rate at which - // inertialAndPositionDataProcessing is called + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} - // Obtain accelerometer data - GetAccelData_g(gIMU.accel_g); - - // Obtain rate-sensor data - GetRateData_degPerSec(gIMU.rate_degPerSec); - - // Obtain magnetometer data - GetMagData_G(gIMU.mag_G); - // Obtain board temperature data - GetBoardTempData(&gIMU.temp_C); +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; - // Obtain GPS data - GetGPSData(&gps); + // Check debug flag. If set then generate the debug message to verify + // the loading of the GPS data into the GPS data structure + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } - // Generate the debug output to test the loading of the sensor data - // into the IMU data structure - if( debugFlag ) { debugOutputCntr++; if(debugOutputCntr >= debugOutputCntrLimit) { debugOutputCntr = 0; - DebugPrintFloat("Time: ", 0.001 * gIMU.timerCntr, 3); - DebugPrintFloat(", AccelZ: ", gIMU.accel_g[Z_AXIS], 3); - DebugPrintFloat(", RateZ: ", gIMU.rate_degPerSec[Z_AXIS], 3); - DebugPrintFloat(", MagX: ", gIMU.mag_G[X_AXIS], 3); - DebugPrintFloat(", Temp: ", gIMU.temp_C,2); - DebugPrintEndline(); + + // Reset 'new GPS data' flag (this should be done in UpdateFunctions + // to ensure the EKF can use the data) + //gGPS.updateFlag = 0; <-- This would make a difference as the input + // to the algorithm isn't set yet. + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + + case 2: + // Other messages go here + break; + } } } } + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} diff --git a/examples/INS/.gitignore b/examples/INS/.gitignore new file mode 100644 index 0000000..04186a1 --- /dev/null +++ b/examples/INS/.gitignore @@ -0,0 +1,11 @@ +.pioenvs +.piolibdeps +.vscode +*.map +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/*.db +.vscode/.browse.c_cpp.db* +.settings +.cproject +.project diff --git a/examples/INS/.travis.yml b/examples/INS/.travis.yml new file mode 100644 index 0000000..52072ef --- /dev/null +++ b/examples/INS/.travis.yml @@ -0,0 +1,55 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/examples/INS/include/API/userAPI.h b/examples/INS/include/API/userAPI.h new file mode 100644 index 0000000..88b2ad4 --- /dev/null +++ b/examples/INS/include/API/userAPI.h @@ -0,0 +1,55 @@ +/** ****************************************************************************** + * @file userAPI.h API functions for Interfacing with user algorithm + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef _USER_API_H +#define _USER_API_H + +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + +#include +#include "gpsAPI.h" + +void inertialAndPositionDataProcessing(uint16_t dacqRate); + +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); +void WriteResultsIntoOutputStream(void *results) ; +void InitUserDataStructures(); +void InitUserFilters(); +void InitUserAlgorithm(); +void initUserDataProcessingEngine(); +void userInitConfigureUnit(); + +#endif diff --git a/examples/INS/include/FreeRTOSConfig.h b/examples/INS/include/FreeRTOSConfig.h new file mode 100644 index 0000000..80ca6b9 --- /dev/null +++ b/examples/INS/include/FreeRTOSConfig.h @@ -0,0 +1,180 @@ +/* + FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. + + *************************************************************************** + >>! NOTE: The modification to the GPL is included to allow you to !<< + >>! distribute a combined work that includes FreeRTOS without being !<< + >>! obliged to provide the source code for proprietary components !<< + >>! outside of the FreeRTOS kernel. !<< + *************************************************************************** + + FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY + WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html. + *----------------------------------------------------------*/ + +/* Ensure stdint is only used by the compiler, and not the assembler. */ +#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__) + #include + extern uint32_t SystemCoreClock; +#endif + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ (SystemCoreClock) +#define configTICK_RATE_HZ ((TickType_t)1000) +#define configMAX_PRIORITIES (7) +#define configMINIMAL_STACK_SIZE ((uint16_t)2048) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) +#define configMAX_TASK_NAME_LEN (30) +#define configUSE_TRACE_FACILITY 1 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 1 +#define configUSE_MUTEXES 1 +#define configQUEUE_REGISTRY_SIZE 8 +#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configUSE_APPLICATION_TASK_TAG 0 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Software timer definitions. */ +#define configUSE_TIMERS 0 +#define configTIMER_TASK_PRIORITY (2) +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH (configMINIMAL_STACK_SIZE * 2) + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 0 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 + +/* Cortex-M specific definitions. */ +#ifdef __NVIC_PRIO_BITS + /* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */ + #define configPRIO_BITS __NVIC_PRIO_BITS +#else + #define configPRIO_BITS 4 /* 15 priority levels */ +#endif + +/* The lowest interrupt priority that can be used in a call to a "set priority" +function. */ +#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 0xf + +/* The highest interrupt priority that can be used by any interrupt service +routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL +INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER +PRIORITY THAN THIS! (higher priorities are lower numeric values. */ +#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 1 + + +//#define configKERNEL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 15, or 255 as only the top four bits are implemented. This is the lowest priority. */ +//#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 5, or 80 as only the top four bits are implemented. */ + + +/* Interrupt priorities used by the kernel port layer itself. These are generic +to all Cortex-M ports, and do not rely on any particular library functions. */ +#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) +/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!! +See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */ +#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) + +/* Normal assert() semantics without relying on the provision of an assert.h +header file. */ +#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); for( ;; ); } + +/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS + standard names. */ +#define vPortSVCHandler SVC_Handler +#define xPortPendSVHandler PendSV_Handler + +/* IMPORTANT: This define MUST be commented when used with STM32Cube firmware, + to prevent overwriting SysTick_Handler defined within STM32Cube HAL */ +//#define xPortSysTickHandler SysTick_Handler +#define osSystickHandler SysTick_Handler + +#endif /* FREERTOS_CONFIG_H */ + diff --git a/examples/INS/include/osresources.h b/examples/INS/include/osresources.h new file mode 100644 index 0000000..47ac800 --- /dev/null +++ b/examples/INS/include/osresources.h @@ -0,0 +1,42 @@ +#ifndef _OS_RES_H +#define _OS_RES_H +#include "cmsis_os.h" +#include "portmacro.h" + + +#ifdef __MAIN + +osSemaphoreDef(GYRO_READY_SEM); +osSemaphoreDef(ACCEL_READY_SEM); +osSemaphoreDef(MAG_READY_SEM); +osSemaphoreDef(TEMP_READY_SEM); +osSemaphoreDef(NAV_DATA_READY_SEM); +osSemaphoreDef(CLI_READY_SEM); +osSemaphoreDef(DATA_ACQ_SEM); +osSemaphoreDef(CAN_DATA_SEM); +osSemaphoreDef(CLI_SEM); +osSemaphoreId gyroReadySem; +osSemaphoreId accelReadySem; +osSemaphoreId magReadySem; +osSemaphoreId tempReadySem; +osSemaphoreId navDataReadySem; +osSemaphoreId cliReadySem; +osSemaphoreId dataAcqSem; +osSemaphoreId canDataSem; +osSemaphoreId cliSem; + +#else + +extern osSemaphoreId gyroReadySem; +extern osSemaphoreId accelReadySem; +extern osSemaphoreId magReadySem; +extern osSemaphoreId tempReadySem; +extern osSemaphoreId navDataReadySem; +extern osSemaphoreId dataAcqSem; +extern osSemaphoreId canDataSem; +extern osSemaphoreId cliSem; + +#endif + + +#endif diff --git a/examples/INS/include/taskDataAcquisition.h b/examples/INS/include/taskDataAcquisition.h new file mode 100644 index 0000000..891fb4e --- /dev/null +++ b/examples/INS/include/taskDataAcquisition.h @@ -0,0 +1,41 @@ +/** *************************************************************************** + * @file taskDataAcquisition.h + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef _TASK_DATA_ACQUISITION_H_ +#define _TASK_DATA_ACQUISITION_H_ + +#include "stdint.h" +#include "GlobalConstants.h" +extern void TaskDataAcquisition(void const *argument); +extern void PrepareToNewDacqTick(); +extern void TaskDataAcquisition_Init(void); +extern void GetSensorsData(void); +extern void EnterMainAlgLoop(void); +extern void DataAquisitionStart(void); +extern BOOL isOneHundredHertzFlag(void); + +#endif diff --git a/examples/INS/ldscripts/stm32f40x.ld b/examples/INS/ldscripts/stm32f40x.ld new file mode 100644 index 0000000..1c0c9f6 --- /dev/null +++ b/examples/INS/ldscripts/stm32f40x.ld @@ -0,0 +1,202 @@ +/* +***************************************************************************** +** + +** File : LinkerScript.ld +** +** Abstract : Linker script for STM32F405RGx Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +**

© COPYRIGHT(c) 2014 Ac6

+** +** Redistribution and use in source and binary forms, with or without modification, +** are permitted provided that the following conditions are met: +** 1. Redistributions of source code must retain the above copyright notice, +** this list of conditions and the following disclaimer. +** 2. Redistributions in binary form must reproduce the above copyright notice, +** this list of conditions and the following disclaimer in the documentation +** and/or other materials provided with the distribution. +** 3. Neither the name of Ac6 nor the names of its contributors +** may be used to endorse or promote products derived from this software +** without specific prior written permission. +** +** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = 0x2001ff00; /* end of RAM */ +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x2000; /* required amount of heap */ +_Min_Stack_Size = 0x2000; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ +FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 448K +EEPROM (rw) : ORIGIN = 0x08008000, LENGTH = 16K +USER_EEPROM(rw) : ORIGIN = 0x0800C000, LENGTH = 16K +RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data goes into FLASH */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH + + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + .eeprom 0x08008000: + { + . = ALIGN(4); + KEEP(*(.xbowEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eeeprom = .; /* define a global symbols at end of code */ + } >EEPROM + + .usereeprom 0x0800C000: + { + . = ALIGN(4); + KEEP(*(.userEeprom)) /* keep my variable even if not referenced */ + . = ALIGN(4); + _eusereeprom = .; /* define a global symbols at end of code */ + } >USER_EEPROM + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/examples/INS/lib/CLI/include/commandLine.h b/examples/INS/lib/CLI/include/commandLine.h new file mode 100644 index 0000000..33535f0 --- /dev/null +++ b/examples/INS/lib/CLI/include/commandLine.h @@ -0,0 +1,54 @@ +/** *************************************************************************** + * @file commandLine.h DEBUG parser functions + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef COMMAND_LINE_H +#define COMMAND_LINE_H + +#include + + +typedef void(*tShellCallback)(uint32_t); + +/// command token, callback and help table +typedef struct { + char const *name; + tShellCallback callback; + uint32_t callbackData; + char const *help; +} tCommand; + +extern const tCommand gCommands[]; + +void CmdPrintPrompt(); +void CmdLineLookup(tCommand const *cmd_table); +int CmdLineGetArgString(uint8_t **s); +int CmdLineGetArgInt(int32_t *i); +int CmdLineGetArgUInt(uint32_t *i); +int CmdLineGetArgFloat(float *f); +void CmdLineExec(char const *cmdline, const tCommand *cmd_table); +void TaskCommandLine(void const *argument); + + +#endif //COMMAND_LINE_H \ No newline at end of file diff --git a/examples/INS/lib/CLI/include/commandTable.h b/examples/INS/lib/CLI/include/commandTable.h new file mode 100644 index 0000000..14a2ee0 --- /dev/null +++ b/examples/INS/lib/CLI/include/commandTable.h @@ -0,0 +1,44 @@ +/** *************************************************************************** + * @file commandTable.h table of commands, calbacks, and help strings + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Table of commands available to be sent from the commandLine.c shell + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + + +//#include "commandLine.h" +#include "commands.h" + +#define COMMAND_TABLE_END {"",0,0,""} + +//Command table +// {char *name, tShellCallback callback, uint32_t callbackData, const *help } +const tCommand gCommands[] = +{ + {"ver", &CmdVersion, 0, "Display firmware version"}, + {"raccel", &CmdReadAccelerometer, 0, "Read accelerometer"}, + {"rgyro", &CmdReadGyro, 0, "Read Gyro"}, + {"rmag", &CmdReadMagnetometer, 0, "Read magnetometer"}, +// {"rGPS", &CmdGpsRead, 0, "Read current GPS value" }, + COMMAND_TABLE_END //MUST BE LAST!!! +}; + diff --git a/examples/INS/lib/CLI/include/commands.h b/examples/INS/lib/CLI/include/commands.h new file mode 100644 index 0000000..3c4e563 --- /dev/null +++ b/examples/INS/lib/CLI/include/commands.h @@ -0,0 +1,33 @@ +/****************************************************************************** + * @file commands.h +*******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef COMMANDS_H +#define _COMMANDS_H + +void CmdVersion(uint32_t data); +void CmdUsartBaudRate( uint32_t data ); +void CmdReadAccelerometer(uint32_t data); +void CmdReadAccelTemp(uint32_t data); +void CmdReadGyro(uint32_t data); +void CmdReadGyroTemp(uint32_t data); +void CmdGpsRead(uint32_t data); +void CmdReadMagnetometer(uint32_t data); + + +#endif /* COMMANDS_H */ \ No newline at end of file diff --git a/examples/INS/lib/CLI/src/commandLine.c b/examples/INS/lib/CLI/src/commandLine.c new file mode 100644 index 0000000..3e0bf34 --- /dev/null +++ b/examples/INS/lib/CLI/src/commandLine.c @@ -0,0 +1,250 @@ +/** *************************************************************************** + * @file commandLine.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Simple interactive serial console shell - line input + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include +#include +#include +#include "commandLine.h" +#include "debug.h" +#include "debug_usart.h" +#include "utilities.h" +#include "osapi.h" +#include "osresources.h" + +static void _ExecLine(tCommand const *cmd_table); +static void _Dispatch(char const *token, tCommand const *table); +static void _CmdHelp(tCommand const *table); + +static char gCmdLine[80]; +static char *gCmdLineIndex; +#define PARSE_SEPARATOR ' ' + +/** *************************************************************************** + * @name CmdPrintPrompt() + * @brief send prompt characters out serial to the debug serial console + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void CmdPrintPrompt() +{ + static uint16_t lineno; + DebugPrintInt("", lineno); + lineno++; + DebugPrintString("> "); +} + +/** *************************************************************************** + * @name CmdLineLookup() + * @brief at startup call send prompt characters out serial to console + * the debug serial + * + * @param [in] cmd_table - table entry to parse + * @param [in] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void CmdLineLookup(tCommand const *cmd_table) +{ + static uint32_t index = 0; + if (DebugSerialReadLine((uint8_t*) gCmdLine, &index, 80)) { + strrep(gCmdLine, '\r', 0); + strrep(gCmdLine, '\n', 0); + if (gCmdLine[0]) {//don't process empty string + //Ignore lines that start with # character + if (gCmdLine[0] != '#') { + _ExecLine(cmd_table); + } + } + CmdPrintPrompt(); + } // else wait until prompt is ready +} + +/** *************************************************************************** + * @name _ExecLine() LOCAL + * @brief extract tokens from strings with ' ' as the delimiter + * + * @param [in] cmd_table - table entry to parse + * @param [in] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +static void _ExecLine(tCommand const *cmd_table) { + char *token = strtok_r1(gCmdLine, PARSE_SEPARATOR, &gCmdLineIndex); + _Dispatch(token, cmd_table); +} + +void CmdLinexec(char const *cmdline, + tCommand const *cmd_table) { + char *dst; + if (cmdline && *cmdline) { + dst = gCmdLine; + while ((*dst++ = *cmdline++)){;} + _ExecLine(cmd_table); + } +} + +/** *************************************************************************** + * @name CmdLineGetArgString() + * @brief get string from command line arguments + * + * @param [out] s - pointer to a string + * @retval N/A + ******************************************************************************/ +int CmdLineGetArgString(uint8_t **s) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (s) { + *s = (uint8_t*)token; + } + return 1; + } else { + return 0; + } +} + +int CmdLineGetArgInt(int32_t *i) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (i) { + *i = atoi(token); + } + return 1; + } else { + return 0; + } +} + +/** *************************************************************************** + * @name CmdLineGetArgUInt() + * @brief get unsigned integer from command line arguments + * + * @param [out] t - ingteger to return + * @retval N/A + ******************************************************************************/ +int CmdLineGetArgUInt(uint32_t *i) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (i) { + *i = atoi(token); + } + return 1; + } else { + return 0; + } +} + +int CmdLineGetArgFloat(float *f) { + char *token = strtok_r1(0, PARSE_SEPARATOR, &gCmdLineIndex); + if (token) { + if (f) { + *f = atof(token); + } + return 1; + } else { + return 0; + } +} + + +/** *************************************************************************** + * @name _Dispatch() + * @brief Traverses a command table and dispatches the appropriate callback + * + * @param [in] token - command text from the console + * @param [in] table - command table + * @retval N/A + ******************************************************************************/ +static void _Dispatch(char const *token, + tCommand const *table) +{ + tCommand *cmd = (tCommand*) table; + if (0 == strcmpi(token, "help")) { + _CmdHelp(table); + } else { + while (cmd->callback != 0) { + if (0 == strcmpi(token, cmd->name)) { + (*(cmd->callback))(cmd->callbackData); + return; + } else { + cmd++; + } + } + //If we get here, the command wasn't found + DebugPrintString("Unknown command ("); + DebugPrintString(token); + DebugPrintString(") Type 'help' for list.\r\n"); + } +} + +/** *************************************************************************** + * @name _CmdHelp() LOCAL + * @brief traverses command table and prints the help strings + * + * @param [in] token - command text from the console + * @param [in] table - command table + * @retval N/A + ******************************************************************************/ +static void _CmdHelp(tCommand const *table) { + char command[30]; + + tCommand *cmd = (tCommand*) table; + + while (cmd->callback != 0) { + DebugPrintString(" "); + sprintf(command, "%-10s", cmd->name); +// DebugPrintString(cmd->name); + DebugPrintString(command); +// DebugPrintString(" "); + DebugPrintString(cmd->help); + DebugPrintEndline(); + DebugPrintEndline(); + cmd++; + while (!IsDebugSerialIdle()) + {/*spin*/;} + } +} + +/** **************************************************************************** + * @name TaskCommandLine() LOCAL task callback function + * @brief handles debug serial console command line messaging + * When the USART interrupt handler indicates a RX on the USART line, + * toggle the LED and parse the received message using the commandTable + * + * @param [in] gCommands - command line command and arguments + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void TaskCommandLine(void const *argument) +{ + CmdPrintPrompt(); // out to serial console + + while (1) { + /// context switch and wait for the semaphore, BINSEM_SERIAL_RX, to + /// change state: occurs in the USART interrupt handler (DEBUG_USART_IRQ) + osSemaphoreWait( cliSem, OS_INFINITE_TIMEOUT ); + CmdLineLookup( gCommands ); // commandLine.c + } +} diff --git a/examples/INS/lib/CLI/src/commands.c b/examples/INS/lib/CLI/src/commands.c new file mode 100644 index 0000000..915b60c --- /dev/null +++ b/examples/INS/lib/CLI/src/commands.c @@ -0,0 +1,126 @@ +/** *************************************************************************** + * @file commands.c callback functions from the commandTable + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * Commands available to the commandLine.c shell + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + + +#include +#include +#include +#include "commandLine.h" +#include "osapi.h" +#include "sensorsAPI.h" +#include "platformAPI.h" + + +#define LOGGING_LEVEL LEVEL_INFO +#include "debug.h" + +char str[100]; +// Display firmware version +void CmdVersion(uint32_t data) +{ + uint8_t *model = (uint8_t*)unitVersionString(); + uint8_t *rev = (uint8_t*)platformBuildInfo(); + unsigned int serialNum = unitSerialNumber(); + snprintf(str, sizeof(str), "%s %s SN:%u", model, rev, serialNum ); + + DEBUG_STRING(str); + DEBUG_ENDLINE(); +} // End of display firmware version + +/** *************************************************************************** + * @name CmdReadAccelerometer() Read accelerometer + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadAccelerometer(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetAccelData_mPerSecSq(readings); + OSEnableHook(); + + DEBUG_STRING("Reading accelerometer data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + +/** *************************************************************************** + * @name CmdReadGyro() Read gyro data + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadGyro(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetRateData_degPerSec(readings); + OSEnableHook(); + + DEBUG_STRING("Reading gyro data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + +/** *************************************************************************** + * @name CmdReadMagnetometer. Read magnetometer data + * @brief + * + * @retval N/A + ******************************************************************************/ +void CmdReadMagnetometer(uint32_t readInGs) +{ + double readings[3]; + + OSDisableHook(); + GetMagData_G(readings); + OSEnableHook(); + + DEBUG_STRING("Reading magnetometer data\r\n"); + sprintf(str, " X = %3.5lf\r\n", readings[0]); + DEBUG_STRING(str); + sprintf(str, " Y = %3.5lf\r\n", readings[1]); + DEBUG_STRING(str); + sprintf(str, " Z = %3.5lf\r\n", readings[2]); + DEBUG_STRING(str); + +} + + +#include "commandTable.h" diff --git a/examples/INS/lib/readme.txt b/examples/INS/lib/readme.txt new file mode 100644 index 0000000..131f1bf --- /dev/null +++ b/examples/INS/lib/readme.txt @@ -0,0 +1,41 @@ + +This directory is intended for the project specific (private) libraries. +PlatformIO will compile them to static libraries and link to executable file. + +The source code of each library should be placed in separate directory, like +"lib/private_lib/[here are source files]". + +For example, see how can be organized `Foo` and `Bar` libraries: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) http://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- readme.txt --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- http://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/INS/platformio.ini b/examples/INS/platformio.ini new file mode 100644 index 0000000..974015f --- /dev/null +++ b/examples/INS/platformio.ini @@ -0,0 +1,49 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; http://docs.platformio.org/page/projectconf.html +[platformio] +description = + A Kalman filter based algorithm that uses rate-sensors to propagate attitude + (roll, pitch, and heading angles) forward in time and accelerometers and + magnetometers as references, to correct for bias in the rate-sensor signal. + Additionally, the algorithm uses accelerometer data to propagate velocity and + position and velocity (in the North/East/Down-Frame) and GPS data to correct + for errors and estimate bias in the accelerometer signals. + +[env:OpenIMU300ZA] +platform = aceinna_imu +lib_archive = false +board = OpenIMU300ZA +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps = ../../../openimu-core +lib_deps = openimu-core-libraries@^1.0.22 +build_flags = + -D CLI + -D GPS + -D __FPU_PRESENT + -D ARM_MATH_CM4 + -I include + -I include/API + -I src/user +; -L ldscripts + -O0 +; -Wno-comment + -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld + -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 + +;upload_protocol = jlink +;debug_tool = jlink + + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/INS/src/main.c b/examples/INS/src/main.c new file mode 100644 index 0000000..db370fd --- /dev/null +++ b/examples/INS/src/main.c @@ -0,0 +1,228 @@ +/** *************************************************************************** + * @file main.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * main is the center of the system universe, it all starts here. And never ends. + * entry point for system (pins, clocks, interrupts), data and task initialization. + * contains the main processing loop. - this is a standard implementation + * which has mainly os functionality in the main loop + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ +#define __MAIN + +#include + +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "debug.h" + +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" + +#include "osapi.h" +#include "osresources.h" + +#ifdef CLI +#include "commandLine.h" +#endif + +#ifdef GPS +#include "gpsAPI.h" +#endif + +#ifdef MEMSIC_CAN +#include "canAPI.h" +#endif + +char buildInfo[] = {__DATE__"," __TIME__}; + +/** *************************************************************************** + * @name getBuildInfo - provides the pointer to the build date and time string + * @brief + * + * @param [in] N/A + * @param [out] pointer to build info string + * @retval N/A + ******************************************************************************/ +char *getBuildInfo() +{ + return buildInfo; +} + +/** *************************************************************************** + * @name initDebugInterface Debug serial interface initialization + * @brief Initializes debug serial console messaging support + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +void DebugInterfaceInit(void) +{ + char status[100]; + + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + + if(debugChannel == UART_CHANNEL_NONE){ + //nothing to do + return; + } + + // Add a delay to allow the system to stabilize after the reset line (nRst) + // is released + for(int i = 0; i < 4000000; i++) ; // TODO - calculate more precisely + //DelayMs(300); //todo - calculate + + // Initialize the DEBUG USART (serial) port + InitDebugSerialCommunication(230400); // debug_usart.c + //DEBUG_STRING("\r\nDMU380 System\r\n"); + + BoardGetResetStatus(status, sizeof(status)); + + ERROR_STRING(status); +} + +void CreateTasks(void) +{ + osThreadId iD; + + /// Create RTOS tasks: + // dacq task + osThreadDef(DACQ_TASK, TaskDataAcquisition, osPriorityAboveNormal, 0, 2048); + iD = osThreadCreate(osThread(DACQ_TASK), NULL); + if(iD == NULL){ + while(1); + } + + //user communication task + osThreadDef(USER_COMM_TASK, TaskUserCommunication, osPriorityNormal, 0, 2048); + iD = osThreadCreate(osThread(USER_COMM_TASK), NULL); + if(iD == NULL){ + while(1); + } + + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); + accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); + magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); + tempReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); + navDataReadySem = osSemaphoreCreate(osSemaphore(NAV_DATA_READY_SEM), 1); + dataAcqSem = osSemaphoreCreate(osSemaphore(DATA_ACQ_SEM), 1); + +#ifdef GPS + osThreadDef(GPS_TASK, TaskGps, osPriorityBelowNormal, 0, 1024); + iD = osThreadCreate(osThread(GPS_TASK), NULL); + if(iD == NULL){ + while(1); + } + osThreadDef(WMM_TASK, TaskWorldMagneticModel, osPriorityLow, 0, 512); + iD = osThreadCreate(osThread(WMM_TASK), NULL); + if(iD == NULL){ + while(1); + } +#endif + +#ifdef CLI + osThreadDef(CLI_TASK, TaskCommandLine, osPriorityLow, 0, 1024); + iD = osThreadCreate(osThread(CLI_TASK), NULL); + if(iD == NULL){ + while(1); + } + cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); +#endif +} + +/** *************************************************************************** + * @name main() The DMU380 firmware initialization and entry point + * @brief creates tasks for data collection SPI1 and user communications SPI3 + * or USART serial stream (Nav-View) debug serial console messaging + * + * @param [in] N/A + * @param [out] "#> " out serial to console + * @retval N/A + ******************************************************************************/ +int main(void) +{ + // Initialize processor and board-related signals + BoardInit(); + + platformSetUnitCommunicationType(UART_COMM); + + // Un-assign and reassign ports +#ifdef GPS + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_1); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid +#else + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid + //res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_1); + //while(!res){}; // check if valid +#endif + + // Apply factory configuration + platformInitConfigureUnit(); + + // Apply user-chosen configuration + userInitConfigureUnit(); + + // Initialize OS and create required tasks + CreateTasks(); + + // Initialize the DEBUG USART (serial) port +#ifndef GPS_ON_DEBUG_PORT + DebugInterfaceInit(); +#endif + + //InitTimer_Watchdog( ENABLE ); + + // Start Running the tasks... + // Start scheduler + osKernelStart(); + + // We should never get here as control is now taken by the scheduler + for (;;); + +} + + +void vApplicationStackOverflowHook() +{ + while(1); +} +void vApplicationMallocFailedHook() +{ + while(1); +} diff --git a/examples/INS/src/taskDataAcquisition.c b/examples/INS/src/taskDataAcquisition.c new file mode 100644 index 0000000..ecbf729 --- /dev/null +++ b/examples/INS/src/taskDataAcquisition.c @@ -0,0 +1,174 @@ +/***************************************************************************** + * @file taskDataAcquisition.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "algorithmAPI.h" +#include "bitAPI.h" +#include "boardAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" + +#include "osapi.h" +#include "osresources.h" + +/** *************************************************************************** + * @name TaskDataAcquisition() CALLBACK main loop + * @brief Get the sensor data at the specified frequency (based on the + * configuration of the accelerometer rate-sensor). Process and provide + * information to the user via the UART or SPI. + * @param N/A + * @retval N/A + ******************************************************************************/ +void TaskDataAcquisition(void const *argument) +{ + // + int res; + uint16_t dacqRate; + +#pragma GCC diagnostic ignored "-Wunused-but-set-variable" + BOOL overRange = FALSE; //uncomment this line if overrange processing required +#pragma GCC diagnostic warning "-Wunused-but-set-variable" + + // This routine sets up the timer. Can use the structure below this point. + TaskDataAcquisition_Init(); + // Initialize user algorithm parameters, if needed + initUserDataProcessingEngine(); + // set the mag align scale factors + InitMagAlignParams(); + // Start sensors data acquisition + DataAquisitionStart(); + + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; + + //************** Add user initialization here, if needed **************** + + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. + + while( 1 ) + { + // ***************************************************************** + // NOTE: This task loop runs at 100 or 200 Hz (default 200 Hz) + // user can choose period of this task by + // ***************************************************************** + // Handle Timing vard, watchdog and BIT + PrepareToNewDacqTick(); + // Wait for next tick + // Upon timeout of TIM5 (or user sync), let the process continue + + res = osSemaphoreWait(dataAcqSem, 1000); + if(res != osOK){ + // Wait timeout expired. Something wrong wit the dacq system + // Process timeout here + } + + // inform user, that new data set is being prepared (if required) + // in case of UART communication interface sets pin IO2 high + setIO2Pin (1); + + // Get calibrated sensor data: + // Inside this function the sensor data is filtered by a second-order low-pass + // Butterworth filter, with a cutoff frequency selected by the user (or zero to + // disable). The cutoff is selected using the following: + // + // Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) + // + // Refer to UserConfiguration.c for implementation and to the enumerator structure + // 'eFilterType' in file filter.h for available selections. + // + // Low pass filtering is followed by application of the unit calibration parameters + // - scaling, temperature compensation, bias removal, and misalignment. + // + // Results are placed in the structure of type double. The pointer to this + // structure is 'pScaledSensors'. Ordering of sensors data in this structure + // defined by 'rawSensor_e' enumerator in the file indices.h + GetSensorsData(); + + // Check if sensors data over range + // If overRange is TRUE - limit values are put into sensors data based + // on chosen sensors sensitivity + overRange = handleOverRange(); + + // ***************************************************************** + // At this point sensors data is in next units + // Acceleration - g's + // Rates - rad/s + // Magnetometer - Gauss + // Temperature - degrees C + //****************************************************************** + + // BIT status. May have inadvertently changed this during an update. + updateBITandStatus(); + + // ********************** Algorithm ************************ + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. + inertialAndPositionDataProcessing(dacqRate); + + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output + //***************************************************************** + // applyNewScaledSensorsData(); + //***************************************************************** + + // Inform user, that new data set is ready (if required) + // in case of UART communication interface clears pin IO2 + // Pin IO2 can be used for timing of data processing + setIO2Pin(0); + + if (platformHasMag() ) { + // Mag Alignment (follows Kalman filter or user algorithm as the + // innovation routine calculates the euler angles and the magnetic + // vector in the NED-frame) + MagAlign(); // only does this on align + } + + if(platformGetUnitCommunicationType() != UART_COMM){ + // Perform interface - specific processing here + } else { + // Process commands and output continuous packets to UART + // Processing of user commands always goes first + ProcessUserCommands (); + SendContinuousPacket(dacqRate); + } + } +} diff --git a/examples/INS/src/user/Leveler.c b/examples/INS/src/user/Leveler.c new file mode 100644 index 0000000..15f9da3 --- /dev/null +++ b/examples/INS/src/user/Leveler.c @@ -0,0 +1,137 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "Leveler.h" + +#include "VectorMath.h" +#include "math.h" + +#include "gpsAPI.h" + +#include "MagAlign.h" + +static void _Leveler_IncrementTimer(void); + +// Variables and functions for the leveler algorithm +LevelerDataStruct gLeveler; + +// Extract the attitude (expressed in Euler-angles) of the body-frame (B) +// in the NED-frame (N) in degrees. +void Leveler_GetAttitude_EA(real *EulerAngles) +{ + EulerAngles[ROLL] = gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + EulerAngles[PITCH] = gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; +} + + +// Replace this with algorithm_setExeFreq +// Setter used to specify the leveler execution frequency at initialization +void Leveler_SetExeFreq(uint16_t freq) +{ + gLeveler.callingFreq = freq; +} + + +// Initialization routine used to set up the timing output variables +void Leveler_InitializeAlgorithmStruct(uint16_t callingFreq) +{ + // Compute dt from the calling frequency of the function, compute the + // timer delta-value from dt (account for rounding), and initialize + // the timer. + gLeveler.callingFreq = callingFreq; + + gLeveler.dt = 1.0 / (real)callingFreq; + gLeveler.dTimerCntr = (uint32_t)( 1000.0 * gLeveler.dt + 0.5 ); + gLeveler.timerCntr = 0; +} + + +// +void Leveler_Algorithm(void) +{ + // Measurement variables + float accelsFloat_B[NUM_AXIS], aHat_B[NUM_AXIS]; + volatile float gHat_B[NUM_AXIS]; + + // + _Leveler_IncrementTimer(); + + // -------- Compute the roll/pitch values from accelerometer readings -------- + // Compute the acceleration unit-vector + accelsFloat_B[X_AXIS] = (float)gLeveler.input.accel_B[X_AXIS]; + accelsFloat_B[Y_AXIS] = (float)gLeveler.input.accel_B[Y_AXIS]; + accelsFloat_B[Z_AXIS] = (float)gLeveler.input.accel_B[Z_AXIS]; + + VectorNormalize( accelsFloat_B, aHat_B ); + + // Form the gravity vector in the body-frame (negative of the + // accelerometer measurement) + gHat_B[X_AXIS] = -aHat_B[X_AXIS]; + gHat_B[Y_AXIS] = -aHat_B[Y_AXIS]; + gHat_B[Z_AXIS] = -aHat_B[Z_AXIS]; + + // Form the roll and pitch angles (in radians) from the gravity + // unit-vector. Formulation is based on a 321-rotation sequence + // and assumption that the gravity-vector is constant. + gLeveler.measuredEulerAngles_BinN[ROLL] = (real)( atan2( gHat_B[Y_AXIS], + gHat_B[Z_AXIS] ) ); + gLeveler.measuredEulerAngles_BinN[PITCH] = (real)( -asin( gHat_B[X_AXIS] ) ); +} + + +static void _Leveler_IncrementTimer(void) +{ + // Increment the timerCntr + gLeveler.timerCntr = gLeveler.timerCntr + gLeveler.dTimerCntr; +} + + +// Populate the EKF input structure with sensor and GPS data (if used) +void Leveler_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) +{ + // Accelerometer signal is in [g] + gLeveler.input.accel_B[X_AXIS] = accels[X_AXIS]; + gLeveler.input.accel_B[Y_AXIS] = accels[Y_AXIS]; + gLeveler.input.accel_B[Z_AXIS] = accels[Z_AXIS]; + + // Angular-rate signal is in [rad/s] + gLeveler.input.angRate_B[X_AXIS] = rates[X_AXIS]; + gLeveler.input.angRate_B[Y_AXIS] = rates[Y_AXIS]; + gLeveler.input.angRate_B[Z_AXIS] = rates[Z_AXIS]; + + // Magnetometer signal is in [G] + gLeveler.input.magField_B[X_AXIS] = mags[X_AXIS]; + gLeveler.input.magField_B[Y_AXIS] = mags[Y_AXIS]; + gLeveler.input.magField_B[Z_AXIS] = mags[Z_AXIS]; +} + + +// Populate the compass output structure with algorithm results +void Leveler_SetOutputStruct(void) +{ + // Euler-angles in [deg] + gLeveler.output.measuredEulerAngles_BinN[ROLL] = gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + gLeveler.output.measuredEulerAngles_BinN[PITCH] = gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; +} diff --git a/examples/INS/src/user/Leveler.h b/examples/INS/src/user/Leveler.h new file mode 100644 index 0000000..517c21a --- /dev/null +++ b/examples/INS/src/user/Leveler.h @@ -0,0 +1,60 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _COMPASS_H_ +#define _COMPASS_H_ + +//#include "GlobalConstants.h" +#include "Indices.h" + +#include "gpsAPI.h" // for gpsDataStruct_t + +// Leveler related functions +void Leveler_GetAttitude_EA(real *EulerAngles); +void Leveler_SetExeFreq(uint16_t freq); +void Leveler_InitializeAlgorithmStruct(uint16_t callingFreq); + +void Leveler_Algorithm(void); +void Leveler_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps); +void Leveler_SetOutputStruct(void); + +// Compass input data structure +typedef struct { + // Sensor readings + double accel_B[NUM_AXIS]; + double angRate_B[NUM_AXIS]; + double magField_B[NUM_AXIS]; +} LevelerInputDataStruct; + + +// Compass output data structure +typedef struct { + // Angles converted to degrees + double measuredEulerAngles_BinN[2]; +} LevelerOutputDataStruct; + + +// Leveler data structure +typedef struct { + // Leveler calling-frequency and derived + uint16_t callingFreq; + real dt; + + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + real measuredEulerAngles_BinN[2]; + + // Input/output data items + LevelerInputDataStruct input; + LevelerOutputDataStruct output; +} LevelerDataStruct; + +extern LevelerDataStruct gLeveler; + +#endif /* _COMPASS_H_ */ diff --git a/examples/INS/src/user/UserAlgorithm.c b/examples/INS/src/user/UserAlgorithm.c new file mode 100644 index 0000000..9cd91fc --- /dev/null +++ b/examples/INS/src/user/UserAlgorithm.c @@ -0,0 +1,286 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "Indices.h" +#include "GlobalConstants.h" + +#include "arm_math.h" + +#include "algorithm.h" +#include "EKF_Algorithm.h" +#include "BitStatus.h" + +#include "bsp.h" +#include "debug.h" + +#include "MagAlign.h" + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _InitAlgo(uint8_t algoType); + +// Initialize GPS algorithm variables +void InitUserAlgorithm() +{ + // Initialize built-in algorithm structure + InitializeAlgorithmStruct(FREQ_100_HZ); + + // place additional required initialization here +} + + +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) +{ + // This can be set at startup based on the packet type selected + static uint8_t algoType = INS; + + // Initialize variable related to the UserNavAlgorithm + _InitAlgo(algoType); + + // Populate the EKF input data structure + EKF_SetInputStruct(accels, rates, mags, gps); + + // Call the desired algorithm based on the EKF with different + // calling rates and different settings. + _Algorithm(dacqRate, algoType); + + // Fill the output data structure with the EKF states and other + // desired information + EKF_SetOutputStruct(); + + // Generate a debug message that provides algorithm output to verify the + // algorithm is generating the proper output. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // The returned value from this function is unused by external functions. The + // NULL pointer is returned instead of a data structure. + return NULL; +} + + +// +static void _InitAlgo(uint8_t algoType) +{ + // Initialize the timer variables + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the configuration variables for a VG-type solution + // (useMags = 0 forces the VG solution) + gAlgorithm.Behavior.bit.freeIntegrate = 0; + gAlgorithm.Behavior.bit.useMag = 0; + gAlgorithm.Behavior.bit.useGPS = 0; + gAlgorithm.Behavior.bit.stationaryLockYaw = 0; + gAlgorithm.Behavior.bit.restartOnOverRange = 0; + gAlgorithm.Behavior.bit.dynamicMotion = 1; + + // Set the system configuration based on system type + switch( algoType ) { + case VG: + // Nothing additional to do (already configured for a VG + // solution) + break; + case AHRS: + // Set the configuration variables for AHRS solution + // (useMags = 1 and enable mags) + enableMagInAlgorithm(TRUE); + gAlgorithm.Behavior.bit.useMag = 1; + break; + case INS: + // Nothing additional to do (already configured for a VG + // solution) + enableMagInAlgorithm(TRUE); + gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above + gAlgorithm.Behavior.bit.useMag = 1; + gAlgorithm.Behavior.bit.useGPS = 1; + break; + default: + // Nothing to do + break; + } + } +} + + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType) +{ + // + static uint8_t algoCntr = 0, algoCntrLimit = 0; + + // Initialize the configuration variables needed to make the system + // generate a VG-type solution. + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the variables that control the algorithm execution rate + algoCntrLimit = (uint8_t)( (float)dacqRate / (float)gAlgorithm.callingFreq + 0.5 ); + if( algoCntrLimit < 1 ) { + // If this logic is reached, also need to adjust the algorithm + // parameters to match the modified calling freq (or stop the + // program to indicate that the user must adjust the program) + algoCntrLimit = 1; + } + algoCntr = algoCntrLimit; + } + + // Increment the counter. If greater than or equal to the limit, reset + // the counter to cause the algorithm to run on the next pass through. + algoCntr++; + if(algoCntr >= algoCntrLimit) { + // Reset counter + algoCntr = 0; + + // Aceinna VG/AHRS/INS algorithm + EKF_Algorithm(); + } +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the output of the EKF algorithm + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; +#if 1 + // For testing, the output packet should contain: + // 1) ITOW + // 2) Euler Angles + // 3) Ang-rate (meas) + // 4) Est ang-rate bias + // 5) Accel (meas) + // 6) Est accel bias + // 7) LLA (est) + // 8) NED-Vel (est) + // 9) Oper mode + + DebugPrintLongInt("", gGPS.itow ); + + DebugPrintFloat(", ", gEKFOutputData.eulerAngs_BinN[ROLL], 4); + DebugPrintFloat(", ", gEKFOutputData.eulerAngs_BinN[PITCH], 4); + DebugPrintFloat(", ", gEKFOutputData.eulerAngs_BinN[YAW], 4); + + DebugPrintFloat(", ", gEKFInputData.angRate_B[X_AXIS], 5); + DebugPrintFloat(", ", gEKFInputData.angRate_B[Y_AXIS], 5); + DebugPrintFloat(", ", gEKFInputData.angRate_B[Z_AXIS], 5); + + DebugPrintFloat(", ", gEKFOutputData.angRateBias_B[X_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.angRateBias_B[Y_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.angRateBias_B[Z_AXIS], 5); + + DebugPrintFloat(", ", gEKFInputData.accel_B[X_AXIS], 5); + DebugPrintFloat(", ", gEKFInputData.accel_B[Y_AXIS], 5); + DebugPrintFloat(", ", gEKFInputData.accel_B[Z_AXIS], 5); + + DebugPrintFloat(", ", gEKFOutputData.accelBias_B[X_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.accelBias_B[Y_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.accelBias_B[Z_AXIS], 5); + + DebugPrintFloat(", ", gEKFOutputData.llaDeg[LAT], 8); + DebugPrintFloat(", ", gEKFOutputData.llaDeg[LON], 8); + DebugPrintFloat(", ", gEKFOutputData.llaDeg[ALT], 5); + + DebugPrintFloat(", ", gEKFOutputData.velocity_N[X_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.velocity_N[Y_AXIS], 5); + DebugPrintFloat(", ", gEKFOutputData.velocity_N[Z_AXIS], 5); +#if 1 + DebugPrintInt(", ", gEKFOutputData.opMode); +#else + switch(gEKFOutputData.opMode) { + case 0: + DebugPrintString(", Stab"); + break; + case 1: + DebugPrintString(", Init"); + break; + case 2: + DebugPrintString(", HG"); + break; + case 3: + DebugPrintString(", LG"); + break; + case 4: + DebugPrintString(", INS"); + break; + } +#endif + DebugPrintEndline(); +#else + // + DebugPrintLongInt("ITOW: ", gGPS.itow ); + // LLA + DebugPrintFloat(", valid: ", (float)gGPS.gpsValid, 1); + // LLA + DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); + DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); + DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); + // Velocity + //DebugPrintFloat(", vN: ", (float)gGPS.vNed[X_AXIS], 5); + //DebugPrintFloat(", vE: ", (float)gGPS.vNed[Y_AXIS], 5); + //DebugPrintFloat(", vD: ", (float)gGPS.vNed[Z_AXIS], 5); + // v^2 + //double vSquared = gGPS.vNed[X_AXIS] * gGPS.vNed[X_AXIS] + + // gGPS.vNed[Y_AXIS] * gGPS.vNed[Y_AXIS]; + //DebugPrintFloat(", vSq: ", (float)vSquared, 5); + //DebugPrintFloat(", vSq: ", (float)gGPS.rawGroundSpeed * (float)gGPS.rawGroundSpeed, 5); + // Newline + DebugPrintEndline(); +#endif + } + } +} diff --git a/examples/INS/src/user/UserAlgorithm.h b/examples/INS/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/INS/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/INS/src/user/UserConfiguration.c b/examples/INS/src/user/UserConfiguration.c new file mode 100644 index 0000000..57688f7 --- /dev/null +++ b/examples/INS/src/user/UserConfiguration.c @@ -0,0 +1,575 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "string.h" + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" +#include "platformAPI.h" + +#include "UserConfiguration.h" +#include "UserMessaging.h" +#include "Indices.h" + +// Default user configuration structure +// Applied to unit upon reception of "zR" command +// Do Not remove - just add extra parameters if needed +// Change default settings if desired +const UserConfigurationStruct gDefaultUserConfig = { + .dataCRC = 0, + .dataSize = sizeof(UserConfigurationStruct), + .userUartBaudRate = 115200, + .userPacketType = "e2", + .userPacketRate = 20, + .lpfAccelFilterFreq = 25, + .lpfRateFilterFreq = 25, + .orientation = "-X-Y+Z", + .gpsBaudRate = 115200, + .gpsProtocol = NOVATEL_BINARY, + // add default parameter values here, if desired + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 +}; + +UserConfigurationStruct gUserConfiguration; +UserConfigurationStruct gTmpUserConfiguration; + +uint8_t UserDataBuffer[4096]; +volatile char *info; +BOOL configValid = FALSE; + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} + + +void userInitConfigureUnit() +{ + uint64_t *ptr = (uint64_t*)&gUserConfiguration; + int size = sizeof(gUserConfiguration); // total size in bytes + + // sanity check for maximum size of user config structure; + if(size >= 0x4000){ + while(1); + } + + if(appStartedFirstTime()) { + // comment next line if want to keep previously stored in EEPROM parameters + // after rebuilding and/or reloading new application + RestoreDefaultUserConfig(); //JSM - Commented out so the mag-align values + // won't be overwritten each time the + // firmware is reloaded + } + + // Validate checksum of user configuration structure + configValid = validateUserConfigInEeprom(&size); + + if(configValid == TRUE) { + // Here we have validated User configuration image. + // Load it from eeprom into ram on top of the default configuration + loadUserConfigFromEeprom((void*)&gUserConfiguration, &size); + } else{ + memset((void*)&gUserConfiguration, 0xff, sizeof(gUserConfiguration)); + } + + // assign new actual size + gUserConfiguration.dataSize = sizeof(UserConfigurationStruct); + + // apply parameters to the platform + for(int i = USER_USER_BAUD_RATE; i <= USER_LAST_SYSTEM_PARAM && configValid; i++){ + UpdateSystemParameter(i, ptr[i], TRUE); + } + + // validate and apply own parameters if desired + for(int i = USER_LAST_SYSTEM_PARAM+1; i < USER_MAX_PARAM && configValid; i++){ + UpdateUserParameter(i, ptr[i], TRUE); + } + + info = getBuildInfo(); +} + + + +/** *************************************************************************** + * @name UpdateSystemParameter - updating of system configuration parameter based of user preferences + * @brief + * + * @param [in] number - parameter number in user configuration structure + * @param [in] data - value of the parameter in little endian format + * @retval error (1), no error (0) + ******************************************************************************/ +// NEEDS TO BE CHECKED +BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) +{ + BOOL result = TRUE; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + if(number < USER_CRC || number >= USER_MAX_PARAM ){ + return FALSE; + } + + switch (number) { + case USER_USER_BAUD_RATE: + result = platformSetBaudRate((int)data, fApply); + break; + case USER_USER_PACKET_TYPE: + result = setUserPacketType((uint8_t*)&data, fApply); + break; + case USER_USER_PACKET_RATE: + result = platformSetPacketRate((int)data, fApply); + break; + case USER_LPF_ACCEL_TYPE: + result = platformSelectLPFilter(ACCEL_SENSOR, (uint32_t)data, fApply); + break; + case USER_LPF_RATE_TYPE: + result = platformSelectLPFilter(RATE_SENSOR, (uint32_t)data, fApply); + break; + case USER_ORIENTATION: + result = platformSetOrientation((uint16_t*)&data, fApply); + break; + case USER_CRC: + case USER_DATA_SIZE: + return TRUE; + + // case USER_XXX: add function calls here if parameter XXXX + // required be updated on the fly + // break; + default: + // by default result should be FALSE for system parameter + result = FALSE; + break; + } + + if(result == TRUE){ + ptr[number] = data; + } + + return result; +} + + +/** *************************************************************************** + * @name UpdateUserParameter - updating user configuration parameter based of preferences + * @brief + * + * @param [in] number - parameter number in user configuration structure + * @param [in] data - value of the parameter in little endian format + * @retval error (1), no error (0) + ******************************************************************************/ +// NEEDS TO BE CHECKED +BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply) +{ + BOOL result; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + if(number <= USER_LAST_SYSTEM_PARAM || number >= USER_MAX_PARAM ){ + return FALSE; + } + + switch (number) { + case USER_GPS_BAUD_RATE: + result = SetGpsBaudRate((int) data, fApply); + break; + case USER_GPS_PROTOCOL: + result = SetGpsProtocol((int) data, fApply); + break; + // case USER_XXX_OFFSET: add function calls here if parameter XXXX + // required be updated on the fly and/or validated + // break; + default: + // by default result should be true if there is no special + // consideration or criteria for parameter validity + result = TRUE; + break; + } + + if(result == TRUE){ + ptr[number] = data; + } + + return result; +} + +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL offsetValid = TRUE; + BOOL lenValid = TRUE; + BOOL numValid = TRUE; + BOOL ret = FALSE; + int32_t result = 0; + + maxParam = sizeof(UserConfigurationStruct)/8; + + // Validate parameters numbers and quantity + if(pld->numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ + lenValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + if(pld->paramOffset >= maxParam){ + offsetValid = FALSE; + result = INVALID_PARAM; + } + + if((pld->numParams + pld->paramOffset) > maxParam){ + numValid = FALSE; + result = INVALID_PARAM; + } + + if(offsetValid && numValid && lenValid){ + // Validate parameters values first + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + result = INVALID_VALUE; + break; + } + } + offset++; + } + if(ret == TRUE){ + // Apply parameters values here + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], TRUE); + } + offset++; + } + } + } + + pld->numParams = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateUserParam + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + uint32_t maxParam; + BOOL offsetValid; + BOOL ret = TRUE; + int32_t result = 0; + + maxParam = sizeof(UserConfigurationStruct)/8; + offsetValid = pld->paramNum < maxParam; + + if(offsetValid){ + // Validate parameter first + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(pld->paramNum, pld->parameter, FALSE); + } + if(ret == TRUE){ + // Apply parameter if valid + ret = UpdateSystemParameter(pld->paramNum, pld->parameter, TRUE); + if (ret != TRUE){ + ret = UpdateUserParameter(pld->paramNum, pld->parameter, TRUE); + } + }else{ + result = INVALID_VALUE; + } + } else { + result = INVALID_PARAM; + } + + pld->paramNum = result; + *payloadLen = 4; + + return TRUE; +} + + +/** **************************************************************************** + * @name UpdateAllUserParams + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +/** **************************************************************************** + * @name UpdateUserConfig + * @brief writes user data into user configuration structure, validates data if + * required, updates system parameters + * + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL lenValid = TRUE; + BOOL numValid = TRUE; + BOOL ret = FALSE; + int32_t result = 0; + + int numParams = (*payloadLen)/8; + maxParam = sizeof(UserConfigurationStruct)/8; + + if(numParams > MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET){ + lenValid = FALSE; + result = INVALID_PAYLOAD_SIZE; + } + + if(numParams > maxParam){ + numValid = FALSE; + result = INVALID_PARAM; + } + + if(numValid && lenValid){ + // Validate parameters here + offset = 0; + for (i = 0; i < numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], FALSE); + if (ret != TRUE){ + result = INVALID_VALUE; + break; + } + } + offset++; + } + if(ret == TRUE){ + // Apply parameters here + offset = 0; + for (i = 0; i < numParams; i++){ + ret = UpdateSystemParameter(offset, pld->parameters[i], TRUE); + if (ret != TRUE){ + ret = UpdateUserParameter(offset, pld->parameters[i], TRUE); + } + offset++; + } + } + } + + pld->parameters[0] = result; + *payloadLen = 4; // return error code + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserConfig + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, maxParam; + BOOL offsetValid = TRUE; + BOOL lenValid = TRUE; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + maxParam = sizeof(UserConfigurationStruct)/8; + + offsetValid = pld->paramOffset < maxParam; + + lenValid = ((pld->numParams + pld->paramOffset) <= maxParam) && + (pld->numParams <= MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET); + + if(offsetValid && lenValid){ + offset = pld->paramOffset; + for (i = 0; i < pld->numParams; i++, offset++) + { + pld->parameters[i] = ptr[offset]; + } + *payloadLen = (pld->numParams + 1) * 8; + } else { + *payloadLen = 4; + pld->numParams = INVALID_PARAM; + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetUserParam + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, maxParam; + BOOL offsetValid; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + maxParam = sizeof(UserConfigurationStruct)/8; + offsetValid = pld->paramNum < maxParam; + + if(offsetValid){ + offset = pld->paramNum; + pld->parameter = ptr[offset]; + *payloadLen = 8 + 4; // parameter + number + } else { + *payloadLen = 4; // number + pld->paramNum = INVALID_PARAM; // invalid + } + + return TRUE; + +} + + +/** **************************************************************************** + * @name GetAllUserParams + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + numParams = sizeof(UserConfigurationStruct)/8; + + offset = 0; + for (i = 0; i < numParams; i++, offset++){ + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + +/** **************************************************************************** + * @name SetMagAlignCmds + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL SetMagAlignCmds(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + numParams = sizeof(UserConfigurationStruct)/8; + + offset = 0; + for (i = 0; i < numParams; i++, offset++){ + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + +/** *************************************************************************** + * @name SaveUserConfig - saving of user configuration structure un the + * predefined flash sector + * @brief + * + * @param [in] N/A + * @retval error (0), no error (1) + ******************************************************************************/ +BOOL SaveUserConfig(void) +{ + int size; + BOOL status; + + size = sizeof(UserConfigurationStruct); + status = saveUserConfigInEeprom((uint8_t *)&gUserConfiguration, size); + + if(status){ + return TRUE; + } + + return FALSE; + +} + + +BOOL RestoreDefaultUserConfig(void) +{ + BOOL valid = TRUE; + // Load default user configuration + memcpy((void*)&gUserConfiguration, (void*)&gDefaultUserConfig, sizeof(UserConfigurationStruct)); + if(!SaveUserConfig()){ + valid = FALSE; + } + return valid; +} diff --git a/examples/INS/src/user/UserConfiguration.h b/examples/INS/src/user/UserConfiguration.h new file mode 100644 index 0000000..6e7bf7a --- /dev/null +++ b/examples/INS/src/user/UserConfiguration.h @@ -0,0 +1,162 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on JAn 25, 2017 + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef USER_CONFIGURATION_H +#define USER_CONFIGURATION_H + +#include + +#include "GlobalConstants.h" +#include "UserMessaging.h" +#include "filter.h" + +/// User defined configuration strucrture +///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types + +typedef struct { + uint64_t dataCRC; /// CRC of user configuration structure CRC-16 + uint64_t dataSize; /// Size of the user configuration structure + + int64_t userUartBaudRate; /// baudrate of user UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + /// 460800 + uint8_t userPacketType[8]; /// User packet to be continiously sent by unit + /// Packet types defined in structure UserOutPacketType + /// in file UserMessaging.h + + int64_t userPacketRate; /// Packet rate for continiously output packet, Hz. + /// Valid settings are: 0 ,2, 5, 10, 20, 25, 50, 100, 200 + + int64_t lpfAccelFilterFreq; /// built-in lpf filter cutoff frequency selection for accelerometers + int64_t lpfRateFilterFreq; /// built-in lpf filter cutoff frequency selection for rate sensors + /// Options are: + /// 0 - Filter turned off + /// 50 - Butterworth LPF 50HZ + /// 20 - Butterworth LPF 20HZ + /// 10 - Butterworth LPF 10HZ + /// 05 - Butterworth LPF 5HZ + /// 02 - Butterworth LPF 2HZ + /// 25 - Butterworth LPF 25HZ + /// 40 - Butterworth LPF 40HZ + + uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff + /// where dd - down axis, rr - right axis, ff - forward axis + /// next axis values a valid : + /// 'X' (0x58) -> plus X, 'Y' (0x59) -> plus Y, 'Z' (0x5a) -> plus Z + /// 'x' (0x78) -> minus X, 'y' (0x79) -> minus Y, 'z' (0x7a) ->minusZ + + //*************************************************************************************** + // here is the border between arbitrary parameters and platform configuration parameters + //*************************************************************************************** + + int64_t gpsBaudRate; /// baudrate of GPS UART, bps. + /// valid options are: + /// 4800 + /// 9600 + /// 19200 + /// 38400 + /// 57600 + /// 115200 + /// 230400 + int64_t gpsProtocol; /// protocol of GPS receicer. + /// so far valid options are: + /// NMEA_TEXT + /// NOVATEL_BINARY + // place new arbitrary configuration parameters here + // parameter size should even to 4 bytes + // Add parameter offset in UserConfigParamOffset structure if validation or + // special processing required + + float hardIron_X; + float hardIron_Y; + float softIron_Ratio; + float softIron_Angle; +} UserConfigurationStruct; + +typedef enum { +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) + USER_CRC = 0, + USER_DATA_SIZE , // 1 + USER_USER_BAUD_RATE , // 2 order of next 4 parameters + USER_USER_PACKET_TYPE , // 3 of required unit output bandwidth + USER_USER_PACKET_RATE , // 4 + USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer + USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor + USER_ORIENTATION , // 7 unit orientation + USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** +// add parameter enumerator here while adding new parameter in user UserConfigurationStruct + USER_GPS_BAUD_RATE , + USER_GPS_PROTOCOL , + USER_HARD_IRON_X , + USER_HARD_IRON_Y , + USER_SOFT_IRON_RATIO , + USER_SOFT_IRON_ANGLE , + USER_MAX_PARAM +} UserConfigParamNumber; + +#define MAX_SYSTEM_PARAM USER_ORIENTATION + +extern int userPacketOut; + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +#define INVALID_PARAM -1 +#define INVALID_VALUE -2 +#define INVALID_PAYLOAD_SIZE -3 + + + +extern UserConfigurationStruct gUserConfiguration; + +extern void InitializeUserAlgorithmParams(void); +extern BOOL validateUserConfigInEeprom(int *numParams); +extern uint32_t getUserParamFromEeprom(uint32_t offset); +extern BOOL saveUserConfigInEeprom(uint8_t *ptrToUserConfigStruct, int userConfigStructLen); +extern int getUserPayloadLength(void); +extern BOOL checkIfUserEEPROMErased(void); +extern BOOL SaveUserData(void); +extern BOOL loadUserConfigFromEeprom(uint8_t *ptrToUserConfigInRam, int *userConfigSize); +//extern UcbPacketType checkPacketType(UcbPacketCodeType receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); +extern BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern BOOL SaveUserConfig(void); +extern BOOL RestoreDefaultUserConfig(void); +extern BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen); +extern BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen); +extern BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen); +extern BOOL UpdateUserParameter(uint32_t number, uint64_t data, BOOL fApply); +extern BOOL UpdateSystemParameter(uint32_t offset, uint64_t data, BOOL fApply); + +#endif /* USER_CONFIGURATION_H */ + + diff --git a/examples/INS/src/user/UserMessaging.c b/examples/INS/src/user/UserMessaging.c new file mode 100644 index 0000000..4c3dd08 --- /dev/null +++ b/examples/INS/src/user/UserMessaging.c @@ -0,0 +1,852 @@ +/** *************************************************************************** + * @file UserConfiguration.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include +#include +#include + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" +#include "platformAPI.h" +#include "sensorsAPI.h" +#include "userAPI.h" + +#include "UserMessaging.h" +#include "UserConfiguration.h" + +#include "MagAlign.h" + +#include "algorithm.h" + +// Declare the IMU data structure +IMUDataStruct gIMU; +gpsDataStruct_t gGPS; + +// Version string +char userVersionString[] = "INS 1.0.0"; + +// for EKFOutputDataStruct +#include "EKF_Algorithm.h" +EKF_OutputDataStruct *algo_res; + +// Example inputs: +// Ping: 55 55 70 47 00 5D 5F +// Mag Align: 55 55 6D 61 00 F0 2D +// 55 55 6D 61 01 00 F1 2E +// 55 55 6D 61 01 01 E1 0F +// 55 55 6D 61 01 02 D1 6C +// 55 55 6D 61 01 03 C1 4D +// 55 55 6D 61 01 04 B1 AA +// 55 55 6D 61 01 05 A1 8B +// 55 55 6D 61 01 06 91 E8 +// 55 55 6D 61 01 07 81 C9 // Get stored values +// 55 55 6D 61 01 08 70 26 +// 55 55 6D 61 01 09 60 07 +// 55 55 6D 61 01 0A 50 64 +// 55 55 6D 61 01 0B +// 55 55 6D 61 01 0C +// 55 55 6D 61 01 0D +// 55 55 6D 61 01 0E 10 E0 +// 55 55 6D 61 01 0F + +// other: 55 55 ... + +/// List of allowed packet codes +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, + {USR_IN_PING, "pG"}, + {USR_IN_UPDATE_CONFIG, "uC"}, + {USR_IN_UPDATE_PARAM, "uP"}, + {USR_IN_UPDATE_ALL, "uA"}, + {USR_IN_SAVE_CONFIG, "sC"}, + {USR_IN_RESTORE_DEFAULTS, "rD"}, + {USR_IN_GET_CONFIG, "gC"}, + {USR_IN_GET_PARAM, "gP"}, + {USR_IN_GET_ALL, "gA"}, + {USR_IN_GET_VERSION, "gV"}, + {USR_IN_RESET, "rS"}, +// place new input packet code here, before USR_IN_MAX + {USR_IN_MAG_ALIGN, "ma"}, // 0x6D 0x61 + {USR_IN_MAX, {0xff, 0xff}}, // "" +}; + + +// packet codes here should be unique - +// should not overlap codes for input packets and system packets +// First byte of Packet code should have value >= 0x61 +usr_packet_t userOutputPackets[] = { +// Packet Type Packet Code + {USR_OUT_NONE, {0x00, 0x00}}, + {USR_OUT_TEST, "zT"}, + {USR_OUT_DATA1, "z1"}, + {USR_OUT_ANG1, "a1"}, + {USR_OUT_ANG2, "a2"}, +// place new type and code here + {USR_OUT_SCALED1, "s1"}, + {USR_OUT_EKF1, "e1"}, + {USR_OUT_EKF2, "e2"}, + {USR_OUT_MAX, {0xff, 0xff}}, // "" +}; + +volatile char *info; +static int _userPayloadLen = 0; +static int _outputPacketType = USR_OUT_MAX; +static int _inputPacketType = USR_IN_MAX; + + +int checkUserPacketType(uint16_t receivedCode) +{ + int res = UCB_ERROR_INVALID_TYPE; + usr_packet_t *packet = &userInputPackets[1]; + uint16_t code; + + // validate packet code here and memorise for further processing + while(packet->packetType != USR_IN_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _inputPacketType = packet->packetType; + return UCB_USER_IN; + } + packet++; + } + + packet = &userOutputPackets[1]; + + // validate packet code here and memorize for further processing + while(packet->packetType != USR_OUT_MAX){ + code = (packet->packetCode[0] << 8) | packet->packetCode[1]; + if(code == receivedCode){ + _outputPacketType = packet->packetType; + return UCB_USER_OUT; + } + packet++; + } + + return res; +} + + +void userPacketTypeToBytes(uint8_t bytes[]) +{ + if(_inputPacketType && _inputPacketType < USR_IN_MAX){ + // response to request. Return same packet code + bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; + bytes[1] = userInputPackets[_inputPacketType].packetCode[1]; + _inputPacketType = USR_IN_MAX; // wait for next input packet + return; + } + + if(_outputPacketType && _outputPacketType < USR_OUT_MAX){ + // continuous packet + bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; + bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; + } else { + bytes[0] = 0; + bytes[1] = 0; + } + +} + + +/** *************************************************************************** + * @name setUserPacketType - set user output packet type + * @brief + * @param [in] packet type + * @retval - TRUE if success, FALSE otherwise + ******************************************************************************/ +BOOL setUserPacketType(uint8_t *data, BOOL fApply) +{ + int type = -1; + uint16_t *code = (uint16_t*)data; + uint16_t tmp; + BOOL result = TRUE; + + usr_packet_t *packet = &userOutputPackets[1]; + for(int i = 0; i < USR_OUT_MAX; i++, packet++){ + if(*code == *((uint16_t*)packet->packetCode)){ + type = packet->packetType; + break; + } + } + + switch(type){ + case USR_OUT_TEST: // simple test packet to check communication + _outputPacketType = type; + _userPayloadLen = USR_OUT_TEST_PAYLOAD_LEN; + break; + case USR_OUT_DATA1: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_DATA1_PAYLOAD_LEN; + break; + case USR_OUT_ANG1: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_ANG1_PAYLOAD_LEN; + break; + case USR_OUT_ANG2: // packet with sensors data. Change at will + _outputPacketType = type; + _userPayloadLen = USR_OUT_ANG2_PAYLOAD_LEN; + break; + case USR_OUT_SCALED1: // packet with arbitrary data + _outputPacketType = type; + _userPayloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + break; + case USR_OUT_EKF1: // packet with EKF algorithm data + _outputPacketType = type; + _userPayloadLen = USR_OUT_EKF1_PAYLOAD_LEN; + break; + case USR_OUT_EKF2: // packet with EKF algorithm data + _outputPacketType = type; + _userPayloadLen = USR_OUT_EKF2_PAYLOAD_LEN; + break; + default: + result = FALSE; + break; + } + + if(result == FALSE){ + return FALSE; + } + + tmp = (data[0] << 8) | data[1]; + + result = platformSetOutputPacketCode(tmp, fApply); + + return result; +} + + +/** *************************************************************************** + * @name getUserPayloadLength - get user payload length for sanity check + * @brief + * + * @retval - user payload length + ******************************************************************************/ +int getUserPayloadLength(void) +{ + // ATTENTION: return actual user payload length, if user packet used + return _userPayloadLen; +} + +/****************************************************************************** + * @name HandleUserInputPacket - API + * @brief general handler + * @param [out] packetPtr - filled in packet from the mapped physical port + * @retval N/A + ******************************************************************************/ +int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) +{ + BOOL valid = TRUE; + int ret = USER_PACKET_OK; + + uint8_t retVal; + int8_t estimatedMagAlignVals[8] = {0}; + int8_t magAlignVals[8] = {0}; + +// userPacket *pkt = (userPacket *)ptrUcbPacket->payload; + + /// call appropriate function based on packet type + switch (_inputPacketType) { + case USR_IN_RESET: + Reset(); + break; + case USR_IN_PING: + { + int len; + uint8_t *model = (uint8_t*)unitVersionString(); + uint8_t *rev = (uint8_t*)platformBuildInfo(); + unsigned int serialNum = unitSerialNumber(); + len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + ptrUcbPacket->payloadLength = len; + } + // leave all the same - it will be bounced back unchanged + break; + case USR_IN_GET_VERSION: + { + int len = snprintf((char*)ptrUcbPacket->payload, 250, "%s", userVersionString ); + ptrUcbPacket->payloadLength = len; + } + break; + case USR_IN_SAVE_CONFIG: + // payload length does not change + if(!SaveUserConfig()){ + valid = FALSE; + } + break; + case USR_IN_UPDATE_CONFIG: + UpdateUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_PARAM: + UpdateUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_UPDATE_ALL: + UpdateAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + break; + case USR_IN_RESTORE_DEFAULTS: + valid = RestoreDefaultUserConfig(); + break; + case USR_IN_GET_CONFIG: + if(!GetUserConfig((userConfigPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_PARAM: + if(!GetUserParam((userParamPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_GET_ALL: + if(!GetAllUserParams((allUserParamsPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength)){ + valid = FALSE; + } + break; + case USR_IN_MAG_ALIGN: + // Set the valid flag true, if the command is not valid then + // it will be set false upon entry into case 0. + valid = TRUE; + + // + retVal = ProcessMagAlignCmds((magAlignCmdPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + switch(retVal-1) + { + // + uint8_t len; + + case 0: // Return the Mag-Align status + //uint8_t *model = (uint8_t*)unitVersionString(); + //uint8_t *rev = (uint8_t*)platformBuildInfo(); + //unsigned int serialNum = unitSerialNumber(); + //len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + //ptrUcbPacket->payloadLength = len; + if(gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND) { + // Start (auto end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x1 ); + } else if(gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND) { + // Start (manual end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x2 ); + } else { + // Start (manual end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x0 ); + } + + ptrUcbPacket->payloadLength = len; + break; + + case 1: // Start mag-align w/ autoend + case 2: // Start mag-align w/ autoend + case 3: // Stop mag-align w/ autoend + case 4: // Accept results + case 5: // Accept results and write to EEPROM + case 6: // Abort Mag-Align or reject results + case 8: // Restore default mag-align values + case 9: // Restore default mag-align values and save in EEPROM + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)retVal-1 ); + ptrUcbPacket->payloadLength = len; + break; + + case 7: // Return stored mag-align values +#if 0 + // Test values: + gMagAlign.estParams.hardIronBias[X_AXIS] = 0.1; + gMagAlign.estParams.hardIronBias[Y_AXIS] = -0.2; + gMagAlign.estParams.softIronScaleRatio = 0.98; + gMagAlign.estParams.softIronAngle = -270.0 * DEG_TO_RAD; +#endif + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + magAlignVals[0] = (char)( ( (int16_t)( gMagAlign.hardIronBias[X_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + magAlignVals[1] = (char)( ( (int16_t)( gMagAlign.hardIronBias[X_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + magAlignVals[2] = (char)( ( (int16_t)( gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + magAlignVals[3] = (char)( ( (int16_t)( gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + magAlignVals[4] = (char)( ( (int16_t)( gMagAlign.softIronScaleRatio * (float)65535.0 ) >> 8 ) & 0xFF ); + magAlignVals[5] = (char)( ( (int16_t)( gMagAlign.softIronScaleRatio * (float)65535.0 ) >> 0 ) & 0xFF ); + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + magAlignVals[6] = (char)( ( (int16_t)( gMagAlign.softIronAngle * (float)10430.37835047046 ) >> 8 ) & 0xFF ); + magAlignVals[7] = (char)( ( (int16_t)( gMagAlign.softIronAngle * (float)10430.37835047046 ) >> 0 ) & 0xFF ); + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + estimatedMagAlignVals[0] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[1] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + estimatedMagAlignVals[2] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[3] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + estimatedMagAlignVals[4] = (char)( ( (int16_t)( gMagAlign.estParams.softIronScaleRatio * (float)65535.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[5] = (char)( ( (int16_t)( gMagAlign.estParams.softIronScaleRatio * (float)65535.0 ) >> 0 ) & 0xFF ); + + // Angle can be +/- pi (in radians) + // Correct for angles that exceed +/-180 + if(gMagAlign.estParams.softIronAngle > PI) { + gMagAlign.estParams.softIronAngle = (float)PI - gMagAlign.estParams.softIronAngle; + } else if(gMagAlign.estParams.softIronAngle < -PI) { + gMagAlign.estParams.softIronAngle = TWO_PI + gMagAlign.estParams.softIronAngle; + } + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + estimatedMagAlignVals[6] = (char)( ( (int16_t)( gMagAlign.estParams.softIronAngle * (float)10430.37835047046 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[7] = (char)( ( (int16_t)( gMagAlign.estParams.softIronAngle * (float)10430.37835047046 ) >> 0 ) & 0xFF ); + +#if 0 + DebugPrintFloat(" ", (float)estimatedMagAlignVals[0], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[1], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[2], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[3], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[4], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[5], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[6], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[7], 1); + DebugPrintEndline(); +#endif + + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", (char)magAlignVals[0], + (char)magAlignVals[1], + (char)magAlignVals[2], + (char)magAlignVals[3], + (char)magAlignVals[4], + (char)magAlignVals[5], + (char)magAlignVals[6], + (char)magAlignVals[7], + (char)estimatedMagAlignVals[0], + (char)estimatedMagAlignVals[1], + (char)estimatedMagAlignVals[2], + (char)estimatedMagAlignVals[3], + (char)estimatedMagAlignVals[4], + (char)estimatedMagAlignVals[5], + (char)estimatedMagAlignVals[6], + (char)estimatedMagAlignVals[7] ); + ptrUcbPacket->payloadLength = len; + break; + + case 10: // Load user computed mag-align values + break; + + default: + valid = FALSE; + break; + } + break; + default: + /// default handler - unknown packet + valid = FALSE; + break; + } + + if(!valid){ + ptrUcbPacket->payloadLength = 0; + ret = USER_PACKET_ERROR; + } + + ptrUcbPacket->packetType = UCB_USER_OUT; // do not remove - done for proper packet routing + + return ret; +} + + +/****************************************************************************** + * @name HandleUserOutputPacket - API call ro prepare continuous user output packet + * @brief general handler + * @param [in] payload pointer to put user data to + * @param [in/out] number of bytes in user payload + * @retval N/A + ******************************************************************************/ +BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) +{ + static uint32_t _testVal = 0; + BOOL ret = TRUE; + + switch (_outputPacketType) { + case USR_OUT_TEST: + { + uint32_t *testParam = (uint32_t*)(payload); + *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; + *testParam = _testVal++; + } + break; + + case USR_OUT_DATA1: + { + int n = 0; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; + data1_payload_t *pld = (data1_payload_t *)payload; + + pld->timer = platformGetDacqTime(); + GetAccelData_mPerSecSq(accels); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)accels[i]; + } + GetRateData_degPerSec(rates); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)rates[i]; + } + GetMagData_G(mags); + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ + pld->sensorsData[n] = (float)mags[i]; + } + *payloadLen = sizeof(data1_payload_t); + } + break; + + case USR_OUT_ANG1: + { + // Variables used to hold the EKF values + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; + + // Diagnostic flags + uint8_t OperMode, LinAccelSwitch, TurnSwitch; + + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 2 floats (4 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 uint8_t (1 byte) = 3 bytes + // ================================= + // NumOfBytes = 47 bytes + *payloadLen = USR_OUT_ANG1_PAYLOAD_LEN; + + // Output time as reprented by gAlgorithm.itow (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gAlgorithm.itow; + + // Output a double representation of time generated from + // gAlgorithm.itow + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gAlgorithm.itow); + + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); + + EKF_GetAttitude_EA(EulerAngles); + *algoData_3++ = (float)EulerAngles[ROLL]; + *algoData_3++ = (float)EulerAngles[PITCH]; + + EKF_GetCorrectedAngRates(CorrRates_B); + *algoData_3++ = (float)CorrRates_B[X_AXIS]; + *algoData_3++ = (float)CorrRates_B[Y_AXIS]; + *algoData_3++ = (float)CorrRates_B[Z_AXIS]; + + GetAccelData_mPerSecSq(accels); + *algoData_3++ = (float)accels[X_AXIS]; + *algoData_3++ = (float)accels[Y_AXIS]; + *algoData_3++ = (float)accels[Z_AXIS]; + + //real CorrAccels_B[NUM_AXIS]; + //_GetEKFCorrectedAccels(CorrAccels_B); + + //real Quaternions[4]; + //_GetEKFAttitude_Quaternions(Quaternions); + + // Output algorithm diagnostic information reprented by uint8_t variables + uint8_t *algoData_4 = (uint8_t*)(algoData_3); + + EKF_GetOperationalMode(&OperMode); + *algoData_4++ = OperMode; + + EKF_GetOperationalSwitches(&LinAccelSwitch, &TurnSwitch); + *algoData_4++ = LinAccelSwitch; + *algoData_4++ = TurnSwitch; + } + break; + + case USR_OUT_ANG2: + { + int n = 0; + + //uint32_t timerCount = 0; + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; + + // Set the pointer of the algoData array to the payload + float *algoData = (float*)(payload); + //uint32_t *payload1, float * payload2 + *payloadLen = USR_OUT_ANG2_PAYLOAD_LEN; +// float *ptr = (float *)&gAlgorithm.itow; + + //timerCount = TIM5->CNT; + //algoData[n++] = *ptr; + + //ptr = + + n = 0; + algoData[n++] = 0.0; // *ptr; + + EKF_GetAttitude_EA(EulerAngles); + algoData[n++] = (float)EulerAngles[ROLL]; + algoData[n++] = (float)EulerAngles[PITCH]; + algoData[n++] = (float)EulerAngles[YAW]; + + EKF_GetCorrectedAngRates(CorrRates_B); + algoData[n++] = (float)CorrRates_B[X_AXIS]; + algoData[n++] = (float)CorrRates_B[Y_AXIS]; + algoData[n++] = (float)CorrRates_B[Z_AXIS]; + + GetAccelData_mPerSecSq(accels); + algoData[n++] = (float)accels[X_AXIS]; + algoData[n++] = (float)accels[Y_AXIS]; + algoData[n++] = (float)accels[Z_AXIS]; + } + break; + + case USR_OUT_SCALED1: + { + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 1 floats (4 bytes) = 4 bytes + // ================================= + // NumOfBytes = 52 bytes + *payloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + + // Output time as represented by gIMU.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Output a double representation of time generated from + // gLeveler.itow + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gIMU.timerCntr); + + // Set the pointer of the sensor array to the payload + float *algoData_3 = (float*)(algoData_2); + *algoData_3++ = (float)gIMU.accel_g[X_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Y_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Z_AXIS]; + + *algoData_3++ = (float)gIMU.rate_radPerSec[X_AXIS]; + *algoData_3++ = (float)gIMU.rate_radPerSec[Y_AXIS]; + *algoData_3++ = (float)gIMU.rate_radPerSec[Z_AXIS]; + + *algoData_3++ = (float)gIMU.mag_G[X_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Y_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Z_AXIS]; + + *algoData_3++ = (float)gIMU.temp_C; + } + break; + + // place additional user packet preparing calls here + // case USR_OUT_XXXX: + // *payloadLen = YYYY; // total user payload length, including user packet type + // payload[0] = ZZZZ; // user packet type + // prepare dada here + // break; + case USR_OUT_EKF1: + { + // Variables used to hold the EKF values + real EulerAngles[NUM_AXIS]; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // ================================= + // NumOfBytes = 75 bytes + *payloadLen = USR_OUT_EKF1_PAYLOAD_LEN; + + // Output time as represented by gLeveler.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Set the pointer of the algoData array to the payload + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = (double)( 0.001 * gIMU.timerCntr ); + + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); + EKF_GetAttitude_EA(EulerAngles); + *algoData_3++ = (float)EulerAngles[ROLL]; + *algoData_3++ = (float)EulerAngles[PITCH]; + *algoData_3++ = (float)EulerAngles[YAW]; + + GetAccelData_g(accels); + *algoData_3++ = (float)accels[X_AXIS]; + *algoData_3++ = (float)accels[Y_AXIS]; + *algoData_3++ = (float)accels[Z_AXIS]; + + double rates[NUM_AXIS]; + GetRateData_degPerSec(rates); + *algoData_3++ = (float)rates[X_AXIS]; + *algoData_3++ = (float)rates[Y_AXIS]; + *algoData_3++ = (float)rates[Z_AXIS]; + + float rateBias[NUM_AXIS]; + EKF_GetEstimatedAngRateBias(rateBias); + *algoData_3++ = (float)rateBias[X_AXIS]; + *algoData_3++ = (float)rateBias[Y_AXIS]; + *algoData_3++ = (float)rateBias[Z_AXIS]; + + GetMagData_G(mags); + *algoData_3++ = (float)mags[X_AXIS]; + *algoData_3++ = (float)mags[Y_AXIS]; + *algoData_3++ = (float)mags[Z_AXIS]; + + // Set the pointer of the algoData array to the payload + uint8_t *algoData_4 = (uint8_t*)(algoData_3); + uint8_t opMode, linAccelSw, turnSw; + EKF_GetOperationalMode(&opMode); + EKF_GetOperationalSwitches(&linAccelSw, &turnSw); + *algoData_4++ = opMode; + *algoData_4++ = linAccelSw; + *algoData_4++ = turnSw; + } + break; + + case USR_OUT_EKF2: + { + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes timer + // 1 double (8 bytes) = 8 bytes timer(double) + // 3 floats (4 bytes) = 12 bytes ea + // 3 floats (4 bytes) = 12 bytes a + // 3 floats (4 bytes) = 12 bytes aBias + // 3 floats (4 bytes) = 12 bytes w + // 3 floats (4 bytes) = 12 bytes wBias + // 3 floats (4 bytes) = 12 bytes v + // 3 floats (4 bytes) = 12 bytes m + // 3 double (8 bytes) = 24 bytes lla + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // 1 uint8_t (1 byte) = 1 bytes + // ================================= + // NumOfBytes = 123 bytes + *payloadLen = USR_OUT_EKF2_PAYLOAD_LEN; + + // Output time as represented by gLeveler.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Set the pointer of the algoData array to the payload + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = (double)( 0.001 * gIMU.timerCntr ); + + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); + real EulerAngles[NUM_AXIS]; + EKF_GetAttitude_EA(EulerAngles); + *algoData_3++ = (float)EulerAngles[ROLL]; + *algoData_3++ = (float)EulerAngles[PITCH]; + *algoData_3++ = (float)EulerAngles[YAW]; + + double accels[NUM_AXIS]; + GetAccelData_g(accels); + *algoData_3++ = (float)accels[X_AXIS]; + *algoData_3++ = (float)accels[Y_AXIS]; + *algoData_3++ = (float)accels[Z_AXIS]; + + float accelBias[NUM_AXIS]; + EKF_GetEstimatedAccelBias(accelBias); + *algoData_3++ = (float)accelBias[X_AXIS]; + *algoData_3++ = (float)accelBias[Y_AXIS]; + *algoData_3++ = (float)accelBias[Z_AXIS]; + + double rates[NUM_AXIS]; + GetRateData_degPerSec(rates); + *algoData_3++ = (float)rates[X_AXIS]; + *algoData_3++ = (float)rates[Y_AXIS]; + *algoData_3++ = (float)rates[Z_AXIS]; + + float rateBias[NUM_AXIS]; + EKF_GetEstimatedAngRateBias(rateBias); + *algoData_3++ = (float)rateBias[X_AXIS]; + *algoData_3++ = (float)rateBias[Y_AXIS]; + *algoData_3++ = (float)rateBias[Z_AXIS]; + + float vel[NUM_AXIS]; + EKF_GetEstimatedVelocity(vel); + *algoData_3++ = (float)vel[X_AXIS]; + *algoData_3++ = (float)vel[Y_AXIS]; + *algoData_3++ = (float)vel[Z_AXIS]; + + double mags[NUM_AXIS]; + GetMagData_G(mags); + *algoData_3++ = (float)mags[X_AXIS]; + *algoData_3++ = (float)mags[Y_AXIS]; + *algoData_3++ = (float)mags[Z_AXIS]; + + // Set the pointer of the algoData array to the payload + double *algoData_4 = (double*)(algoData_3); + double lla[NUM_AXIS]; + EKF_GetEstimatedLLA(lla); + *algoData_4++ = (double)lla[LAT]; + *algoData_4++ = (double)lla[LON]; + *algoData_4++ = (double)lla[ALT]; + + // Set the pointer of the algoData array to the payload + uint8_t *algoData_5 = (uint8_t*)(algoData_4); + uint8_t opMode, linAccelSw, turnSw; + EKF_GetOperationalMode(&opMode); + EKF_GetOperationalSwitches(&linAccelSw, &turnSw); + *algoData_5++ = opMode; + *algoData_5++ = linAccelSw; + *algoData_5++ = turnSw; + } + break; + + default: + { + *payloadLen = 0; + ret = FALSE; + } + break; /// unknown user packet, will send error in response + } + + return ret; +} + + +void WriteResultsIntoOutputStream(void *results) +{ +// implement specific data processing/saving here + algo_res = results; +} diff --git a/examples/INS/src/user/UserMessaging.h b/examples/INS/src/user/UserMessaging.h new file mode 100644 index 0000000..a0b2b60 --- /dev/null +++ b/examples/INS/src/user/UserMessaging.h @@ -0,0 +1,162 @@ +/******************************************************************************* + * File: UserConfiguration.h + * Created on Jan 25, 2017 + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef USER_MESSAGING_H +#define USER_MESSAGING_H + +#include + +#include "GlobalConstants.h" +#include "ucb_packet_struct.h" + +#define USER_PACKET_OK 0 +#define UNKNOWN_USER_PACKET 1 +#define USER_PACKET_ERROR 2 + +// here is definition for packet rate divider +// considering that data acquisition task runs at 200 Hz +typedef enum { + PACKET_RATE_INVALID = -1, + PACKET_RATE_QUIET = 0, // quiet mode + PACKET_RATE_200HZ = 200, // packet rate 200 Hz + PACKET_RATE_100HZ = 100, // packet rate 100 Hz + PACKET_RATE_50HZ = 50, // packet rate 50 Hz + PACKET_RATE_25HZ = 25, // packet rate 25 Hz + PACKET_RATE_20HZ = 20, // packet rate 20 Hz + PACKET_RATE_10HZ = 10, // packet rate 10 Hz + PACKET_RATE_5HZ = 5, // packet rate 5 Hz + PACKET_RATE_2HZ = 2, // packet rate 2 Hz + PACKET_RATE_1HZ = 1, // packet rate 1 Hz +} packet_rate_t; + + +// User Input packet payload has next structure: +// number offset +// of of first +// parameters parameter +// U2 U2 U4/I4/F +// XXXX YYYY [parameters] +// User input packet codes, change at will +typedef enum { + USR_IN_NONE = 0 , + USR_IN_PING , + USR_IN_UPDATE_CONFIG , + USR_IN_UPDATE_PARAM , + USR_IN_UPDATE_ALL , + USR_IN_SAVE_CONFIG , + USR_IN_RESTORE_DEFAULTS , + USR_IN_GET_CONFIG , + USR_IN_GET_PARAM , + USR_IN_GET_ALL , + USR_IN_GET_VERSION , + USR_IN_RESET , + // add new packet type here, before USR_IN_MAX + USR_IN_MAG_ALIGN , + USR_IN_MAX +} UserInPacketType; + +// User output packet codes, change at will +typedef enum { + USR_OUT_NONE = 0, + USR_OUT_TEST, + USR_OUT_DATA1, + USR_OUT_ANG1, + USR_OUT_ANG2, +// add new output packet type here, before USR_OUT_MAX + USR_OUT_SCALED1, + USR_OUT_EKF1, + USR_OUT_EKF2, + USR_OUT_MAX +} UserOutPacketType; + + +// total size of user packet structure should not exceed 255 bytes +#pragma pack(1) +typedef struct { + uint8_t packetPayload[252]; // maximum 252 bytes +} userPacket; +#define MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET 30 +#define FIRST_30_PARAMS 0xFFFFFFFF + +// example of user payload structure +typedef struct { + uint32_t numParams; // number of consecutive parameters to update (little endian) + uint32_t paramOffset; // parameter number in parameters structure (little endian) + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}userConfigPayload; + +#pragma pack(1) +// example of user payload structure +typedef struct { + uint32_t paramNum; // parameter number in parameters structure (little endian) + uint64_t parameter; // up to 30 64-bit parameters (little endian) +}userParamPayload; +#pragma pack() + +// example of user payload structure +typedef struct { + uint64_t parameters[MAX_NUMBER_OF_USER_PARAMS_IN_THE_PACKET]; // up to 30 64-bit parameters (little endian) +}allUserParamsPayload; + +typedef struct { + uint32_t timer; + float sensorsData[9]; +}data1_payload_t; + + +#pragma pack() + + +#define USR_OUT_TEST_PAYLOAD_LEN (4) // test parameter (uint32_t) +#define USR_OUT_DATA1_PAYLOAD_LEN (4*9) // 3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) +#define USR_OUT_ANG1_PAYLOAD_LEN (47) // See message loading code,HandleUserOutputPacket(), for information +#define USR_OUT_ANG2_PAYLOAD_LEN (4*10) +#define USR_OUT_SCALED1_PAYLOAD_LEN (52) +#define USR_OUT_EKF1_PAYLOAD_LEN (75) +#define USR_OUT_EKF2_PAYLOAD_LEN (123) + +#define USER_OK 0x00 +#define USER_NAK 0x80 +#define USER_INVALID 0x81 + +extern int userPacketOut; + +extern int getUserPayloadLength(void); +extern int checkUserPacketType(uint16_t receivedCode); +extern void userPacketTypeToBytes(uint8_t bytes[]); +extern void WriteResultsIntoOutputStream(void *results); +BOOL setUserPacketType(uint8_t* type, BOOL fApply); + +// IMU data structure +typedef struct { + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + double accel_g[3]; + double rate_radPerSec[3]; + double rate_degPerSec[3]; + double mag_G[3]; + double temp_C; +} IMUDataStruct; + +extern IMUDataStruct gIMU; + +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/INS/src/user/dataProcessingAndPresentation.c b/examples/INS/src/user/dataProcessingAndPresentation.c new file mode 100644 index 0000000..67ff447 --- /dev/null +++ b/examples/INS/src/user/dataProcessingAndPresentation.c @@ -0,0 +1,232 @@ +/***************************************************************************** + * @file dataProcessingAndPresentation.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * contains of data post-processing framework + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "userAPI.h" +#include "sensorsAPI.h" +#include "gpsAPI.h" +#include "UserMessaging.h" + +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); +static void _GPSDebugMessage(void); + +/* * + ****************** + * + Sensors data processing flow (from left to right) + +Raw *************** Filtered * ************ Calibrated **************** ********************* Data +Data * Built-in * Raw Data * * Data * User Filter s* * Algorithm * Output +****** LP Filter ************* Calibration ************* (if desired) ***** (Kalman Filter ******** + * * * * * * * or user Algorithm)* + ************* * ************** **************** ********************* + ^^^^^^^ + Can be selected during + initialization or turned + OFF +*///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> + + +// Next function is common for all platforms, but implementation of the methods inside is platform-dependent +// Call to this function made from DataAcquisitionTask during initialization phase +// All user algorithm and data structures should be initialized here, if used +void initUserDataProcessingEngine() +{ + InitUserAlgorithm(); // default implementation located in file user_algorithm.c +} + + +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +void inertialAndPositionDataProcessing(uint16_t dacqRate) +{ + // + void *results; + + // Increment the IMU timer by the calling rate of the data-acquisition task + _IncrementIMUTimer(dacqRate); + + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + GetBoardTempData(&gIMU.temp_C); + + // Obtain GPS data (lat/lon: deg, alt: meters, vel: m/s, ITOW: msec, ) + GetGPSData(&gGPS); + + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // Execute user algorithm (default implementation located in file user_algorithm.c) + results = RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, &gGPS, dacqRate); + + // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure + WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c +} + + +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ + // Initialize timer variables (used to control the output of the debug + // messages and the IMU timer value) + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the IMU output delta-counter value + gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); + } + + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called + gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the loading of the GPS data into the GPS data structure + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; + + // Reset 'new GPS data' flag (this should be done in UpdateFunctions + // to ensure the EKF can use the data) + //gGPS.updateFlag = 0; <-- This would make a difference as the input + // to the algorithm isn't set yet. + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + + case 2: + // GPS data + _GPSDebugMessage(); + break; + } + } + } +} + + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} + + +static void _GPSDebugMessage(void) +{ +#if 1 + // GPS Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); + DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); + DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); + DebugPrintLongInt(", ITOW: ", gGPS.itow ); +#else + // + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintLongInt(", ITOW: ", gGPS.itow ); + + // LLA + DebugPrintFloat(", valid: ", (float)gGPS.gpsValid, 1); + // LLA + DebugPrintFloat(", Lat: ", (float)gGPS.latitude, 8); + DebugPrintFloat(", Lon: ", (float)gGPS.longitude, 8); + DebugPrintFloat(", Alt: ", (float)gGPS.altitude, 5); + // Velocity + //DebugPrintFloat(", vN: ", (float)gGPS.vNed[X_AXIS], 5); + //DebugPrintFloat(", vE: ", (float)gGPS.vNed[Y_AXIS], 5); + //DebugPrintFloat(", vD: ", (float)gGPS.vNed[Z_AXIS], 5); + // v^2 + //double vSquared = gGPS.vNed[X_AXIS] * gGPS.vNed[X_AXIS] + + // gGPS.vNed[Y_AXIS] * gGPS.vNed[Y_AXIS]; + //DebugPrintFloat(", vSq: ", (float)vSquared, 5); + //DebugPrintFloat(", vSq: ", (float)gGPS.rawGroundSpeed * (float)gGPS.rawGroundSpeed, 5); + // Newline +#endif + DebugPrintEndline(); +} diff --git a/examples/LEVELER/src/user/UserAlgorithm.c b/examples/LEVELER/src/user/UserAlgorithm.c deleted file mode 100644 index 858a7b8..0000000 --- a/examples/LEVELER/src/user/UserAlgorithm.c +++ /dev/null @@ -1,167 +0,0 @@ -/** *************************************************************************** - * @file UserAlgorithm.c - * - * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - * - ******************************************************************************/ -/******************************************************************************* -Copyright 2018 ACEINNA, INC - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. -*******************************************************************************/ - -#include "userAPI.h" -#include "gpsAPI.h" -#include "stddef.h" -#include "Indices.h" -#include "GlobalConstants.h" -#include "gpsAPI.h" -#include "algorithmAPI.h" - -#include "math.h" - -//#include "xbowsp_configuration.h" -#include "algorithm.h" -#include "UserAlgorithm.h" -#include "platformAPI.h" - -#include "VectorMath.h" - -// Declare the leveler data structure -LevelerDataStruct gLeveler; - -// Initialize leveler algorithm variables -void InitUserAlgorithm() -{ - // - Leveler_SetExeFreq(FREQ_50_HZ); - Leveler_InitializeDataStruct(); -} - -#include "bsp.h" -#include "debug.h" - -void *RunUserNavAlgorithm(double *accels_B, double *rates_B, double *mags_B, gpsDataStruct_t *gps, int dacqRate) -{ - // Initialization variable - static int initAlgo = 1; - - // Measurement variables - float accelsFloat_B[3], aHat_B[3]; - volatile float gHat_B[3]; - - // Variables that control the output frequency of the debug statement - static uint8_t debugOutputCntr, debugOutputCntrLimit; - - // The following control the execution rate of the algorithm by forcing the - // algorithm to run at a fraction of the data-acquisition task - static uint8_t algoCntr = 0, algoCntrLimit = 0; - - // Initialize the timer variables - if(initAlgo) { - // Reset 'initAlgo' so this is not executed more than once. - initAlgo = 0; - - // Set the variables that control the algorithm execution rate - algoCntrLimit = (int)( dacqRate / (int)gLeveler.callingFreq ); - algoCntr = algoCntrLimit; - - // Set the variables that control the debug-message output-rate (based on - // the desired calling frequency of the debug output) - debugOutputCntr = 0; - - uint16_t debugOutputFreq = 2; // [Hz] - debugOutputCntrLimit = gLeveler.callingFreq / debugOutputFreq; - } - - // ------------ Static-leveler algorithm ------------ - - // Increment algoCntr. If greater than or equal to the limit, execute the - // algorithm and reset the counter so the algorithm will not run until - // the counter limit is once again reached. - algoCntr++; - if(algoCntr >= algoCntrLimit) { - // Reset counter - algoCntr = 0; - - // Increment the timerCntr - gLeveler.timerCntr = gLeveler.timerCntr + gLeveler.dTimerCntr; - - // Compute the acceleration unit-vector - accelsFloat_B[X_AXIS] = (float)accels_B[X_AXIS]; - accelsFloat_B[Y_AXIS] = (float)accels_B[Y_AXIS]; - accelsFloat_B[Z_AXIS] = (float)accels_B[Z_AXIS]; - - VectorNormalize( accelsFloat_B, aHat_B ); - - // Form the gravity vector in the body-frame (negative of the - // accelerometer measurement) - gHat_B[X_AXIS] = -aHat_B[X_AXIS]; - gHat_B[Y_AXIS] = -aHat_B[Y_AXIS]; - gHat_B[Z_AXIS] = -aHat_B[Z_AXIS]; - - // Form the roll and pitch angles (in radians) from the gravity - // unit-vector. Formulation is based on a 321-rotation sequence - // and assumption that the gravity-vector is constant. - gLeveler.measuredEulerAngles_BinN[ROLL] = (real)( atan2( gHat_B[Y_AXIS], - gHat_B[Z_AXIS] ) ); - gLeveler.measuredEulerAngles_BinN[PITCH] = (real)( -asin( gHat_B[X_AXIS] ) ); - - // Generate the debug output - debugOutputCntr++; - if(debugOutputCntr >= debugOutputCntrLimit) { - debugOutputCntr = 0; - DebugPrintFloat("Time: ", gLeveler.timerCntr * 0.001, 3); - DebugPrintFloat(", Roll: ", gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG, 5); - DebugPrintFloat(", Pitch: ", gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG, 5); - DebugPrintEndline(); - } - } - - return NULL; -} - - -// Extract the attitude (expressed in Euler-angles) of the body-frame (B) -// in the NED-frame (N) in degrees. -void Leveler_GetAttitude_EA(real *EulerAngles) -{ - EulerAngles[ROLL] = gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; - EulerAngles[PITCH] = gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; -} - - -// Setter used to specify the leveler execution frequency at initialization -void Leveler_SetExeFreq(uint16_t freq) -{ - gLeveler.callingFreq = freq; -} - - -// Initialization routine used to set up the timing output variables -void Leveler_InitializeDataStruct(void) -{ - // Compute dt from the calling frequency of the function, compute the - // timer delta-value from dt (account for rounding), and initialize - // the timer. - gLeveler.dt = 1.0 / (real)gLeveler.callingFreq; - gLeveler.dTimerCntr = (uint32_t)( 1000.0 * gLeveler.dt + 0.5 ); - gLeveler.timerCntr = 0; -} - - - - diff --git a/examples/LEVELER/src/user/UserAlgorithm.h b/examples/LEVELER/src/user/UserAlgorithm.h deleted file mode 100644 index 2abb518..0000000 --- a/examples/LEVELER/src/user/UserAlgorithm.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * File: UserAlgorithm.h - * Author: joemotyka - * - * Created on June 28, 2018, 12:23 AM - */ - -#ifndef _USER_ALGORITHM_H_ -#define _USER_ALGORITHM_H_ - -#include "GlobalConstants.h" - -// Leveler related functions -void Leveler_GetAttitude_EA(real *EulerAngles); -void Leveler_SetExeFreq(uint16_t freq); -void Leveler_InitializeDataStruct(void); - -// Leveler data structure -typedef struct { - // Leveler calling-frequency and derived - uint16_t callingFreq; - real dt; - - // Timer output counter - uint32_t timerCntr, dTimerCntr; - - // Algorithm states - real measuredEulerAngles_BinN[2]; -} LevelerDataStruct; - -extern LevelerDataStruct gLeveler; - -#endif /* _USER_ALGORITHM_H_ */ - diff --git a/examples/LEVELER/src/user/dataProcessingAndPresentation.c b/examples/LEVELER/src/user/dataProcessingAndPresentation.c deleted file mode 100644 index 450c610..0000000 --- a/examples/LEVELER/src/user/dataProcessingAndPresentation.c +++ /dev/null @@ -1,94 +0,0 @@ -/***************************************************************************** - * @file dataProcessingAndPresentation.c - * - * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - * PARTICULAR PURPOSE. - * - * contains of data post-processing framework - ******************************************************************************/ -/******************************************************************************* -Copyright 2018 ACEINNA, INC - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. -*******************************************************************************/ - -#include "userAPI.h" -#include "sensorsAPI.h" -#include "gpsAPI.h" -#include "gps.h" -#include "UserMessaging.h" - -/* * - ****************** - * - Sensors data processing flow (from left to right) - -Raw *************** Filtered * ************ Calibrated **************** ********************* Data -Data * Built-in * Raw Data * * Data * User Filter s* * Algorithm * Output -****** LP Filter ************* Calibration ************* (if desired) ***** (Kalman Filter ******** - * * * * * * * or user Algorithm)* - ************* * ************** **************** ********************* - ^^^^^^^ - Can be selected during - initialization or turned - OFF -*///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> - - - -// Next function is common for all platforms, but implementation of the methods inside is platform-dependent -// Call to this function made from DataAcquisitionTask during initialization phase -// All user algorithm and data structures should be initialized here, if used -void initUserDataProcessingEngine() -{ - InitUserAlgorithm(); // default implementation located in file user_algorithm.c -} - - -// Next function is common for all platforms, but implementation of the methods inside is platform and user-dependent -// Call to this function made from DataAcquisitionTask after retrieving samples of current sensors data and -// applying corresponding calibration -// dacqRate is rate with which new set of sensors data arrives -void inertialAndPositionDataProcessing(int dacqRate) -{ - double accels[3]; // in g - double rates[3]; // in rad/s - double mags[3]; // Gauss - double boardTemp; // deg C - gpsDataStruct_t gps; - void *results; - - // Obtain accelerometer data - GetAccelData_g(accels); - - // Obtain rates data or comment out - GetRateData_radPerSec(rates); - - // Obtain mags data or comment out - GetMagData_G(mags); - - // Obtain board temperature data or comment out - GetBoardTempData(&boardTemp); - - // Obtain GPS data or comment out - GetGPSData(&gps); - - // execute user algorithm or remove - results = RunUserNavAlgorithm(accels, rates, mags, &gps, dacqRate); // default implementation located in file user_algorithm.c - - // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure - WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c -} - diff --git a/examples/Leveler/.gitignore b/examples/Leveler/.gitignore new file mode 100644 index 0000000..04186a1 --- /dev/null +++ b/examples/Leveler/.gitignore @@ -0,0 +1,11 @@ +.pioenvs +.piolibdeps +.vscode +*.map +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/*.db +.vscode/.browse.c_cpp.db* +.settings +.cproject +.project diff --git a/examples/Leveler/.travis.yml b/examples/Leveler/.travis.yml new file mode 100644 index 0000000..52072ef --- /dev/null +++ b/examples/Leveler/.travis.yml @@ -0,0 +1,55 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < http://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < http://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < http://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choice one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# install: +# - pip install -U platformio +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to by used as a library with examples +# + +# language: python +# python: +# - "2.7" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/examples/Leveler/include/API/userAPI.h b/examples/Leveler/include/API/userAPI.h new file mode 100644 index 0000000..88b2ad4 --- /dev/null +++ b/examples/Leveler/include/API/userAPI.h @@ -0,0 +1,55 @@ +/** ****************************************************************************** + * @file userAPI.h API functions for Interfacing with user algorithm + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + *****************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef _USER_API_H +#define _USER_API_H + +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + +#include +#include "gpsAPI.h" + +void inertialAndPositionDataProcessing(uint16_t dacqRate); + +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); +void WriteResultsIntoOutputStream(void *results) ; +void InitUserDataStructures(); +void InitUserFilters(); +void InitUserAlgorithm(); +void initUserDataProcessingEngine(); +void userInitConfigureUnit(); + +#endif diff --git a/examples/Leveler/include/FreeRTOSConfig.h b/examples/Leveler/include/FreeRTOSConfig.h new file mode 100644 index 0000000..80ca6b9 --- /dev/null +++ b/examples/Leveler/include/FreeRTOSConfig.h @@ -0,0 +1,180 @@ +/* + FreeRTOS V9.0.0 - Copyright (C) 2016 Real Time Engineers Ltd. + All rights reserved + + VISIT http://www.FreeRTOS.org TO ENSURE YOU ARE USING THE LATEST VERSION. + + This file is part of the FreeRTOS distribution. + + FreeRTOS is free software; you can redistribute it and/or modify it under + the terms of the GNU General Public License (version 2) as published by the + Free Software Foundation >>>> AND MODIFIED BY <<<< the FreeRTOS exception. + + *************************************************************************** + >>! NOTE: The modification to the GPL is included to allow you to !<< + >>! distribute a combined work that includes FreeRTOS without being !<< + >>! obliged to provide the source code for proprietary components !<< + >>! outside of the FreeRTOS kernel. !<< + *************************************************************************** + + FreeRTOS is distributed in the hope that it will be useful, but WITHOUT ANY + WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + FOR A PARTICULAR PURPOSE. Full license text is available on the following + link: http://www.freertos.org/a00114.html + + *************************************************************************** + * * + * FreeRTOS provides completely free yet professionally developed, * + * robust, strictly quality controlled, supported, and cross * + * platform software that is more than just the market leader, it * + * is the industry's de facto standard. * + * * + * Help yourself get started quickly while simultaneously helping * + * to support the FreeRTOS project by purchasing a FreeRTOS * + * tutorial book, reference manual, or both: * + * http://www.FreeRTOS.org/Documentation * + * * + *************************************************************************** + + http://www.FreeRTOS.org/FAQHelp.html - Having a problem? Start by reading + the FAQ page "My application does not run, what could be wrong?". Have you + defined configASSERT()? + + http://www.FreeRTOS.org/support - In return for receiving this top quality + embedded software for free we request you assist our global community by + participating in the support forum. + + http://www.FreeRTOS.org/training - Investing in training allows your team to + be as productive as possible as early as possible. Now you can receive + FreeRTOS training directly from Richard Barry, CEO of Real Time Engineers + Ltd, and the world's leading authority on the world's leading RTOS. + + http://www.FreeRTOS.org/plus - A selection of FreeRTOS ecosystem products, + including FreeRTOS+Trace - an indispensable productivity tool, a DOS + compatible FAT file system, and our tiny thread aware UDP/IP stack. + + http://www.FreeRTOS.org/labs - Where new FreeRTOS products go to incubate. + Come and try FreeRTOS+TCP, our new open source TCP/IP stack for FreeRTOS. + + http://www.OpenRTOS.com - Real Time Engineers ltd. license FreeRTOS to High + Integrity Systems ltd. to sell under the OpenRTOS brand. Low cost OpenRTOS + licenses offer ticketed support, indemnification and commercial middleware. + + http://www.SafeRTOS.com - High Integrity Systems also provide a safety + engineered and independently SIL3 certified version for use in safety and + mission critical applications that require provable dependability. + + 1 tab == 4 spaces! +*/ + + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE + * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. + * + * See http://www.freertos.org/a00110.html. + *----------------------------------------------------------*/ + +/* Ensure stdint is only used by the compiler, and not the assembler. */ +#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__) + #include + extern uint32_t SystemCoreClock; +#endif + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ (SystemCoreClock) +#define configTICK_RATE_HZ ((TickType_t)1000) +#define configMAX_PRIORITIES (7) +#define configMINIMAL_STACK_SIZE ((uint16_t)2048) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) +#define configMAX_TASK_NAME_LEN (30) +#define configUSE_TRACE_FACILITY 1 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 1 +#define configUSE_MUTEXES 1 +#define configQUEUE_REGISTRY_SIZE 8 +#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configUSE_APPLICATION_TASK_TAG 0 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Software timer definitions. */ +#define configUSE_TIMERS 0 +#define configTIMER_TASK_PRIORITY (2) +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH (configMINIMAL_STACK_SIZE * 2) + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 0 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 + +/* Cortex-M specific definitions. */ +#ifdef __NVIC_PRIO_BITS + /* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */ + #define configPRIO_BITS __NVIC_PRIO_BITS +#else + #define configPRIO_BITS 4 /* 15 priority levels */ +#endif + +/* The lowest interrupt priority that can be used in a call to a "set priority" +function. */ +#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 0xf + +/* The highest interrupt priority that can be used by any interrupt service +routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL +INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER +PRIORITY THAN THIS! (higher priorities are lower numeric values. */ +#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 1 + + +//#define configKERNEL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 15, or 255 as only the top four bits are implemented. This is the lowest priority. */ +//#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( 0x01 << 4 ) /* Priority 5, or 80 as only the top four bits are implemented. */ + + +/* Interrupt priorities used by the kernel port layer itself. These are generic +to all Cortex-M ports, and do not rely on any particular library functions. */ +#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) +/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!! +See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */ +#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) + +/* Normal assert() semantics without relying on the provision of an assert.h +header file. */ +#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); for( ;; ); } + +/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS + standard names. */ +#define vPortSVCHandler SVC_Handler +#define xPortPendSVHandler PendSV_Handler + +/* IMPORTANT: This define MUST be commented when used with STM32Cube firmware, + to prevent overwriting SysTick_Handler defined within STM32Cube HAL */ +//#define xPortSysTickHandler SysTick_Handler +#define osSystickHandler SysTick_Handler + +#endif /* FREERTOS_CONFIG_H */ + diff --git a/examples/Leveler/include/README b/examples/Leveler/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/examples/Leveler/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/examples/Leveler/include/osresources.h b/examples/Leveler/include/osresources.h new file mode 100644 index 0000000..47ac800 --- /dev/null +++ b/examples/Leveler/include/osresources.h @@ -0,0 +1,42 @@ +#ifndef _OS_RES_H +#define _OS_RES_H +#include "cmsis_os.h" +#include "portmacro.h" + + +#ifdef __MAIN + +osSemaphoreDef(GYRO_READY_SEM); +osSemaphoreDef(ACCEL_READY_SEM); +osSemaphoreDef(MAG_READY_SEM); +osSemaphoreDef(TEMP_READY_SEM); +osSemaphoreDef(NAV_DATA_READY_SEM); +osSemaphoreDef(CLI_READY_SEM); +osSemaphoreDef(DATA_ACQ_SEM); +osSemaphoreDef(CAN_DATA_SEM); +osSemaphoreDef(CLI_SEM); +osSemaphoreId gyroReadySem; +osSemaphoreId accelReadySem; +osSemaphoreId magReadySem; +osSemaphoreId tempReadySem; +osSemaphoreId navDataReadySem; +osSemaphoreId cliReadySem; +osSemaphoreId dataAcqSem; +osSemaphoreId canDataSem; +osSemaphoreId cliSem; + +#else + +extern osSemaphoreId gyroReadySem; +extern osSemaphoreId accelReadySem; +extern osSemaphoreId magReadySem; +extern osSemaphoreId tempReadySem; +extern osSemaphoreId navDataReadySem; +extern osSemaphoreId dataAcqSem; +extern osSemaphoreId canDataSem; +extern osSemaphoreId cliSem; + +#endif + + +#endif diff --git a/examples/Leveler/include/taskDataAcquisition.h b/examples/Leveler/include/taskDataAcquisition.h new file mode 100644 index 0000000..891fb4e --- /dev/null +++ b/examples/Leveler/include/taskDataAcquisition.h @@ -0,0 +1,41 @@ +/** *************************************************************************** + * @file taskDataAcquisition.h + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * sensor data acquisition task runs at 100Hz, gets the data for each sensor + * and applies available calibration + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#ifndef _TASK_DATA_ACQUISITION_H_ +#define _TASK_DATA_ACQUISITION_H_ + +#include "stdint.h" +#include "GlobalConstants.h" +extern void TaskDataAcquisition(void const *argument); +extern void PrepareToNewDacqTick(); +extern void TaskDataAcquisition_Init(void); +extern void GetSensorsData(void); +extern void EnterMainAlgLoop(void); +extern void DataAquisitionStart(void); +extern BOOL isOneHundredHertzFlag(void); + +#endif diff --git a/examples/Leveler/lib/README b/examples/Leveler/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/examples/Leveler/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/Leveler/lib/readme.txt b/examples/Leveler/lib/readme.txt new file mode 100644 index 0000000..131f1bf --- /dev/null +++ b/examples/Leveler/lib/readme.txt @@ -0,0 +1,41 @@ + +This directory is intended for the project specific (private) libraries. +PlatformIO will compile them to static libraries and link to executable file. + +The source code of each library should be placed in separate directory, like +"lib/private_lib/[here are source files]". + +For example, see how can be organized `Foo` and `Bar` libraries: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) http://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- readme.txt --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Then in `src/main.c` you should use: + +#include +#include + +// rest H/C/CPP code + +PlatformIO will find your libraries automatically, configure preprocessor's +include paths and build them. + +More information about PlatformIO Library Dependency Finder +- http://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/LEVELER/platformio.ini b/examples/Leveler/platformio.ini similarity index 71% rename from examples/LEVELER/platformio.ini rename to examples/Leveler/platformio.ini index a2a5ca5..9f1221d 100644 --- a/examples/LEVELER/platformio.ini +++ b/examples/Leveler/platformio.ini @@ -14,14 +14,29 @@ description = A static-leveler uses calibrated accelerometer readings to measure platform = aceinna_imu lib_archive = false board = OpenIMU300ZA -lib_deps = openimu-core-libraries@^1.0.2 +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps = ../../../openimu-core +lib_deps = openimu-core-libraries@^1.0.22 build_flags = -D CLI + -D __FPU_PRESENT + -D ARM_MATH_CM4 -I include -I include/API -I src/user +; -L ldscripts + -O0 +; -Wno-comment -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 ;upload_protocol = jlink ;debug_tool = jlink + + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/LEVELER/src/main.c b/examples/Leveler/src/main.c similarity index 91% rename from examples/LEVELER/src/main.c rename to examples/Leveler/src/main.c index cc57c20..63527f8 100644 --- a/examples/LEVELER/src/main.c +++ b/examples/Leveler/src/main.c @@ -29,25 +29,24 @@ limitations under the License. #define __MAIN #include + #include "boardAPI.h" +#include "magAPI.h" #include "platformAPI.h" #include "userAPI.h" + #include "debug.h" + #include "taskDataAcquisition.h" #include "taskUserCommunication.h" -#include "magAPI.h" + #include "osapi.h" #include "osresources.h" - - - - #ifdef CLI #include "commandLine.h" #endif - #ifdef GPS #include "gpsAPI.h" #endif @@ -83,24 +82,29 @@ void DebugInterfaceInit(void) { char status[100]; - // Initialize the DEBUG USART (serial) port - InitDebugSerialCommunication(115200); // debug_usart.c - DEBUG_STRING("\r\nOpenIMU System\r\n"); + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + + if(debugChannel == UART_CHANNEL_NONE){ + //nothing to do + return; + } // Add a delay to allow the system to stabilize after the reset line (nRst) // is released for(int i = 0; i < 4000000; i++) ; // TODO - calculate more precisely -// DelayMs(300); todo - calculate + //DelayMs(300); //todo - calculate + + // Initialize the DEBUG USART (serial) port + InitDebugSerialCommunication(230400); // debug_usart.c + //DEBUG_STRING("\r\nDMU380 System\r\n"); BoardGetResetStatus(status, sizeof(status)); ERROR_STRING(status); - } void CreateTasks(void) { - osThreadId iD; /// Create RTOS tasks: @@ -110,12 +114,14 @@ void CreateTasks(void) if(iD == NULL){ while(1); } + //user communication task osThreadDef(USER_COMM_TASK, TaskUserCommunication, osPriorityNormal, 0, 2048); iD = osThreadCreate(osThread(USER_COMM_TASK), NULL); if(iD == NULL){ while(1); } + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); @@ -130,8 +136,8 @@ void CreateTasks(void) while(1); } cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); #endif - } /** *************************************************************************** @@ -147,16 +153,17 @@ int main(void) { - // Initialize processor and board-related signals + // Initialize processor and board-related signals BoardInit(); + platformSetUnitCommunicationType(UART_COMM); + // Apply factory configuration platformInitConfigureUnit(); // Apply user-chosen configuration userInitConfigureUnit(); - // Initialize OS and create required tasks CreateTasks(); @@ -184,4 +191,4 @@ void vApplicationStackOverflowHook() void vApplicationMallocFailedHook() { while(1); -} \ No newline at end of file +} diff --git a/examples/LEVELER/src/taskDataAcquisition.c b/examples/Leveler/src/taskDataAcquisition.c similarity index 69% rename from examples/LEVELER/src/taskDataAcquisition.c rename to examples/Leveler/src/taskDataAcquisition.c index 514ad1a..0c5368e 100644 --- a/examples/LEVELER/src/taskDataAcquisition.c +++ b/examples/Leveler/src/taskDataAcquisition.c @@ -25,28 +25,18 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#include "taskDataAcquisition.h" -#include "taskUserCommunication.h" -#include "UserConfiguration.h" -#include "magAPI.h" -#include "bitAPI.h" -//#include "boardAPI.h" -#include "bsp.h" #include "algorithmAPI.h" -#include "Indices.h" -#include "qmath.h" -#include "osapi.h" -#include "osresources.h" -#include "userAPI.h" +#include "bitAPI.h" +#include "boardAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "userAPI.h" -uint32_t dacqTimer = 0; - -uint32_t getDacqTime() -{ - return dacqTimer; -} +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" +#include "osapi.h" +#include "osresources.h" /** *************************************************************************** * @name TaskDataAcquisition() CALLBACK main loop @@ -58,13 +48,14 @@ uint32_t getDacqTime() ******************************************************************************/ void TaskDataAcquisition(void const *argument) { - // uint8_t functionStatus; - int res, dacqRate; + // + int res; + uint16_t dacqRate; + #pragma GCC diagnostic ignored "-Wunused-but-set-variable" BOOL overRange = FALSE; //uncomment this line if overrange processing required #pragma GCC diagnostic warning "-Wunused-but-set-variable" - // This routine sets up the timer. Can use the structure below this point. TaskDataAcquisition_Init(); // Initialize user algorithm parameters, if needed @@ -73,25 +64,22 @@ void TaskDataAcquisition(void const *argument) InitMagAlignParams(); // Start sensors data acquisition DataAquisitionStart(); - // determine the period of data acquisition task - dacqRate = DACQ_200_HZ; - + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; //************** Add user initialization here, if needed **************** - - /// Data is provided by the primary sensors (accelerometer and rate-sensor) - /// as fast as possible. Data is obtained from secondary sensors at a - /// lower rate. The rate-sensor data read is synced to TIM5 or an - /// external signal. When the read is commanded and in the data buffer, - /// The data-event flag is set and the wait is bypassed. Data is obtained - /// from the buffer, calibrated, filtered and provided to the user for - /// subsequent processing. + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. while( 1 ) { - // ***************************************************************** // NOTE: This task loop runs at 100 or 200 Hz (default 200 Hz) // user can choose period of this task by @@ -105,25 +93,28 @@ void TaskDataAcquisition(void const *argument) if(res != osOK){ // Wait timeout expired. Something wrong wit the dacq system // Process timeout here - } // inform user, that new data set is being prepared (if required) // in case of UART communication interface sets pin IO2 high setIO2Pin (1); - // Get calibrated sensors data - // Inside this function done initial low pass data filtering (second order batterworth - // filter). User can choose cutoff frequency of the filter or turn filtering off. - // Use Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) function to choose - // filter type. Refer to UserConfiguration.c for implementation and to the enumerator structure - // eFilterType in file filter.h for available selections. - // Low pass filtering followed by applying of unit calibration parameters - scaling, temperature - // compensation, bias removal. - // Results are placed in the structure of type double. Pointer to this structure - // is pScaledSensors. Ordering of sensors data in this structure defined by rawSensor_e - // enumerator in the file indices.h - + // Get calibrated sensor data: + // Inside this function the sensor data is filtered by a second-order low-pass + // Butterworth filter, with a cutoff frequency selected by the user (or zero to + // disable). The cutoff is selected using the following: + // + // Select_LP_filter(rawSensor_e sensorType, eFilterType filterType) + // + // Refer to UserConfiguration.c for implementation and to the enumerator structure + // 'eFilterType' in file filter.h for available selections. + // + // Low pass filtering is followed by application of the unit calibration parameters + // - scaling, temperature compensation, bias removal, and misalignment. + // + // Results are placed in the structure of type double. The pointer to this + // structure is 'pScaledSensors'. Ordering of sensors data in this structure + // defined by 'rawSensor_e' enumerator in the file indices.h GetSensorsData(); // Check if sensors data over range @@ -139,21 +130,22 @@ void TaskDataAcquisition(void const *argument) // Temperature - degrees C //****************************************************************** - // BIT status. May have inadvertently changed this during an update. updateBITandStatus(); // ********************** Algorithm ************************ - // In AHRS or INS mode due to CPU performance concerns algorithm better be - // performed at 100 or less Hz based on complexity and number calculations - // involved. Incorporate timing logic inside algorithm function, if desired - // For the initial simplicity use pScaledSensors array as input - // and output of algorithm. - // Use is100Hz variable to determine the rate of this task loop + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. inertialAndPositionDataProcessing(dacqRate); - //Uncomment next line if there is intention of using S0 or S1 xbow packets - //for continuous data output + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output //***************************************************************** // applyNewScaledSensorsData(); //***************************************************************** @@ -164,13 +156,13 @@ void TaskDataAcquisition(void const *argument) setIO2Pin (0); if(platformHasMag() ) { - // Mag Alignment (follows Kalman filter or user algorithm as the innovation routine - // calculates the euler angles and the magnetic vector in the - // NED-frame) + // Mag Alignment (follows Kalman filter or user algorithm as the + // innovation routine calculates the euler angles and the magnetic + // vector in the NED-frame) MagAlign(); // only does this on align } - if(getUnitCommunicationType() != UART_COMM){ + if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here }else { // Process commands and output continuous packets to UART @@ -180,6 +172,3 @@ void TaskDataAcquisition(void const *argument) } } } - - - diff --git a/examples/Leveler/src/user/Leveler.c b/examples/Leveler/src/user/Leveler.c new file mode 100644 index 0000000..15f9da3 --- /dev/null +++ b/examples/Leveler/src/user/Leveler.c @@ -0,0 +1,137 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "Leveler.h" + +#include "VectorMath.h" +#include "math.h" + +#include "gpsAPI.h" + +#include "MagAlign.h" + +static void _Leveler_IncrementTimer(void); + +// Variables and functions for the leveler algorithm +LevelerDataStruct gLeveler; + +// Extract the attitude (expressed in Euler-angles) of the body-frame (B) +// in the NED-frame (N) in degrees. +void Leveler_GetAttitude_EA(real *EulerAngles) +{ + EulerAngles[ROLL] = gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + EulerAngles[PITCH] = gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; +} + + +// Replace this with algorithm_setExeFreq +// Setter used to specify the leveler execution frequency at initialization +void Leveler_SetExeFreq(uint16_t freq) +{ + gLeveler.callingFreq = freq; +} + + +// Initialization routine used to set up the timing output variables +void Leveler_InitializeAlgorithmStruct(uint16_t callingFreq) +{ + // Compute dt from the calling frequency of the function, compute the + // timer delta-value from dt (account for rounding), and initialize + // the timer. + gLeveler.callingFreq = callingFreq; + + gLeveler.dt = 1.0 / (real)callingFreq; + gLeveler.dTimerCntr = (uint32_t)( 1000.0 * gLeveler.dt + 0.5 ); + gLeveler.timerCntr = 0; +} + + +// +void Leveler_Algorithm(void) +{ + // Measurement variables + float accelsFloat_B[NUM_AXIS], aHat_B[NUM_AXIS]; + volatile float gHat_B[NUM_AXIS]; + + // + _Leveler_IncrementTimer(); + + // -------- Compute the roll/pitch values from accelerometer readings -------- + // Compute the acceleration unit-vector + accelsFloat_B[X_AXIS] = (float)gLeveler.input.accel_B[X_AXIS]; + accelsFloat_B[Y_AXIS] = (float)gLeveler.input.accel_B[Y_AXIS]; + accelsFloat_B[Z_AXIS] = (float)gLeveler.input.accel_B[Z_AXIS]; + + VectorNormalize( accelsFloat_B, aHat_B ); + + // Form the gravity vector in the body-frame (negative of the + // accelerometer measurement) + gHat_B[X_AXIS] = -aHat_B[X_AXIS]; + gHat_B[Y_AXIS] = -aHat_B[Y_AXIS]; + gHat_B[Z_AXIS] = -aHat_B[Z_AXIS]; + + // Form the roll and pitch angles (in radians) from the gravity + // unit-vector. Formulation is based on a 321-rotation sequence + // and assumption that the gravity-vector is constant. + gLeveler.measuredEulerAngles_BinN[ROLL] = (real)( atan2( gHat_B[Y_AXIS], + gHat_B[Z_AXIS] ) ); + gLeveler.measuredEulerAngles_BinN[PITCH] = (real)( -asin( gHat_B[X_AXIS] ) ); +} + + +static void _Leveler_IncrementTimer(void) +{ + // Increment the timerCntr + gLeveler.timerCntr = gLeveler.timerCntr + gLeveler.dTimerCntr; +} + + +// Populate the EKF input structure with sensor and GPS data (if used) +void Leveler_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps) +{ + // Accelerometer signal is in [g] + gLeveler.input.accel_B[X_AXIS] = accels[X_AXIS]; + gLeveler.input.accel_B[Y_AXIS] = accels[Y_AXIS]; + gLeveler.input.accel_B[Z_AXIS] = accels[Z_AXIS]; + + // Angular-rate signal is in [rad/s] + gLeveler.input.angRate_B[X_AXIS] = rates[X_AXIS]; + gLeveler.input.angRate_B[Y_AXIS] = rates[Y_AXIS]; + gLeveler.input.angRate_B[Z_AXIS] = rates[Z_AXIS]; + + // Magnetometer signal is in [G] + gLeveler.input.magField_B[X_AXIS] = mags[X_AXIS]; + gLeveler.input.magField_B[Y_AXIS] = mags[Y_AXIS]; + gLeveler.input.magField_B[Z_AXIS] = mags[Z_AXIS]; +} + + +// Populate the compass output structure with algorithm results +void Leveler_SetOutputStruct(void) +{ + // Euler-angles in [deg] + gLeveler.output.measuredEulerAngles_BinN[ROLL] = gLeveler.measuredEulerAngles_BinN[ROLL] * RAD_TO_DEG; + gLeveler.output.measuredEulerAngles_BinN[PITCH] = gLeveler.measuredEulerAngles_BinN[PITCH] * RAD_TO_DEG; +} diff --git a/examples/Leveler/src/user/Leveler.h b/examples/Leveler/src/user/Leveler.h new file mode 100644 index 0000000..517c21a --- /dev/null +++ b/examples/Leveler/src/user/Leveler.h @@ -0,0 +1,60 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _COMPASS_H_ +#define _COMPASS_H_ + +//#include "GlobalConstants.h" +#include "Indices.h" + +#include "gpsAPI.h" // for gpsDataStruct_t + +// Leveler related functions +void Leveler_GetAttitude_EA(real *EulerAngles); +void Leveler_SetExeFreq(uint16_t freq); +void Leveler_InitializeAlgorithmStruct(uint16_t callingFreq); + +void Leveler_Algorithm(void); +void Leveler_SetInputStruct(double *accels, double *rates, double *mags, gpsDataStruct_t *gps); +void Leveler_SetOutputStruct(void); + +// Compass input data structure +typedef struct { + // Sensor readings + double accel_B[NUM_AXIS]; + double angRate_B[NUM_AXIS]; + double magField_B[NUM_AXIS]; +} LevelerInputDataStruct; + + +// Compass output data structure +typedef struct { + // Angles converted to degrees + double measuredEulerAngles_BinN[2]; +} LevelerOutputDataStruct; + + +// Leveler data structure +typedef struct { + // Leveler calling-frequency and derived + uint16_t callingFreq; + real dt; + + // Timer output counter + uint32_t timerCntr, dTimerCntr; + + // Algorithm states + real measuredEulerAngles_BinN[2]; + + // Input/output data items + LevelerInputDataStruct input; + LevelerOutputDataStruct output; +} LevelerDataStruct; + +extern LevelerDataStruct gLeveler; + +#endif /* _COMPASS_H_ */ diff --git a/examples/Leveler/src/user/UserAlgorithm.c b/examples/Leveler/src/user/UserAlgorithm.c new file mode 100644 index 0000000..0c82da1 --- /dev/null +++ b/examples/Leveler/src/user/UserAlgorithm.c @@ -0,0 +1,210 @@ +/** *************************************************************************** + * @file UserAlgorithm.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include + +#include "algorithmAPI.h" +#include "gpsAPI.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "Indices.h" +#include "GlobalConstants.h" + +#include "math.h" +#include "VectorMath.h" + + +// +#include "algorithm.h" +#include "UserAlgorithm.h" + +#include "bsp.h" +#include "debug.h" + +// Declare the leveler data structure +#include "Leveler.h" + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _InitAlgo(uint8_t algoType); + +// Initialize leveler algorithm variables +void InitUserAlgorithm() +{ + // Initialize built-in algorithm structure + Leveler_InitializeAlgorithmStruct(FREQ_200_HZ); + + // place additional required initialization here +} + + +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) +{ + // This can be set at startup based on the packet type selected + static uint8_t algoType = IMU; + + // Initialize variable related to the UserNavAlgorithm + _InitAlgo(algoType); + + // Populate the leveler input data structure. Load the GPS data + // structure as NULL. + Leveler_SetInputStruct(accels, rates, mags, NULL); + + // Call the desired algorithm based on the EKF with different + // calling rates and different settings. + _Algorithm(dacqRate, algoType); + + // Fill the output data structure with the algorithm states and other + // desired information + Leveler_SetOutputStruct(); + + // Generate a debug message that provides algorithm output to verify the + // algorithm is generating the proper output. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // The returned value from this function is unused by external functions. The + // NULL pointer is returned instead of a data structure. + return NULL; +} + + +// +static void _InitAlgo(uint8_t algoType) +{ + // Initialize the timer variables + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the configuration variables for a VG-type solution + // (useMags = 0 forces the VG solution) + gAlgorithm.Behavior.bit.freeIntegrate = 0; + gAlgorithm.Behavior.bit.useMag = 0; + gAlgorithm.Behavior.bit.useGPS = 0; + gAlgorithm.Behavior.bit.stationaryLockYaw = 0; + gAlgorithm.Behavior.bit.restartOnOverRange = 0; + gAlgorithm.Behavior.bit.dynamicMotion = 1; + + // Set the system configuration based on system type + switch( algoType ) { + case VG: + // Nothing additional to do (already configured for a VG + // solution) + break; + case AHRS: + // Set the configuration variables for AHRS solution + // (useMags = 1 and enable mags) + enableMagInAlgorithm(TRUE); + gAlgorithm.Behavior.bit.useMag = 1; + break; + case INS: + // Nothing additional to do (already configured for a VG + // solution) + enableMagInAlgorithm(TRUE); + gAlgorithm.callingFreq = FREQ_100_HZ; // redundant; set above + gAlgorithm.Behavior.bit.useMag = 1; + gAlgorithm.Behavior.bit.useGPS = 1; + break; + default: + // Nothing to do + break; + } + } +} + + +// +static void _Algorithm(uint16_t dacqRate, uint8_t algoType) +{ + // + static uint8_t algoCntr = 0, algoCntrLimit = 0; + + // Initialize the configuration variables needed to make the system + // generate a VG-type solution. + static uint8_t initAlgo = 1; + if(initAlgo) { + // Reset 'initAlgo' so this is not executed more than once. This + // prevents the algorithm from being switched during run-time. + initAlgo = 0; + + // Set the variables that control the algorithm execution rate + algoCntrLimit = (uint8_t)( (float)dacqRate / (float)gLeveler.callingFreq + 0.5 ); + if( algoCntrLimit < 1 ) { + // If this logic is reached, also need to adjust the algorithm + // parameters to match the modified calling freq (or stop the + // program to indicate that the user must adjust the program) + algoCntrLimit = 1; + } + algoCntr = algoCntrLimit; + } + + // Increment the counter. If greater than or equal to the limit, reset + // the counter to cause the algorithm to run on the next pass through. + algoCntr++; + if(algoCntr >= algoCntrLimit) { + // Reset counter + algoCntr = 0; + + // Aceinna VG/AHRS/INS algorithm + Leveler_Algorithm(); + } +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the output of the EKF algorithm + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; + DebugPrintFloat("Time: ", gLeveler.timerCntr * 0.001, 3); + DebugPrintFloat(", Roll: ", gLeveler.output.measuredEulerAngles_BinN[ROLL], 5); + DebugPrintFloat(", Pitch: ", gLeveler.output.measuredEulerAngles_BinN[PITCH], 5); + DebugPrintEndline(); + } + } +} diff --git a/examples/Leveler/src/user/UserAlgorithm.h b/examples/Leveler/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/Leveler/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/LEVELER/src/user/UserConfiguration.c b/examples/Leveler/src/user/UserConfiguration.c similarity index 94% rename from examples/LEVELER/src/user/UserConfiguration.c rename to examples/Leveler/src/user/UserConfiguration.c index f44d229..a41850a 100644 --- a/examples/LEVELER/src/user/UserConfiguration.c +++ b/examples/Leveler/src/user/UserConfiguration.c @@ -24,12 +24,14 @@ limitations under the License. *******************************************************************************/ #include "string.h" -#include "UserConfiguration.h" -#include "UserMessaging.h" -#include "Indices.h" + #include "algorithmAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "UserConfiguration.h" +#include "UserMessaging.h" +#include "Indices.h" // Default user configuration structure // Applied to unit upon reception of "zR" command @@ -38,15 +40,18 @@ limitations under the License. const UserConfigurationStruct gDefaultUserConfig = { .dataCRC = 0, .dataSize = sizeof(UserConfigurationStruct), - .userUartBaudRate = 115200, - .userPacketType = "l1", - .userPacketRate = 5, + .userUartBaudRate = 115200, + .userPacketType = "l1", + .userPacketRate = 10, .lpfAccelFilterFreq = 50, .lpfRateFilterFreq = 50, - .orientation = "+X+Y+Z" + .orientation = "+X+Y+Z", // add default parameter values here, if desired -} ; - + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 +}; UserConfigurationStruct gUserConfiguration; UserConfigurationStruct gTmpUserConfiguration; @@ -54,7 +59,22 @@ UserConfigurationStruct gTmpUserConfiguration; uint8_t UserDataBuffer[4096]; volatile char *info; BOOL configValid = FALSE; -//extern BOOL setUserPacketType(int type, B); + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} void userInitConfigureUnit() @@ -98,7 +118,6 @@ void userInitConfigureUnit() } info = getBuildInfo(); - } @@ -159,6 +178,8 @@ BOOL UpdateSystemParameter(uint32_t number, uint64_t data, BOOL fApply) return result; } + + /** *************************************************************************** * @name UpdateUserParameter - updating user configuration parameter based of preferences * @brief @@ -261,9 +282,9 @@ BOOL UpdateUserConfig(userConfigPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateUserParam * @brief writes user data into user configuration structure, validates data if @@ -305,9 +326,9 @@ BOOL UpdateUserParam(userParamPayload* pld, uint8_t *payloadLen) *payloadLen = 4; return TRUE; - } + /** **************************************************************************** * @name UpdateAllUserParams * @brief writes user data into user configuration structure, validates data if @@ -380,8 +401,6 @@ BOOL UpdateAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) } - - /** **************************************************************************** * @name GetUserConfig * @brief Retrieves specified number of user configuration parameters data for @@ -421,6 +440,7 @@ BOOL GetUserConfig(userConfigPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetUserParam * @brief Retrieves specified number of user configuration parameters data for @@ -452,6 +472,7 @@ BOOL GetUserParam(userParamPayload* pld, uint8_t *payloadLen) } + /** **************************************************************************** * @name GetAllUserParams * @brief Retrieves specified number of user configuration parameters data for @@ -504,6 +525,7 @@ BOOL SaveUserConfig(void) } + BOOL RestoreDefaultUserConfig(void) { BOOL valid = TRUE; diff --git a/examples/LEVELER/src/user/UserConfiguration.h b/examples/Leveler/src/user/UserConfiguration.h similarity index 92% rename from examples/LEVELER/src/user/UserConfiguration.h rename to examples/Leveler/src/user/UserConfiguration.h index 98abedc..e6fb5f3 100644 --- a/examples/LEVELER/src/user/UserConfiguration.h +++ b/examples/Leveler/src/user/UserConfiguration.h @@ -20,14 +20,13 @@ limitations under the License. #ifndef USER_CONFIGURATION_H #define USER_CONFIGURATION_H + #include + #include "GlobalConstants.h" #include "UserMessaging.h" #include "filter.h" - -#include - /// User defined configuration strucrture ///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types @@ -64,7 +63,6 @@ typedef struct { /// 25 - Butterworth LPF 25HZ /// 40 - Butterworth LPF 40HZ -// int64_t orientation; /// unit orientation in format 0x0000000000ddrrff uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff /// where dd - down axis, rr - right axis, ff - forward axis /// next axis values a valid : @@ -80,9 +78,15 @@ typedef struct { // Add parameter offset in UserConfigParamOffset structure if validation or // special processing required + float hardIron_X; + float hardIron_Y; + float softIron_Ratio; + float softIron_Angle; } UserConfigurationStruct; typedef enum{ +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) USER_CRC = 0, USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters @@ -91,15 +95,18 @@ typedef enum{ USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation -// add system parameters here and reassign USER_LAST_SYSTEM_PARAM - USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, + USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct + USER_HARD_IRON_X , + USER_HARD_IRON_Y , + USER_SOFT_IRON_RATIO , + USER_SOFT_IRON_ANGLE , USER_MAX_PARAM } UserConfigParamNumber; #define MAX_SYSTEM_PARAM USER_ORIENTATION - extern int userPacketOut; #define USER_OK 0x00 diff --git a/examples/LEVELER/src/user/UserMessaging.c b/examples/Leveler/src/user/UserMessaging.c similarity index 80% rename from examples/LEVELER/src/user/UserMessaging.c rename to examples/Leveler/src/user/UserMessaging.c index 25e8226..1b90cd1 100644 --- a/examples/LEVELER/src/user/UserMessaging.c +++ b/examples/Leveler/src/user/UserMessaging.c @@ -26,24 +26,33 @@ limitations under the License. #include #include #include -#include "UserMessaging.h" -#include "UserConfiguration.h" + #include "algorithmAPI.h" -#include "userAPI.h" #include "platformAPI.h" #include "sensorsAPI.h" -#include "Indices.h" +#include "userAPI.h" + +#include "UserMessaging.h" +#include "UserConfiguration.h" + +#include "Indices.h" // For X_AXIS, etc -// provided as example +// Declare the IMU data structure +IMUDataStruct gIMU; +gpsDataStruct_t gGPS; + +// Version string char userVersionString[] = "StaticLeveler 1.0.0"; #include "UserAlgorithm.h" // for EKFOutputDataStruct + +#include "Leveler.h" LevelerDataStruct *algo_res; /// List of allowed packet codes -usr_packet_t userInputPackets[] = { // - {USR_IN_NONE, {0,0}}, // " " +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, {USR_IN_PING, "pG"}, {USR_IN_UPDATE_CONFIG, "uC"}, {USR_IN_UPDATE_PARAM, "uP"}, @@ -60,7 +69,6 @@ usr_packet_t userInputPackets[] = { // }; - // packet codes here should be unique - // should not overlap codes for input packets and system packets // First byte of Packet code should have value >= 0x61 @@ -70,12 +78,11 @@ usr_packet_t userOutputPackets[] = { {USR_OUT_TEST, "zT"}, {USR_OUT_DATA1, "z1"}, // place new type and code here + {USR_OUT_SCALED1, "s1"}, {USR_OUT_LEV1, "l1"}, {USR_OUT_MAX, {0xff, 0xff}}, // "" }; - - volatile char *info; static int _userPayloadLen = 0; static int _outputPacketType = USR_OUT_MAX; @@ -116,7 +123,6 @@ int checkUserPacketType(uint16_t receivedCode) void userPacketTypeToBytes(uint8_t bytes[]) { - if(_inputPacketType && _inputPacketType < USR_IN_MAX){ // response to request. Return same packet code bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; @@ -167,6 +173,10 @@ BOOL setUserPacketType(uint8_t *data, BOOL fApply) _outputPacketType = type; _userPayloadLen = USR_OUT_DATA1_PAYLOAD_LEN; break; + case USR_OUT_SCALED1: // packet with arbitrary data + _outputPacketType = type; + _userPayloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + break; case USR_OUT_LEV1: // packet with sensors data. Change at will _outputPacketType = type; _userPayloadLen = USR_OUT_LEV1_PAYLOAD_LEN; @@ -213,8 +223,6 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) // userPacket *pkt = (userPacket *)ptrUcbPacket->payload; /// call appropriate function based on packet type - - switch (_inputPacketType) { case USR_IN_RESET: Reset(); @@ -285,6 +293,7 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) return ret; } + /****************************************************************************** * @name HandleUserOutputPacket - API call ro prepare continuous user output packet * @brief general handler @@ -299,39 +308,84 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) switch (_outputPacketType) { case USR_OUT_TEST: - { uint32_t *testParam = (uint32_t*)(payload); + { + uint32_t *testParam = (uint32_t*)(payload); *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; *testParam = _testVal++; } break; + case USR_OUT_DATA1: - { int n = 0; - double accels[3]; - double mags[3]; - double rates[3]; + { + int n = 0; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; data1_payload_t *pld = (data1_payload_t *)payload; - pld->timer = getDacqTime(); + pld->timer = platformGetDacqTime(); GetAccelData_mPerSecSq(accels); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)accels[i]; } GetRateData_degPerSec(rates); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)rates[i]; } GetMagData_G(mags); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)mags[i]; } *payloadLen = sizeof(data1_payload_t); - break; } + break; + + case USR_OUT_SCALED1: + { + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 1 floats (4 bytes) = 4 bytes + // ================================= + // NumOfBytes = 52 bytes + *payloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; + + // Output time as represented by gIMU.timerCntr (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gIMU.timerCntr; + + // Output a double representation of time generated from + // gLeveler.itow + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gIMU.timerCntr); + + // Set the pointer of the sensor array to the payload + float *algoData_3 = (float*)(algoData_2); + *algoData_3++ = (float)gIMU.accel_g[X_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Y_AXIS]; + *algoData_3++ = (float)gIMU.accel_g[Z_AXIS]; + + *algoData_3++ = (float)gIMU.rate_degPerSec[X_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Y_AXIS]; + *algoData_3++ = (float)gIMU.rate_degPerSec[Z_AXIS]; + + *algoData_3++ = (float)gIMU.mag_G[X_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Y_AXIS]; + *algoData_3++ = (float)gIMU.mag_G[Z_AXIS]; + + *algoData_3++ = (float)gIMU.temp_C; + } + break; + case USR_OUT_LEV1: { // Variables used to hold the EKF values - real EulerAngles[3]; - double accels[3]; + real EulerAngles[NUM_AXIS]; + double accels[NUM_AXIS]; // The payload length (NumOfBytes) is based on the following: // 1 uint32_t (4 bytes) = 4 bytes @@ -354,7 +408,6 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) // Set the pointer of the algoData array to the payload float *algoData_3 = (float*)(algoData_2); - Leveler_GetAttitude_EA(EulerAngles); *algoData_3++ = (float)EulerAngles[ROLL]; *algoData_3++ = (float)EulerAngles[PITCH]; @@ -365,6 +418,7 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *algoData_3++ = (float)accels[Z_AXIS]; } break; + // place additional user packet preparing calls here // case USR_OUT_XXXX: // *payloadLen = YYYY; // total user payload length, including user packet type @@ -372,9 +426,11 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) // prepare dada here // break; default: - *payloadLen = 0; - ret = FALSE; - break; /// unknown user packet, will send error in response + { + *payloadLen = 0; + ret = FALSE; + } + break; /// unknown user packet, will send error in response } return ret; @@ -386,7 +442,3 @@ void WriteResultsIntoOutputStream(void *results) // implement specific data processing/saving here algo_res = results; } - - - - diff --git a/examples/LEVELER/src/user/UserMessaging.h b/examples/Leveler/src/user/UserMessaging.h similarity index 91% rename from examples/LEVELER/src/user/UserMessaging.h rename to examples/Leveler/src/user/UserMessaging.h index a77a33b..d846593 100644 --- a/examples/LEVELER/src/user/UserMessaging.h +++ b/examples/Leveler/src/user/UserMessaging.h @@ -1,6 +1,6 @@ /******************************************************************************* * File: UserConfiguration.h - * Created on JAn 25, 2017 + * Created on Jan 25, 2017 ******************************************************************************/ /******************************************************************************* Copyright 2018 ACEINNA, INC @@ -20,17 +20,16 @@ limitations under the License. #ifndef USER_MESSAGING_H #define USER_MESSAGING_H + #include + #include "GlobalConstants.h" #include "ucb_packet_struct.h" - -#include #define USER_PACKET_OK 0 #define UNKNOWN_USER_PACKET 1 #define USER_PACKET_ERROR 2 - // here is definition for packet rate divider // considering that data acquisition task runs at 200 Hz typedef enum { @@ -74,11 +73,12 @@ typedef enum { // User output packet codes, change at will typedef enum { - USR_OUT_NONE = 0, // 0 - USR_OUT_TEST, // 1 - USR_OUT_DATA1, // 2 + USR_OUT_NONE = 0, + USR_OUT_TEST, + USR_OUT_DATA1, // add new output packet type here, before USR_OUT_MAX - USR_OUT_LEV1, // 3 + USR_OUT_SCALED1, + USR_OUT_LEV1, USR_OUT_MAX } UserOutPacketType; @@ -122,24 +122,34 @@ typedef struct { #define USR_OUT_TEST_PAYLOAD_LEN (4) // test parameter (uint32_t) #define USR_OUT_DATA1_PAYLOAD_LEN (4*9) // 3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) +#define USR_OUT_SCALED1_PAYLOAD_LEN (52) #define USR_OUT_LEV1_PAYLOAD_LEN (32) -extern int userPacketOut; - #define USER_OK 0x00 #define USER_NAK 0x80 #define USER_INVALID 0x81 - - extern int userPacketOut; - extern int getUserPayloadLength(void); extern int checkUserPacketType(uint16_t receivedCode); extern void userPacketTypeToBytes(uint8_t bytes[]); extern void WriteResultsIntoOutputStream(void *results); BOOL setUserPacketType(uint8_t* type, BOOL fApply); -#endif /* USER_CONFIGURATION_H */ +// IMU data structure +typedef struct { + // Timer output counter + uint32_t timerCntr, dTimerCntr; + // Algorithm states + double accel_g[3]; + double rate_radPerSec[3]; + double rate_degPerSec[3]; + double mag_G[3]; + double temp_C; +} IMUDataStruct; + +extern IMUDataStruct gIMU; + +#endif /* USER_CONFIGURATION_H */ diff --git a/examples/Leveler/src/user/dataProcessingAndPresentation.c b/examples/Leveler/src/user/dataProcessingAndPresentation.c new file mode 100644 index 0000000..4101cf0 --- /dev/null +++ b/examples/Leveler/src/user/dataProcessingAndPresentation.c @@ -0,0 +1,187 @@ +/***************************************************************************** + * @file dataProcessingAndPresentation.c + * + * THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * contains of data post-processing framework + ******************************************************************************/ +/******************************************************************************* +Copyright 2018 ACEINNA, INC + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*******************************************************************************/ + +#include "userAPI.h" +#include "sensorsAPI.h" +#include "gpsAPI.h" +#include "UserMessaging.h" + +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + +// Local-function prototypes +static void _IncrementIMUTimer(uint16_t dacqRate); +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq); +static void _IMUDebugMessage(void); + +/* * + ****************** + * + Sensors data processing flow (from left to right) + +Raw *************** Filtered * ************ Calibrated **************** ********************* Data +Data * Built-in * Raw Data * * Data * User Filter s* * Algorithm * Output +****** LP Filter ************* Calibration ************* (if desired) ***** (Kalman Filter ******** + * * * * * * * or user Algorithm)* + ************* * ************** **************** ********************* + ^^^^^^^ + Can be selected during + initialization or turned + OFF +*///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> + + +// Next function is common for all platforms, but implementation of the methods inside is platform-dependent +// Call to this function made from DataAcquisitionTask during initialization phase +// All user algorithm and data structures should be initialized here, if used +void initUserDataProcessingEngine() +{ + InitUserAlgorithm(); // default implementation located in file user_algorithm.c +} + + +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +void inertialAndPositionDataProcessing(uint16_t dacqRate) +{ + // + void *results; + + // Increment the IMU timer by the calling rate of the data-acquisition task + _IncrementIMUTimer(dacqRate); + + // Obtain accelerometer data [g] + GetAccelData_g(gIMU.accel_g); + + // Obtain rate-sensor data [rad/sec] + GetRateData_radPerSec(gIMU.rate_radPerSec); + GetRateData_degPerSec(gIMU.rate_degPerSec); + + // Obtain magnetometer data [G] + GetMagData_G(gIMU.mag_G); + + // Obtain board temperature data [degC] + GetBoardTempData(&gIMU.temp_C); + + // Generate a debug message that provide sensor data in order to verify the + // algorithm input data is as expected. + _GenerateDebugMessage(dacqRate, ZERO_HZ); + + // Execute user algorithm (default implementation located in file user_algorithm.c) + results = RunUserNavAlgorithm(gIMU.accel_g, gIMU.rate_radPerSec, gIMU.mag_G, NULL, dacqRate); + + // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure + WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c +} + + +// +static void _IncrementIMUTimer(uint16_t dacqRate) +{ + // Initialize timer variables (used to control the output of the debug + // messages and the IMU timer value) + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the IMU output delta-counter value + gIMU.dTimerCntr = (uint32_t)( 1000.0 / (float)(dacqRate) + 0.5 ); + } + + // Increment the timer-counter by the sampling period equivalent to the + // rate at which inertialAndPositionDataProcessing is called + gIMU.timerCntr = gIMU.timerCntr + gIMU.dTimerCntr; +} + + +// +static void _GenerateDebugMessage(uint16_t dacqRate, uint16_t debugOutputFreq) +{ + // Variables that control the output frequency of the debug statement + static uint8_t debugOutputCntr, debugOutputCntrLimit; + + // Check debug flag. If set then generate the debug message to verify + // the loading of the GPS data into the GPS data structure + if( debugOutputFreq > ZERO_HZ ) { + // Initialize variables used to control the output of the debug messages + static int initFlag = 1; + if(initFlag) { + // Reset 'initFlag' so this section is not executed more than once. + initFlag = 0; + + // Set the variables that control the debug-message output-rate (based on + // the desired calling frequency of the debug output) + debugOutputCntrLimit = (uint8_t)( (float)dacqRate / (float)debugOutputFreq + 0.5 ); + debugOutputCntr = debugOutputCntrLimit; + } + + debugOutputCntr++; + if(debugOutputCntr >= debugOutputCntrLimit) { + debugOutputCntr = 0; + + // Create message here + static uint8_t msgType = 1; + switch( msgType ) + { + case 0: + // None + break; + + case 1: + // IMU data + _IMUDebugMessage(); + break; + + case 2: + // + break; + } + } + } +} + + +static void _IMUDebugMessage(void) +{ + // IMU Data + DebugPrintFloat("Time: ", 0.001 * (real)gIMU.timerCntr, 3); + DebugPrintFloat(", a: [ ", (float)gIMU.accel_g[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.accel_g[Z_AXIS], 3); + DebugPrintFloat(" ], w: [ ", (float)gIMU.rate_radPerSec[X_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Y_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ", (float)gIMU.rate_radPerSec[Z_AXIS] * RAD_TO_DEG, 3); + DebugPrintFloat(" ], m: [ ", (float)gIMU.mag_G[X_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Y_AXIS], 3); + DebugPrintFloat(" ", (float)gIMU.mag_G[Z_AXIS], 3); + DebugPrintString(" ]"); + DebugPrintEndline(); +} diff --git a/examples/VG_AHRS/include/API/userAPI.h b/examples/VG_AHRS/include/API/userAPI.h index 986a71f..88b2ad4 100644 --- a/examples/VG_AHRS/include/API/userAPI.h +++ b/examples/VG_AHRS/include/API/userAPI.h @@ -26,12 +26,25 @@ limitations under the License. #ifndef _USER_API_H #define _USER_API_H +// Some common constants used in the user algorithm logic +#define ZERO_HZ 0 +#define ONE_HZ 1 +#define TWO_HZ 2 +#define FOUR_HZ 4 +#define FIVE_HZ 5 +#define TEN_HZ 10 +#define TWENTY_HZ 20 +#define TWENTY_FIVE_HZ 25 +#define FIFTY_HZ 50 + +#define NUM_AXIS 3 + #include #include "gpsAPI.h" -void inertialAndPositionDataProcessing(int dacqRate); +void inertialAndPositionDataProcessing(uint16_t dacqRate); -void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, int dacqRate); +void *RunUserNavAlgorithm(double *accels, double *rates, double* mags, gpsDataStruct_t *gps, uint16_t dacqRate); void WriteResultsIntoOutputStream(void *results) ; void InitUserDataStructures(); void InitUserFilters(); @@ -39,5 +52,4 @@ void InitUserAlgorithm(); void initUserDataProcessingEngine(); void userInitConfigureUnit(); - #endif diff --git a/examples/VG_AHRS/include/FreeRTOSConfig.h b/examples/VG_AHRS/include/FreeRTOSConfig.h index 690eca8..80ca6b9 100644 --- a/examples/VG_AHRS/include/FreeRTOSConfig.h +++ b/examples/VG_AHRS/include/FreeRTOSConfig.h @@ -96,7 +96,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES (7) #define configMINIMAL_STACK_SIZE ((uint16_t)2048) -#define configTOTAL_HEAP_SIZE ((size_t)(30 * 2048)) +#define configTOTAL_HEAP_SIZE ((size_t)(38000)) #define configMAX_TASK_NAME_LEN (30) #define configUSE_TRACE_FACILITY 1 #define configUSE_16_BIT_TICKS 0 @@ -109,6 +109,7 @@ #define configUSE_APPLICATION_TASK_TAG 0 #define configUSE_COUNTING_SEMAPHORES 1 #define configGENERATE_RUN_TIME_STATS 0 +#define configAPPLICATION_ALLOCATED_HEAP 0 /* Co-routine definitions. */ #define configUSE_CO_ROUTINES 0 diff --git a/examples/VG_AHRS/include/README b/examples/VG_AHRS/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/examples/VG_AHRS/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/examples/VG_AHRS/include/taskDataAcquisition.h b/examples/VG_AHRS/include/taskDataAcquisition.h index a02b74a..891fb4e 100644 --- a/examples/VG_AHRS/include/taskDataAcquisition.h +++ b/examples/VG_AHRS/include/taskDataAcquisition.h @@ -27,9 +27,7 @@ limitations under the License. #ifndef _TASK_DATA_ACQUISITION_H_ #define _TASK_DATA_ACQUISITION_H_ -// Specify the limit used in the int16-limiter -#define INT16_LIMIT 32765 -#define INT12_LIMIT 2045 + #include "stdint.h" #include "GlobalConstants.h" extern void TaskDataAcquisition(void const *argument); @@ -40,8 +38,4 @@ extern void EnterMainAlgLoop(void); extern void DataAquisitionStart(void); extern BOOL isOneHundredHertzFlag(void); -int16_t LimitInt16Value( int16_t value, int16_t limit ); -extern uint32_t dacqTimer; - - -#endif \ No newline at end of file +#endif diff --git a/examples/VG_AHRS/lib/README b/examples/VG_AHRS/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/examples/VG_AHRS/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/examples/VG_AHRS/platformio.ini b/examples/VG_AHRS/platformio.ini index 75d8361..dc4c342 100644 --- a/examples/VG_AHRS/platformio.ini +++ b/examples/VG_AHRS/platformio.ini @@ -14,15 +14,29 @@ description = A Kalman filter based algorithm that uses rate-sensors to propagat platform = aceinna_imu lib_archive = false board = OpenIMU300ZA -lib_deps = openimu-core-libraries@^1.0.2 +;lib_deps= +; ../../openimu-lib/Misc +; ../../openimu-lib/Platform +;lib_deps = ../../../openimu-core +lib_deps = openimu-core-libraries@^1.0.22 build_flags = -D CLI + -D __FPU_PRESENT + -D ARM_MATH_CM4 -I include -I include/API -I src/user +; -L ldscripts + -O0 +; -Wno-comment -Wl,-Map,imu380.map +; -Wl,-Tstm32f40x.ld -mthumb -mcpu=cortex-m4 -mfloat-abi=softfp -mfpu=fpv4-sp-d16 ;upload_protocol = jlink ;debug_tool = jlink + +;debug_tool = custom +;debug_port = :4242 +;debug_server = $PLATFORMIO_HOME_DIR/packages/tool-stlink/bin/st-util diff --git a/examples/VG_AHRS/src/main.c b/examples/VG_AHRS/src/main.c index 21e538a..c4bbfb1 100644 --- a/examples/VG_AHRS/src/main.c +++ b/examples/VG_AHRS/src/main.c @@ -29,25 +29,24 @@ limitations under the License. #define __MAIN #include + #include "boardAPI.h" +#include "magAPI.h" #include "platformAPI.h" #include "userAPI.h" + #include "debug.h" + #include "taskDataAcquisition.h" #include "taskUserCommunication.h" -#include "magAPI.h" + #include "osapi.h" #include "osresources.h" - - - - #ifdef CLI #include "commandLine.h" #endif - #ifdef GPS #include "gpsAPI.h" #endif @@ -83,24 +82,29 @@ void DebugInterfaceInit(void) { char status[100]; + int debugChannel = platformGetSerialChannel(DEBUG_SERIAL_PORT); + + if(debugChannel == UART_CHANNEL_NONE){ + //nothing to do + return; + } + // Add a delay to allow the system to stabilize after the reset line (nRst) // is released for(int i = 0; i < 4000000; i++) ; // TODO - calculate more precisely //DelayMs(300); //todo - calculate // Initialize the DEBUG USART (serial) port - InitDebugSerialCommunication(115200); // debug_usart.c + InitDebugSerialCommunication(230400); // debug_usart.c //DEBUG_STRING("\r\nDMU380 System\r\n"); BoardGetResetStatus(status, sizeof(status)); ERROR_STRING(status); - } void CreateTasks(void) { - osThreadId iD; /// Create RTOS tasks: @@ -110,12 +114,14 @@ void CreateTasks(void) if(iD == NULL){ while(1); } + //user communication task osThreadDef(USER_COMM_TASK, TaskUserCommunication, osPriorityNormal, 0, 2048); iD = osThreadCreate(osThread(USER_COMM_TASK), NULL); if(iD == NULL){ while(1); } + gyroReadySem = osSemaphoreCreate(osSemaphore(GYRO_READY_SEM), 1); accelReadySem = osSemaphoreCreate(osSemaphore(ACCEL_READY_SEM), 1); magReadySem = osSemaphoreCreate(osSemaphore(MAG_READY_SEM), 1); @@ -123,6 +129,19 @@ void CreateTasks(void) navDataReadySem = osSemaphoreCreate(osSemaphore(NAV_DATA_READY_SEM), 1); dataAcqSem = osSemaphoreCreate(osSemaphore(DATA_ACQ_SEM), 1); +#ifdef GPS + osThreadDef(GPS_TASK, TaskGps, osPriorityBelowNormal, 0, 1024); + iD = osThreadCreate(osThread(GPS_TASK), NULL); + if(iD == NULL){ + while(1); + } + osThreadDef(WMM_TASK, TaskWorldMagneticModel, osPriorityLow, 0, 512); + iD = osThreadCreate(osThread(WMM_TASK), NULL); + if(iD == NULL){ + while(1); + } +#endif + #ifdef CLI osThreadDef(CLI_TASK, TaskCommandLine, osPriorityLow, 0, 1024); iD = osThreadCreate(osThread(CLI_TASK), NULL); @@ -130,8 +149,8 @@ void CreateTasks(void) while(1); } cliSem = osSemaphoreCreate(osSemaphore(CLI_SEM), 1); + platformRegisterRxSerialSemaphoreID(DEBUG_SERIAL_PORT, cliSem); #endif - } /** *************************************************************************** @@ -148,13 +167,37 @@ int main(void) // Initialize processor and board-related signals BoardInit(); + platformSetUnitCommunicationType(UART_COMM); + + // Un-assign and reassign ports +#ifdef GPS + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_1); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid +#else + platformUnassignSerialChannels(); + + BOOL res; + res = platformAssignPortTypeToSerialChannel(USER_SERIAL_PORT, UART_CHANNEL_0); + while(!res){}; // check if valid + res = platformAssignPortTypeToSerialChannel(DEBUG_SERIAL_PORT, UART_CHANNEL_2); + while(!res){}; // check if valid + //res = platformAssignPortTypeToSerialChannel(GPS_SERIAL_PORT, UART_CHANNEL_1); + //while(!res){}; // check if valid +#endif + // Apply factory configuration platformInitConfigureUnit(); // Apply user-chosen configuration userInitConfigureUnit(); - // Initialize OS and create required tasks CreateTasks(); @@ -182,4 +225,4 @@ void vApplicationStackOverflowHook() void vApplicationMallocFailedHook() { while(1); -} \ No newline at end of file +} diff --git a/examples/VG_AHRS/src/taskDataAcquisition.c b/examples/VG_AHRS/src/taskDataAcquisition.c index a4ed7aa..ecbf729 100644 --- a/examples/VG_AHRS/src/taskDataAcquisition.c +++ b/examples/VG_AHRS/src/taskDataAcquisition.c @@ -25,28 +25,18 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#include "taskDataAcquisition.h" -#include "taskUserCommunication.h" -#include "UserConfiguration.h" -#include "magAPI.h" -#include "bitAPI.h" -//#include "boardAPI.h" -#include "bsp.h" #include "algorithmAPI.h" -#include "Indices.h" -#include "qmath.h" -#include "osapi.h" -#include "osresources.h" -#include "userAPI.h" +#include "bitAPI.h" +#include "boardAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "userAPI.h" -uint32_t dacqTimer = 0; - -uint32_t getDacqTime() -{ - return dacqTimer; -} +#include "taskDataAcquisition.h" +#include "taskUserCommunication.h" +#include "osapi.h" +#include "osresources.h" /** *************************************************************************** * @name TaskDataAcquisition() CALLBACK main loop @@ -58,13 +48,14 @@ uint32_t getDacqTime() ******************************************************************************/ void TaskDataAcquisition(void const *argument) { - // uint8_t functionStatus; - int res, dacqRate; + // + int res; + uint16_t dacqRate; + #pragma GCC diagnostic ignored "-Wunused-but-set-variable" BOOL overRange = FALSE; //uncomment this line if overrange processing required #pragma GCC diagnostic warning "-Wunused-but-set-variable" - // This routine sets up the timer. Can use the structure below this point. TaskDataAcquisition_Init(); // Initialize user algorithm parameters, if needed @@ -73,21 +64,19 @@ void TaskDataAcquisition(void const *argument) InitMagAlignParams(); // Start sensors data acquisition DataAquisitionStart(); - // determine the period of data acquisition task - dacqRate = DACQ_200_HZ; - + // Set the sampling frequency of data acquisition task + dacqRate = DACQ_200_HZ; //************** Add user initialization here, if needed **************** - - /// Data is provided by the primary sensors (accelerometer and rate-sensor) - /// as fast as possible. Data is obtained from secondary sensors at a - /// lower rate. The rate-sensor data read is synced to TIM5 or an - /// external signal. When the read is commanded and in the data buffer, - /// The data-event flag is set and the wait is bypassed. Data is obtained - /// from the buffer, calibrated, filtered and provided to the user for - /// subsequent processing. + // Data is provided by the primary sensors (accelerometer and rate-sensor) + // as fast as possible. Data is obtained from secondary sensors at a + // lower rate. The rate-sensor data read is synced to TIM5 or an + // external signal. When the read is commanded and in the data buffer, + // The data-event flag is set and the wait is bypassed. Data is obtained + // from the buffer, calibrated, filtered and provided to the user for + // subsequent processing. while( 1 ) { @@ -141,21 +130,22 @@ void TaskDataAcquisition(void const *argument) // Temperature - degrees C //****************************************************************** - // BIT status. May have inadvertently changed this during an update. updateBITandStatus(); // ********************** Algorithm ************************ - // In AHRS or INS mode due to CPU performance concerns algorithm better be - // performed at 100 or less Hz based on complexity and number calculations - // involved. Incorporate timing logic inside algorithm function, if desired - // For the initial simplicity use pScaledSensors array as input - // and output of algorithm. - // Use is100Hz variable to determine the rate of this task loop + // Due to CPU performance related to algorithm math operations, GPS related + // algorithms are run at 100 Hz or less (large number of calculations + // involved). Incorporate timing logic inside algorithm function, if + // desired. + // + // For the initial simplicity use pScaledSensors array as algorithm input + // and output. Use 'is100Hz' variable to determine the rate of this task + // loop. inertialAndPositionDataProcessing(dacqRate); - //Uncomment next line if there is intention of using S0 or S1 xbow packets - //for continuous data output + // Uncomment next line if there is intention of using S0 or S1 xbow + // packets for continuous data output //***************************************************************** // applyNewScaledSensorsData(); //***************************************************************** @@ -166,13 +156,13 @@ void TaskDataAcquisition(void const *argument) setIO2Pin(0); if (platformHasMag() ) { - // Mag Alignment (follows Kalman filter or user algorithm as the innovation routine - // calculates the euler angles and the magnetic vector in the - // NED-frame) + // Mag Alignment (follows Kalman filter or user algorithm as the + // innovation routine calculates the euler angles and the magnetic + // vector in the NED-frame) MagAlign(); // only does this on align } - if(getUnitCommunicationType() != UART_COMM){ + if(platformGetUnitCommunicationType() != UART_COMM){ // Perform interface - specific processing here } else { // Process commands and output continuous packets to UART @@ -182,4 +172,3 @@ void TaskDataAcquisition(void const *argument) } } } - diff --git a/examples/VG_AHRS/src/user/UserAlgorithm.c b/examples/VG_AHRS/src/user/UserAlgorithm.c index 4970266..e495aa0 100644 --- a/examples/VG_AHRS/src/user/UserAlgorithm.c +++ b/examples/VG_AHRS/src/user/UserAlgorithm.c @@ -23,35 +23,39 @@ See the License for the specific language governing permissions and limitations under the License. *******************************************************************************/ -#include "userAPI.h" +#include + +#include "algorithmAPI.h" #include "gpsAPI.h" -#include "stddef.h" +#include "platformAPI.h" +#include "userAPI.h" + +#include "Indices.h" #include "GlobalConstants.h" -#include "gpsAPI.h" -#include "algorithmAPI.h" -extern void InitializeAlgorithmStruct(void); #include "algorithm.h" #include "EKF_Algorithm.h" -#include "platformAPI.h" #include "BitStatus.h" +#include "bsp.h" +#include "debug.h" + +#include "MagAlign.h" // void _Algorithm(int dacqRate, uint8_t algoType); +// Initialize GPS algorithm variables void InitUserAlgorithm() { // Initialize built-in algorithm structure - setAlgorithmExeFreq(FREQ_200_HZ); - InitializeAlgorithmStruct(); + InitializeAlgorithmStruct(FREQ_200_HZ); + // place additional required initialization here } -#include "bsp.h" -#include "debug.h" -void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, int dacqRate) +void *RunUserNavAlgorithm(double *accels, double *rates, double *mags, gpsDataStruct_t *gps, uint16_t dacqRate) { // Initialization variable static int initAlgo = 1; diff --git a/examples/VG_AHRS/src/user/UserAlgorithm.h b/examples/VG_AHRS/src/user/UserAlgorithm.h new file mode 100644 index 0000000..1c8c3d6 --- /dev/null +++ b/examples/VG_AHRS/src/user/UserAlgorithm.h @@ -0,0 +1,13 @@ +/* + * File: UserAlgorithm.h + * Author: joemotyka + * + * Created on June 28, 2018, 12:23 AM + */ + +#ifndef _USER_ALGORITHM_H_ +#define _USER_ALGORITHM_H_ + + +#endif /* _USER_ALGORITHM_H_ */ + diff --git a/examples/VG_AHRS/src/user/UserConfiguration.c b/examples/VG_AHRS/src/user/UserConfiguration.c index 3c3617b..ae65c12 100644 --- a/examples/VG_AHRS/src/user/UserConfiguration.c +++ b/examples/VG_AHRS/src/user/UserConfiguration.c @@ -24,12 +24,15 @@ limitations under the License. *******************************************************************************/ #include "string.h" -#include "UserConfiguration.h" -#include "UserMessaging.h" -#include "Indices.h" + #include "algorithmAPI.h" +#include "gpsAPI.h" +#include "magAPI.h" #include "platformAPI.h" +#include "UserConfiguration.h" +#include "UserMessaging.h" +#include "Indices.h" // Default user configuration structure // Applied to unit upon reception of "zR" command @@ -39,22 +42,40 @@ const UserConfigurationStruct gDefaultUserConfig = { .dataCRC = 0, .dataSize = sizeof(UserConfigurationStruct), .userUartBaudRate = 115200, - .userPacketType = "e1", - .userPacketRate = 100, + .userPacketType = "a2", + .userPacketRate = 10, .lpfAccelFilterFreq = 25, .lpfRateFilterFreq = 25, - .orientation = "+X+Y+Z" + .orientation = "+X+Y+Z", // add default parameter values here, if desired + .hardIron_X = 0.0, + .hardIron_Y = 0.0, + .softIron_Ratio = 1.0, + .softIron_Angle = 0.0 }; - UserConfigurationStruct gUserConfiguration; UserConfigurationStruct gTmpUserConfiguration; uint8_t UserDataBuffer[4096]; volatile char *info; BOOL configValid = FALSE; -//extern BOOL setUserPacketType(int type, B); + +void setUserMagAlignParams(magAlignUserParams_t *params) +{ + gUserConfiguration.hardIron_X = params->hardIron_X; + gUserConfiguration.hardIron_Y = params->hardIron_Y; + gUserConfiguration.softIron_Ratio = params->softIron_Ratio; + gUserConfiguration.softIron_Angle = params->softIron_Angle; +} + +void getUserMagAlignParams(magAlignUserParams_t *params) +{ + params->hardIron_X = gUserConfiguration.hardIron_X; + params->hardIron_Y = gUserConfiguration.hardIron_Y; + params->softIron_Ratio = gUserConfiguration.softIron_Ratio; + params->softIron_Angle = gUserConfiguration.softIron_Angle; +} void userInitConfigureUnit() @@ -98,7 +119,6 @@ void userInitConfigureUnit() } info = getBuildInfo(); - } @@ -481,6 +501,33 @@ BOOL GetAllUserParams(allUserParamsPayload* pld, uint8_t *payloadLen) } +/** **************************************************************************** + * @name SetMagAlignCmds + * @brief Retrieves specified number of user configuration parameters data for + * sending to the external host starting from specified offset in user + * configuration structure (refer to UserConfigParamOffset structure for + * specific value of offsets) + * @param [in] pointer to userData payload in the packet + * @retval N/A + ******************************************************************************/ +BOOL SetMagAlignCmds(allUserParamsPayload* pld, uint8_t *payloadLen) +{ + uint32_t offset, i, numParams; + uint64_t *ptr = (uint64_t *)&gUserConfiguration; + + numParams = sizeof(UserConfigurationStruct)/8; + + offset = 0; + for (i = 0; i < numParams; i++, offset++){ + pld->parameters[i] = ptr[offset]; + } + + *payloadLen = numParams* 8; + + return TRUE; +} + + /** *************************************************************************** * @name SaveUserConfig - saving of user configuration structure un the * predefined flash sector diff --git a/examples/VG_AHRS/src/user/UserConfiguration.h b/examples/VG_AHRS/src/user/UserConfiguration.h index 34fb433..2aa9d64 100644 --- a/examples/VG_AHRS/src/user/UserConfiguration.h +++ b/examples/VG_AHRS/src/user/UserConfiguration.h @@ -20,15 +20,13 @@ limitations under the License. #ifndef USER_CONFIGURATION_H #define USER_CONFIGURATION_H + #include + #include "GlobalConstants.h" #include "UserMessaging.h" -#include "extern_port_config.h" #include "filter.h" - -#include - /// User defined configuration strucrture ///Please notice, that parameters are 64 bit to accomodate double types as well as longer string types @@ -65,7 +63,6 @@ typedef struct { /// 25 - Butterworth LPF 25HZ /// 40 - Butterworth LPF 40HZ -// int64_t orientation; /// unit orientation in format 0x0000000000ddrrff uint8_t orientation[8]; /// unit orientation in format 0x0000000000ddrrff /// where dd - down axis, rr - right axis, ff - forward axis /// next axis values a valid : @@ -81,9 +78,15 @@ typedef struct { // Add parameter offset in UserConfigParamOffset structure if validation or // special processing required + float hardIron_X; + float hardIron_Y; + float softIron_Ratio; + float softIron_Angle; } UserConfigurationStruct; typedef enum{ +//***************************************************************************************** +// add system parameters here and reassign USER_LAST_SYSTEM_PARAM (DO NOT CHANGE THIS!!!) USER_CRC = 0, USER_DATA_SIZE , // 1 USER_USER_BAUD_RATE , // 2 order of next 4 parameters @@ -92,15 +95,19 @@ typedef enum{ USER_LPF_ACCEL_TYPE , // 5 prefered LPF filter type for accelerometer USER_LPF_RATE_TYPE , // 6 prefered LPF filter type for rate sensor USER_ORIENTATION , // 7 unit orientation -// add system parameters here and reassign USER_LAST_SYSTEM_PARAM USER_LAST_SYSTEM_PARAM = USER_ORIENTATION, +//***************************************************************************************** // add parameter enumerator here while adding new parameter in user UserConfigurationStruct + + USER_HARD_IRON_X , + USER_HARD_IRON_Y , + USER_SOFT_IRON_RATIO , + USER_SOFT_IRON_ANGLE , USER_MAX_PARAM } UserConfigParamNumber; #define MAX_SYSTEM_PARAM USER_ORIENTATION - extern int userPacketOut; #define USER_OK 0x00 diff --git a/examples/VG_AHRS/src/user/UserMessaging.c b/examples/VG_AHRS/src/user/UserMessaging.c index 619174d..32fcd29 100644 --- a/examples/VG_AHRS/src/user/UserMessaging.c +++ b/examples/VG_AHRS/src/user/UserMessaging.c @@ -26,22 +26,55 @@ limitations under the License. #include #include #include -#include "UserMessaging.h" -#include "UserConfiguration.h" + #include "algorithmAPI.h" -#include "userAPI.h" +#include "magAPI.h" #include "platformAPI.h" #include "sensorsAPI.h" +#include "userAPI.h" + +#include "UserMessaging.h" +#include "UserConfiguration.h" -// provided as example +#include "MagAlign.h" + +# include "algorithm.h" + +// Declare the IMU data structure +IMUDataStruct gIMU; + +// Version string char userVersionString[] = "VG_AHRS 1.0.0"; -#include "EKF_Algorithm.h" // for EKFOutputDataStruct +// for EKFOutputDataStruct +#include "EKF_Algorithm.h" EKF_OutputDataStruct *algo_res; +// Example inputs: +// Ping: 55 55 70 47 00 5D 5F +// Mag Align: 55 55 6D 61 00 F0 2D +// 55 55 6D 61 01 00 F1 2E +// 55 55 6D 61 01 01 E1 0F +// 55 55 6D 61 01 02 D1 6C +// 55 55 6D 61 01 03 C1 4D +// 55 55 6D 61 01 04 B1 AA +// 55 55 6D 61 01 05 A1 8B +// 55 55 6D 61 01 06 91 E8 +// 55 55 6D 61 01 07 81 C9 // Get stored values +// 55 55 6D 61 01 08 70 26 +// 55 55 6D 61 01 09 60 07 +// 55 55 6D 61 01 0A 50 64 +// 55 55 6D 61 01 0B +// 55 55 6D 61 01 0C +// 55 55 6D 61 01 0D +// 55 55 6D 61 01 0E 10 E0 +// 55 55 6D 61 01 0F + +// other: 55 55 ... + /// List of allowed packet codes -usr_packet_t userInputPackets[] = { // - {USR_IN_NONE, {0,0}}, // " " +usr_packet_t userInputPackets[] = { + {USR_IN_NONE, {0,0}}, {USR_IN_PING, "pG"}, {USR_IN_UPDATE_CONFIG, "uC"}, {USR_IN_UPDATE_PARAM, "uP"}, @@ -54,11 +87,11 @@ usr_packet_t userInputPackets[] = { // {USR_IN_GET_VERSION, "gV"}, {USR_IN_RESET, "rS"}, // place new input packet code here, before USR_IN_MAX + {USR_IN_MAG_ALIGN, "ma"}, // 0x6D 0x61 {USR_IN_MAX, {0xff, 0xff}}, // "" }; - // packet codes here should be unique - // should not overlap codes for input packets and system packets // First byte of Packet code should have value >= 0x61 @@ -75,8 +108,6 @@ usr_packet_t userOutputPackets[] = { {USR_OUT_MAX, {0xff, 0xff}}, // "" }; - - volatile char *info; static int _userPayloadLen = 0; static int _outputPacketType = USR_OUT_MAX; @@ -117,7 +148,6 @@ int checkUserPacketType(uint16_t receivedCode) void userPacketTypeToBytes(uint8_t bytes[]) { - if(_inputPacketType && _inputPacketType < USR_IN_MAX){ // response to request. Return same packet code bytes[0] = userInputPackets[_inputPacketType].packetCode[0]; @@ -127,7 +157,7 @@ void userPacketTypeToBytes(uint8_t bytes[]) } if(_outputPacketType && _outputPacketType < USR_OUT_MAX){ - // continious packet + // continuous packet bytes[0] = userOutputPackets[_outputPacketType].packetCode[0]; bytes[1] = userOutputPackets[_outputPacketType].packetCode[1]; }else { @@ -223,11 +253,14 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) { BOOL valid = TRUE; int ret = USER_PACKET_OK; + + uint8_t retVal; + int8_t estimatedMagAlignVals[8] = {0}; + int8_t magAlignVals[8] = {0}; + // userPacket *pkt = (userPacket *)ptrUcbPacket->payload; /// call appropriate function based on packet type - - switch (_inputPacketType) { case USR_IN_RESET: Reset(); @@ -282,6 +315,138 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) valid = FALSE; } break; + case USR_IN_MAG_ALIGN: + // Set the valid flag true, if the command is not valid then + // it will be set false upon entry into case 0. + valid = TRUE; + + // + retVal = ProcessMagAlignCmds((magAlignCmdPayload*)ptrUcbPacket->payload, &ptrUcbPacket->payloadLength); + switch(retVal-1) + { + // + uint8_t len; + + case 0: // Return the Mag-Align status + //uint8_t *model = (uint8_t*)unitVersionString(); + //uint8_t *rev = (uint8_t*)platformBuildInfo(); + //unsigned int serialNum = unitSerialNumber(); + //len = snprintf((char*)ptrUcbPacket->payload, 250, "%s %s SN:%u", model, rev, serialNum ); + //ptrUcbPacket->payloadLength = len; + if(gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITH_AUTOEND) { + // Start (auto end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x1 ); + } else if(gMagAlign.state == MAG_ALIGN_STATUS_START_CAL_WITHOUT_AUTOEND) { + // Start (manual end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x2 ); + } else { + // Start (manual end) + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)0x0 ); + } + + ptrUcbPacket->payloadLength = len; + break; + + case 1: // Start mag-align w/ autoend + case 2: // Start mag-align w/ autoend + case 3: // Stop mag-align w/ autoend + case 4: // Accept results + case 5: // Accept results and write to EEPROM + case 6: // Abort Mag-Align or reject results + case 8: // Restore default mag-align values + case 9: // Restore default mag-align values and save in EEPROM + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c", (char)retVal-1 ); + ptrUcbPacket->payloadLength = len; + break; + + case 7: // Return stored mag-align values +#if 0 + // Test values: + gMagAlign.estParams.hardIronBias[X_AXIS] = 0.1; + gMagAlign.estParams.hardIronBias[Y_AXIS] = -0.2; + gMagAlign.estParams.softIronScaleRatio = 0.98; + gMagAlign.estParams.softIronAngle = -270.0 * DEG_TO_RAD; +#endif + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + magAlignVals[0] = (char)( ( (int16_t)( gMagAlign.hardIronBias[X_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + magAlignVals[1] = (char)( ( (int16_t)( gMagAlign.hardIronBias[X_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + magAlignVals[2] = (char)( ( (int16_t)( gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + magAlignVals[3] = (char)( ( (int16_t)( gMagAlign.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + magAlignVals[4] = (char)( ( (int16_t)( gMagAlign.softIronScaleRatio * (float)65535.0 ) >> 8 ) & 0xFF ); + magAlignVals[5] = (char)( ( (int16_t)( gMagAlign.softIronScaleRatio * (float)65535.0 ) >> 0 ) & 0xFF ); + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + magAlignVals[6] = (char)( ( (int16_t)( gMagAlign.softIronAngle * (float)10430.37835047046 ) >> 8 ) & 0xFF ); + magAlignVals[7] = (char)( ( (int16_t)( gMagAlign.softIronAngle * (float)10430.37835047046 ) >> 0 ) & 0xFF ); + + // Bias can be +/- 8.0 [g] (full scale of sensor) + // SF = 2^15 / maxVal = 2^15 / 8.0 = 4096 + estimatedMagAlignVals[0] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[1] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[X_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + estimatedMagAlignVals[2] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[3] = (char)( ( (int16_t)( gMagAlign.estParams.hardIronBias[Y_AXIS] * (float)4096.0 ) >> 0 ) & 0xFF ); + + // Ratio can be 0 --> 1 + // SF = (2^16-1) / maxVal = (2^16-1) / 1.0 = 65535 + estimatedMagAlignVals[4] = (char)( ( (int16_t)( gMagAlign.estParams.softIronScaleRatio * (float)65535.0 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[5] = (char)( ( (int16_t)( gMagAlign.estParams.softIronScaleRatio * (float)65535.0 ) >> 0 ) & 0xFF ); + + // Angle can be +/- pi (in radians) + // Correct for angles that exceed +/-180 + if(gMagAlign.estParams.softIronAngle > PI) { + gMagAlign.estParams.softIronAngle = (float)PI - gMagAlign.estParams.softIronAngle; + } else if(gMagAlign.estParams.softIronAngle < -PI) { + gMagAlign.estParams.softIronAngle = TWO_PI + gMagAlign.estParams.softIronAngle; + } + + // SF = 2^15 / maxVal = 2^15 / pi = 10430.37835047045 + estimatedMagAlignVals[6] = (char)( ( (int16_t)( gMagAlign.estParams.softIronAngle * (float)10430.37835047046 ) >> 8 ) & 0xFF ); + estimatedMagAlignVals[7] = (char)( ( (int16_t)( gMagAlign.estParams.softIronAngle * (float)10430.37835047046 ) >> 0 ) & 0xFF ); + +#if 0 + DebugPrintFloat(" ", (float)estimatedMagAlignVals[0], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[1], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[2], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[3], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[4], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[5], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[6], 1); + DebugPrintFloat(", ", (float)estimatedMagAlignVals[7], 1); + DebugPrintEndline(); +#endif + + len = snprintf((char*)ptrUcbPacket->payload, 250, "%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", (char)magAlignVals[0], + (char)magAlignVals[1], + (char)magAlignVals[2], + (char)magAlignVals[3], + (char)magAlignVals[4], + (char)magAlignVals[5], + (char)magAlignVals[6], + (char)magAlignVals[7], + (char)estimatedMagAlignVals[0], + (char)estimatedMagAlignVals[1], + (char)estimatedMagAlignVals[2], + (char)estimatedMagAlignVals[3], + (char)estimatedMagAlignVals[4], + (char)estimatedMagAlignVals[5], + (char)estimatedMagAlignVals[6], + (char)estimatedMagAlignVals[7] ); + ptrUcbPacket->payloadLength = len; + break; + + case 10: // Load user computed mag-align values + break; + + default: + valid = FALSE; + break; + } + break; default: /// default handler - unknown packet valid = FALSE; @@ -299,11 +464,6 @@ int HandleUserInputPacket(UcbPacketStruct *ptrUcbPacket) } -# include "algorithm.h" - -// Declare the IMU data structure -IMUDataStruct gIMU; - /****************************************************************************** * @name HandleUserOutputPacket - API call ro prepare continuous user output packet * @brief general handler @@ -318,40 +478,44 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) switch (_outputPacketType) { case USR_OUT_TEST: - { uint32_t *testParam = (uint32_t*)(payload); + { + uint32_t *testParam = (uint32_t*)(payload); *payloadLen = USR_OUT_TEST_PAYLOAD_LEN; *testParam = _testVal++; } break; + case USR_OUT_DATA1: - { int n = 0; - double accels[3]; - double mags[3]; - double rates[3]; + { + int n = 0; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; + double rates[NUM_AXIS]; data1_payload_t *pld = (data1_payload_t *)payload; - pld->timer = getDacqTime(); + pld->timer = platformGetDacqTime(); GetAccelData_mPerSecSq(accels); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)accels[i]; } GetRateData_degPerSec(rates); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)rates[i]; } GetMagData_G(mags); - for (int i = 0; i < 3; i++, n++){ + for (int i = X_AXIS; i < NUM_AXIS; i++, n++){ pld->sensorsData[n] = (float)mags[i]; } *payloadLen = sizeof(data1_payload_t); - break; } + break; + case USR_OUT_ANG1: { // Variables used to hold the EKF values - real EulerAngles[3]; - real CorrRates_B[3]; - double accels[3]; + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; // Diagnostic flags uint8_t OperMode, LinAccelSwitch, TurnSwitch; @@ -394,12 +558,6 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *algoData_3++ = (float)accels[Y_AXIS]; *algoData_3++ = (float)accels[Z_AXIS]; - //real CorrAccels_B[3]; - //_GetEKFCorrectedAccels(CorrAccels_B); - - //real Quaternions[4]; - //_GetEKFAttitude_Quaternions(Quaternions); - // Output algorithm diagnostic information reprented by uint8_t variables uint8_t *algoData_4 = (uint8_t*)(algoData_3); @@ -411,45 +569,56 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *algoData_4++ = TurnSwitch; } break; + case USR_OUT_ANG2: { int n = 0; //uint32_t timerCount = 0; - real EulerAngles[3]; - real CorrRates_B[3]; - double accels[3]; + real EulerAngles[NUM_AXIS]; + real CorrRates_B[NUM_AXIS]; + double accels[NUM_AXIS]; - // Set the pointer of the algoData array to the payload - float *algoData = (float*)(payload); - //uint32_t *payload1, float * payload2 + // The payload length (NumOfBytes) is based on the following: + // 1 uint32_t (4 bytes) = 4 bytes + // 1 double (8 bytes) = 8 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // 3 floats (4 bytes) = 12 bytes + // ================================= + // NumOfBytes = 48 bytes *payloadLen = USR_OUT_ANG2_PAYLOAD_LEN; -// float *ptr = (float *)&gAlgorithm.itow; - //timerCount = TIM5->CNT; - //algoData[n++] = *ptr; + // Output time as reprented by gAlgorithm.itow (uint32_t + // incremented at each call of the algorithm) + uint32_t *algoData_1 = (uint32_t*)(payload); + *algoData_1++ = gAlgorithm.itow; - //ptr = + // Output a double representation of time generated from + // gAlgorithm.itow + double *algoData_2 = (double*)(algoData_1); + *algoData_2++ = 1.0e-3 * (double)(gAlgorithm.itow); - n = 0; - algoData[n++] = 0.0; // *ptr; + // Set the pointer of the algoData array to the payload + float *algoData_3 = (float*)(algoData_2); EKF_GetAttitude_EA(EulerAngles); - algoData[n++] = (float)EulerAngles[ROLL]; - algoData[n++] = (float)EulerAngles[PITCH]; - algoData[n++] = (float)EulerAngles[YAW]; + algoData_3[n++] = (float)EulerAngles[ROLL]; + algoData_3[n++] = (float)EulerAngles[PITCH]; + algoData_3[n++] = (float)EulerAngles[YAW]; EKF_GetCorrectedAngRates(CorrRates_B); - algoData[n++] = (float)CorrRates_B[X_AXIS]; - algoData[n++] = (float)CorrRates_B[Y_AXIS]; - algoData[n++] = (float)CorrRates_B[Z_AXIS]; + algoData_3[n++] = (float)CorrRates_B[X_AXIS]; + algoData_3[n++] = (float)CorrRates_B[Y_AXIS]; + algoData_3[n++] = (float)CorrRates_B[Z_AXIS]; GetAccelData_mPerSecSq(accels); - algoData[n++] = (float)accels[X_AXIS]; - algoData[n++] = (float)accels[Y_AXIS]; - algoData[n++] = (float)accels[Z_AXIS]; + algoData_3[n++] = (float)accels[X_AXIS]; + algoData_3[n++] = (float)accels[Y_AXIS]; + algoData_3[n++] = (float)accels[Z_AXIS]; } break; + case USR_OUT_SCALED1: { // The payload length (NumOfBytes) is based on the following: @@ -463,7 +632,7 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) // NumOfBytes = 52 bytes *payloadLen = USR_OUT_SCALED1_PAYLOAD_LEN; - // Output time as reprented by gIMU.timerCntr (uint32_t + // Output time as represented by gIMU.timerCntr (uint32_t // incremented at each call of the algorithm) uint32_t *algoData_1 = (uint32_t*)(payload); *algoData_1++ = gIMU.timerCntr; @@ -490,6 +659,7 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *algoData_3++ = (float)gIMU.temp_C; } break; + // place additional user packet preparing calls here // case USR_OUT_XXXX: // *payloadLen = YYYY; // total user payload length, including user packet type @@ -499,9 +669,9 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) case USR_OUT_EKF1: { // Variables used to hold the EKF values - real EulerAngles[3]; - double accels[3]; - double mags[3]; + real EulerAngles[NUM_AXIS]; + double accels[NUM_AXIS]; + double mags[NUM_AXIS]; // The payload length (NumOfBytes) is based on the following: // 1 uint32_t (4 bytes) = 4 bytes @@ -539,13 +709,13 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) *algoData_3++ = (float)accels[Y_AXIS]; *algoData_3++ = (float)accels[Z_AXIS]; - double rates[3]; + double rates[NUM_AXIS]; GetRateData_degPerSec(rates); *algoData_3++ = (float)rates[X_AXIS]; *algoData_3++ = (float)rates[Y_AXIS]; *algoData_3++ = (float)rates[Z_AXIS]; - float rateBias[3]; + float rateBias[NUM_AXIS]; EKF_GetEstimatedAngRateBias(rateBias); *algoData_3++ = (float)rateBias[X_AXIS]; *algoData_3++ = (float)rateBias[Y_AXIS]; @@ -567,9 +737,11 @@ BOOL HandleUserOutputPacket(uint8_t *payload, uint8_t *payloadLen) } break; default: - *payloadLen = 0; - ret = FALSE; - break; /// unknown user packet, will send error in response + { + *payloadLen = 0; + ret = FALSE; + } + break; /// unknown user packet, will send error in response } return ret; @@ -581,4 +753,3 @@ void WriteResultsIntoOutputStream(void *results) // implement specific data processing/saving here algo_res = results; } - diff --git a/examples/VG_AHRS/src/user/UserMessaging.h b/examples/VG_AHRS/src/user/UserMessaging.h index 6167385..c388dfa 100644 --- a/examples/VG_AHRS/src/user/UserMessaging.h +++ b/examples/VG_AHRS/src/user/UserMessaging.h @@ -1,6 +1,6 @@ /******************************************************************************* * File: UserConfiguration.h - * Created on JAn 25, 2017 + * Created on Jan 25, 2017 ******************************************************************************/ /******************************************************************************* Copyright 2018 ACEINNA, INC @@ -20,18 +20,16 @@ limitations under the License. #ifndef USER_MESSAGING_H #define USER_MESSAGING_H + #include + #include "GlobalConstants.h" -#include "extern_port_config.h" #include "ucb_packet_struct.h" - -#include #define USER_PACKET_OK 0 #define UNKNOWN_USER_PACKET 1 #define USER_PACKET_ERROR 2 - // here is definition for packet rate divider // considering that data acquisition task runs at 200 Hz typedef enum { @@ -70,19 +68,20 @@ typedef enum { USR_IN_GET_VERSION , USR_IN_RESET , // add new packet type here, before USR_IN_MAX - USR_IN_MAX , + USR_IN_MAG_ALIGN , + USR_IN_MAX }UserInPacketType; -// User input packet codes, change at will +// User output packet codes, change at will typedef enum { - USR_OUT_NONE = 0, // 0 - USR_OUT_TEST, // 1 - USR_OUT_DATA1, // 2 - USR_OUT_ANG1, // 3 - USR_OUT_ANG2, // 4 + USR_OUT_NONE = 0, + USR_OUT_TEST, + USR_OUT_DATA1, + USR_OUT_ANG1, + USR_OUT_ANG2, // add new output packet type here, before USR_OUT_MAX - USR_OUT_SCALED1, // 5 - USR_OUT_EKF1, // 6 + USR_OUT_SCALED1, + USR_OUT_EKF1, USR_OUT_MAX } UserOutPacketType; @@ -127,21 +126,16 @@ typedef struct { #define USR_OUT_TEST_PAYLOAD_LEN (4) // test parameter (uint32_t) #define USR_OUT_DATA1_PAYLOAD_LEN (4*9) // 3accels (float LE) + 3gyros (float LE) + 3 mags (floatLE) #define USR_OUT_ANG1_PAYLOAD_LEN (47) // See message loading code,HandleUserOutputPacket(), for information -#define USR_OUT_ANG2_PAYLOAD_LEN (4*10) +#define USR_OUT_ANG2_PAYLOAD_LEN (48) #define USR_OUT_SCALED1_PAYLOAD_LEN (52) #define USR_OUT_EKF1_PAYLOAD_LEN (75) -extern int userPacketOut; - #define USER_OK 0x00 #define USER_NAK 0x80 #define USER_INVALID 0x81 - - extern int userPacketOut; - extern int getUserPayloadLength(void); extern int checkUserPacketType(uint16_t receivedCode); extern void userPacketTypeToBytes(uint8_t bytes[]); @@ -156,6 +150,7 @@ typedef struct { // Algorithm states double accel_g[3]; double rate_radPerSec[3]; + double rate_degPerSec[3]; double mag_G[3]; double temp_C; } IMUDataStruct; @@ -163,4 +158,3 @@ typedef struct { extern IMUDataStruct gIMU; #endif /* USER_CONFIGURATION_H */ - diff --git a/examples/VG_AHRS/src/user/dataProcessingAndPresentation.c b/examples/VG_AHRS/src/user/dataProcessingAndPresentation.c index 9a3e966..df6b715 100644 --- a/examples/VG_AHRS/src/user/dataProcessingAndPresentation.c +++ b/examples/VG_AHRS/src/user/dataProcessingAndPresentation.c @@ -27,9 +27,11 @@ limitations under the License. #include "userAPI.h" #include "sensorsAPI.h" #include "gpsAPI.h" -#include "gps.h" #include "UserMessaging.h" +#include "Indices.h" // For X_AXIS and Z_AXIS +#include "debug.h" // For debug commands + /* * ****************** * @@ -47,7 +49,6 @@ Data * Built-in * Raw Data * * Data * User Filter s* * Al *///<--------------------- Cyclical processing at 100 or 200 Hz in Data Acquisition Task --------------> - // Next function is common for all platforms, but implementation of the methods inside is platform-dependent // Call to this function made from DataAcquisitionTask during initialization phase // All user algorithm and data structures should be initialized here, if used @@ -56,16 +57,14 @@ void initUserDataProcessingEngine() InitUserAlgorithm(); // default implementation located in file user_algorithm.c } -#include "boardAPI.h" // For setIO3Pin -#include "Indices.h" // For X_AXIS and Z_AXIS -#include "debug.h" // For debug commands - -// Next function is common for all platforms, but implementation of the methods inside is platform and user-dependent -// Call to this function made from DataAcquisitionTask after retrieving samples of current sensors data and -// applying corresponding calibration -// dacqRate is rate with which new set of sensors data arrives -void inertialAndPositionDataProcessing(int dacqRate) +// Notes: +// 1) 'inertialAndPositionDataProcessing' is common for all platforms, but implementation +// of the methods inside is platform and user-dependent. +// 2) 'DataAcquisitionTask' calls this function after retrieving samples of current +// sensors data and applying corresponding calibration +// 3) 'dacqRate' is rate with which new set of sensors data arrives +void inertialAndPositionDataProcessing(uint16_t dacqRate) { // void *results; @@ -131,4 +130,3 @@ void inertialAndPositionDataProcessing(int dacqRate) // add current result to output queue for subsequent sending out as continuous packet // returns pointer to user-defined results structure WriteResultsIntoOutputStream(results) ; // default implementation located in file file UserMessaging.c } - diff --git a/platform.json b/platform.json index 552ce8b..e228c87 100644 --- a/platform.json +++ b/platform.json @@ -13,7 +13,7 @@ "type": "git", "url": "https://github.com/aceinna/platform-aceinna_imu.git" }, - "version": "1.0.2", + "version": "1.1.0", "packageRepositories": [ "https://dl.bintray.com/platformio/dl-packages/manifest.json", "http://dl.platformio.org/packages/manifest.json"