From 602165b41a5d0b6d44e01083da8d4dff078176e9 Mon Sep 17 00:00:00 2001 From: Johannes Huebner Date: Mon, 4 Nov 2019 22:39:02 +0100 Subject: [PATCH 01/83] Initial commit --- LICENSE | 165 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ README.md | 2 + 2 files changed, 167 insertions(+) create mode 100644 LICENSE create mode 100644 README.md diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..0a04128 --- /dev/null +++ b/LICENSE @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/README.md b/README.md new file mode 100644 index 0000000..48ba7b9 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# libopeninv +Generic modules that can be used in many projects From 1628a9a473b398dc68adae770c28fdc61969360b Mon Sep 17 00:00:00 2001 From: Johannes Huebner Date: Mon, 4 Nov 2019 22:45:55 +0100 Subject: [PATCH 02/83] Add files via upload --- include/anain.h | 44 +++ include/digio.h | 40 +++ include/errormessage.h | 62 ++++ include/my_fp.h | 46 +++ include/my_math.h | 13 + include/my_string.h | 44 +++ include/param_save.h | 16 + include/params.h | 83 +++++ include/printf.h | 43 +++ include/stm32_can.h | 54 ++++ include/stm32scheduler.h | 63 ++++ include/terminal.h | 38 +++ src/anain.cpp | 156 ++++++++++ src/digio.cpp | 140 +++++++++ src/errormessage.cpp | 124 ++++++++ src/my_fp.c | 141 +++++++++ src/my_string.c | 142 +++++++++ src/param_save.cpp | 119 ++++++++ src/params.cpp | 224 ++++++++++++++ src/printf.c | 281 +++++++++++++++++ src/stm32_can.cpp | 646 +++++++++++++++++++++++++++++++++++++++ src/stm32scheduler.cpp | 99 ++++++ src/terminal.c | 176 +++++++++++ 23 files changed, 2794 insertions(+) create mode 100644 include/anain.h create mode 100644 include/digio.h create mode 100644 include/errormessage.h create mode 100644 include/my_fp.h create mode 100644 include/my_math.h create mode 100644 include/my_string.h create mode 100644 include/param_save.h create mode 100644 include/params.h create mode 100644 include/printf.h create mode 100644 include/stm32_can.h create mode 100644 include/stm32scheduler.h create mode 100644 include/terminal.h create mode 100644 src/anain.cpp create mode 100644 src/digio.cpp create mode 100644 src/errormessage.cpp create mode 100644 src/my_fp.c create mode 100644 src/my_string.c create mode 100644 src/param_save.cpp create mode 100644 src/params.cpp create mode 100644 src/printf.c create mode 100644 src/stm32_can.cpp create mode 100644 src/stm32scheduler.cpp create mode 100644 src/terminal.c diff --git a/include/anain.h b/include/anain.h new file mode 100644 index 0000000..e35ed9d --- /dev/null +++ b/include/anain.h @@ -0,0 +1,44 @@ +#ifndef ANAIO_H_INCLUDED +#define ANAIO_H_INCLUDED + +#include +#include "anain_prj.h" + + +class AnaIn +{ +public: + #define ANA_IN_ENTRY(name, port, pin) name, + enum AnaIns + { + ANA_IN_LIST + ANA_IN_COUNT + }; + #undef ANA_IN_ENTRY + + struct AnaInfo + { + uint32_t port; + uint16_t pin; + }; + + static void Init(AnaInfo ins[]); + static uint16_t Get(AnaIn::AnaIns); + +private: + + + static const AnaInfo ins[]; + static uint16_t values[]; + + static uint8_t AdcChFromPort(uint32_t command_port, int command_bit); + static int median3(int a, int b, int c); +}; + +#define ANA_IN_ENTRY(name, port, pin) { port, pin }, +/** Usage: AnaIn::AnaInfo analogInputs[] = ANA_IN_ARRAY; + * AnaIn::Init(analogInputs); */ +#define ANA_IN_ARRAY { ANA_IN_LIST } + + +#endif // ANAIO_H_INCLUDED diff --git a/include/digio.h b/include/digio.h new file mode 100644 index 0000000..d2e9259 --- /dev/null +++ b/include/digio.h @@ -0,0 +1,40 @@ +#ifndef DIGIO_H_INCLUDED +#define DIGIO_H_INCLUDED + +#include +#include "digio_prj.h" + +namespace PinMode { + enum PinMode + { + INPUT_PD, + INPUT_PU, + INPUT_FLT, + INPUT_AIN, + OUTPUT, + LAST + }; +} + +#define DIG_IO_ENTRY(name, port, pin, mode) name, +namespace Pin { + enum DigPin + { + DIG_IO_LIST + DIG_IO_LAST + }; +} +#undef DIG_IO_ENTRY + +class DigIo +{ +public: + static void Init(void); + static void Configure(Pin::DigPin io, uint32_t port, uint16_t pin, PinMode::PinMode mode); + static bool Get(Pin::DigPin io); + static void Set(Pin::DigPin io); + static void Clear(Pin::DigPin io); + static void Toggle(Pin::DigPin io); +}; + +#endif // DIGIO_H_INCLUDED diff --git a/include/errormessage.h b/include/errormessage.h new file mode 100644 index 0000000..45ced99 --- /dev/null +++ b/include/errormessage.h @@ -0,0 +1,62 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef ERRORMESSAGE_H +#define ERRORMESSAGE_H + +#include "errormessage_prj.h" +#include + +#define ERROR_MESSAGE_ENTRY(id, type) ERR_##id, +typedef enum +{ + ERROR_NONE, + ERROR_MESSAGE_LIST + ERROR_MESSAGE_LAST +} ERROR_MESSAGE_NUM; +#undef ERROR_MESSAGE_ENTRY + +typedef enum +{ + ERROR_STOP, + ERROR_DERATE, + ERROR_DISPLAY, + ERROR_LAST +} ERROR_TYPE; + +class ErrorMessage +{ + public: + static void SetTime(uint32_t time); + static void Post(ERROR_MESSAGE_NUM err); + static void UnpostAll(); + static void PrintAllErrors(); + static void PrintNewErrors(); + static ERROR_MESSAGE_NUM GetLastError(); + protected: + private: + static void PrintError(uint32_t time, ERROR_MESSAGE_NUM err); + + static uint32_t timeTick; + static uint32_t currentBufIdx; + static uint32_t lastPrintIdx; + static bool posted[ERROR_MESSAGE_LAST]; + static ERROR_MESSAGE_NUM lastError; +}; + +#endif // ERRORMESSAGE_H diff --git a/include/my_fp.h b/include/my_fp.h new file mode 100644 index 0000000..27ae27b --- /dev/null +++ b/include/my_fp.h @@ -0,0 +1,46 @@ +#ifndef MY_FP_H_INCLUDED +#define MY_FP_H_INCLUDED + +#include + +#define FRAC_DIGITS 5 + +#ifndef CST_DIGITS +#define CST_DIGITS FRAC_DIGITS +#endif + +#define FRAC_FAC (1 << CST_DIGITS) + +#define CST_CONVERT(a) ((a) << (CST_DIGITS - FRAC_DIGITS)) +#define CST_ICONVERT(a) ((a) >> (CST_DIGITS - FRAC_DIGITS)) + +#define UTOA_FRACDEC 100 +#define FP_DECIMALS 2 + +#define FP_FROMINT(a) ((s32fp)((a) << CST_DIGITS)) +#define FP_TOINT(a) ((s32fp)((a) >> CST_DIGITS)) +#define FP_FROMFLT(a) ((s32fp)((a) * FRAC_FAC)) + +#define FP_MUL(a, b) (((a) * (b)) >> CST_DIGITS) +#define FP_DIV(a, b) (((a) << CST_DIGITS) / (b)) + +typedef uint32_t u32fp; +typedef int32_t s32fp; +typedef int16_t s16fp; +typedef uint16_t u16fp; + +#ifdef __cplusplus +extern "C" +{ +#endif + +char* fp_itoa(char * buf, s32fp a); +s32fp fp_atoi(const char *str); +u32fp fp_sqrt(u32fp rad); +s32fp fp_ln(unsigned int x); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/include/my_math.h b/include/my_math.h new file mode 100644 index 0000000..35b8ff8 --- /dev/null +++ b/include/my_math.h @@ -0,0 +1,13 @@ +#ifndef MY_MATH_H_INCLUDED +#define MY_MATH_H_INCLUDED + +#define ABS(a) ((a) < 0?(-a) : (a)) +#define MIN(a,b) ((a) < (b)?(a):(b)) +#define MAX(a,b) ((a) > (b)?(a):(b)) +#define RAMPUP(current, target, rate) ((target < current || (current + rate) > target) ? target : current + rate) +#define RAMPDOWN(current, target, rate) ((target > current || (current - rate) < target) ? target : current - rate) +#define IIRFILTER(l,n,c) (((n) + ((l) << (c)) - (l)) >> (c)) +#define MEDIAN3(a,b,c) ((a) > (b) ? ((b) > (c) ? (b) : ((a) > (c) ? (c) : (a))) \ + : ((a) > (c) ? (a) : ((b) > (c) ? (c) : (b)))) + +#endif // MY_MATH_H_INCLUDED diff --git a/include/my_string.h b/include/my_string.h new file mode 100644 index 0000000..fe24a55 --- /dev/null +++ b/include/my_string.h @@ -0,0 +1,44 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef NULL +#define NULL 0L +#endif + +#define TOSTR_(...) #__VA_ARGS__ +#define STRINGIFY(x) TOSTR_(x) + +#ifdef __cplusplus +extern "C" +{ +#endif + +int my_strcmp(const char *str1, const char *str2); +void my_strcat(char *str1, const char *str2); +int my_strlen(const char *str); +const char *my_strchr(const char *str, const char c); +int my_ltoa(char *buf, int val, int base); +int my_atoi(const char *str); +char *my_trim(char *str); +void memcpy32(int* target, int *source, int length); +void my_strcpy(char *str1, const char *str2); + +#ifdef __cplusplus +} +#endif diff --git a/include/param_save.h b/include/param_save.h new file mode 100644 index 0000000..733ce71 --- /dev/null +++ b/include/param_save.h @@ -0,0 +1,16 @@ +#ifndef PARAM_SAVE_H_INCLUDED +#define PARAM_SAVE_H_INCLUDED + +#ifdef __cplusplus +extern "C" +{ +#endif + +uint32_t parm_save(void); +int parm_load(void); + +#ifdef __cplusplus +} +#endif + +#endif // PARAM_SAVE_H_INCLUDED diff --git a/include/params.h b/include/params.h new file mode 100644 index 0000000..03f55be --- /dev/null +++ b/include/params.h @@ -0,0 +1,83 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef PARAM_H_INCLUDED +#define PARAM_H_INCLUDED + +#include "param_prj.h" +#include "my_fp.h" + +namespace Param +{ + #define PARAM_ENTRY(category, name, unit, min, max, def, id) name, + #define VALUE_ENTRY(name, unit, id) name, + typedef enum + { + PARAM_LIST + PARAM_LAST, + PARAM_INVALID + } PARAM_NUM; + #undef PARAM_ENTRY + #undef VALUE_ENTRY + + typedef enum + { + TYPE_PARAM, + TYPE_VALUE, + TYPE_LAST + } PARAM_TYPE; + + typedef enum + { + FLAG_NONE = 0, + FLAG_HIDDEN = 1 + } PARAM_FLAG; + + typedef struct + { + char const *category; + char const *name; + char const *unit; + s32fp min; + s32fp max; + s32fp def; + uint32_t id; + } Attributes; + + int Set(PARAM_NUM ParamNum, s32fp ParamVal); + s32fp Get(PARAM_NUM ParamNum); + int GetInt(PARAM_NUM ParamNum); + s32fp GetScl(PARAM_NUM ParamNum); + bool GetBool(PARAM_NUM ParamNum); + void SetInt(PARAM_NUM ParamNum, int ParamVal); + void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal); + PARAM_NUM NumFromString(const char *name); + PARAM_NUM NumFromId(uint32_t id); + const Attributes *GetAttrib(PARAM_NUM ParamNum); + int IsParam(PARAM_NUM ParamNum); + void LoadDefaults(); + void SetFlagsRaw(PARAM_NUM param, uint8_t rawFlags); + void SetFlag(PARAM_NUM param, PARAM_FLAG flag); + void ClearFlag(PARAM_NUM param, PARAM_FLAG flag); + PARAM_FLAG GetFlag(PARAM_NUM param); +} + +//User defined callback +extern void parm_Change(Param::PARAM_NUM ParamNum); + +#endif //PARAM_H_INCLUDED diff --git a/include/printf.h b/include/printf.h new file mode 100644 index 0000000..ff5e5cd --- /dev/null +++ b/include/printf.h @@ -0,0 +1,43 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2010 Johannes Huebner + * Copyright (C) 2010 Edward Cheeseman + * Copyright (C) 2009 Uwe Hermann + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef PRINTF_H_INCLUDED +#define PRINTF_H_INCLUDED + +#if T_DEBUG +#define debugf(...) printf(__VA_ARGS__) +#else +#define debugf(...) +#endif + +#ifdef __cplusplus +extern "C" +{ +#endif + +int printf(const char *format, ...); +int sprintf(char *out, const char *format, ...); + +#ifdef __cplusplus +} +#endif + + +#endif // PRINTF_H_INCLUDED diff --git a/include/stm32_can.h b/include/stm32_can.h new file mode 100644 index 0000000..9129f50 --- /dev/null +++ b/include/stm32_can.h @@ -0,0 +1,54 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2016 Nail Güzel + * Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef STM32_CAN_H_INCLUDED +#define STM32_CAN_H_INCLUDED +#include "params.h" + +#define CAN_ERR_INVALID_ID -1 +#define CAN_ERR_INVALID_OFS -2 +#define CAN_ERR_INVALID_LEN -3 +#define CAN_ERR_MAXMESSAGES -4 +#define CAN_ERR_MAXITEMS -5 + +class Can +{ +public: + enum baudrates + { + Baud250, Baud500, Baud800, Baud1000, BaudLast + }; + + static void Clear(void); + static void Init(enum baudrates baudrate); + static void SetBaudrate(enum baudrates baudrate); + static void Send(uint32_t canId, uint32_t data[2]); + static void SendAll(); + static void Save(); + static void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)); + static bool RegisterUserMessage(int canId); + static uint32_t GetLastRxTimestamp(); + static int AddSend(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); + static int AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); + static int Remove(Param::PARAM_NUM param); + static bool FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx); +}; + + +#endif diff --git a/include/stm32scheduler.h b/include/stm32scheduler.h new file mode 100644 index 0000000..aa255e4 --- /dev/null +++ b/include/stm32scheduler.h @@ -0,0 +1,63 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2017 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef STM32SCHEDULER_H +#define STM32SCHEDULER_H +#include +#include +#include +#include + +#define MAX_TASKS 4 + +/** @brief Schedules up to 4 tasks using a timer peripheral */ +class Stm32Scheduler +{ + public: + /** @brief construct a new scheduler using given timer + * @pre Timer clock and NVIC interrupt must be enabled + * @param timer Address of timer peripheral to use + */ + Stm32Scheduler(uint32_t timer); + + /** @brief Add a periodic task, can be called up to 4 times + * @param function the task function + * @param period The calling period in 100*ms + */ + void AddTask(void (*function)(void), uint16_t period); + + /** @brief Run the scheduler, must be called by the scheduler timer ISR */ + void Run(); + + /** @brief Return CPU load caused by scheduler tasks + * @return load in 0.1% + */ + int GetCpuLoad(); + + protected: + private: + static void nofunc(void); + static const enum tim_oc_id ocMap[MAX_TASKS]; + void (*functions[MAX_TASKS]) (void); + uint16_t periods[MAX_TASKS]; + uint16_t execTicks[MAX_TASKS]; + uint32_t timer; + int nextTask; +}; + +#endif // STM32SCHEDULER_H diff --git a/include/terminal.h b/include/terminal.h new file mode 100644 index 0000000..5fb8176 --- /dev/null +++ b/include/terminal.h @@ -0,0 +1,38 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include + +typedef struct +{ + char const *cmd; + void (*CmdFunc)(char*); +} TERM_CMD; + +#ifdef __cplusplus +extern "C" +{ +#endif + +void term_Init(); +void term_Run(); +void term_Send(char *str); + +#ifdef __cplusplus +} +#endif diff --git a/src/anain.cpp b/src/anain.cpp new file mode 100644 index 0000000..183a249 --- /dev/null +++ b/src/anain.cpp @@ -0,0 +1,156 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2010 Johannes Huebner + * Copyright (C) 2010 Edward Cheeseman + * Copyright (C) 2009 Uwe Hermann + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include +#include +#include "anain.h" +#include "my_math.h" + +#define ADC_DMA_CHAN 1 +#define MEDIAN3_FROM_ADC_ARRAY(a) median3(*a, *(a + ANA_IN_COUNT), *(a + 2*ANA_IN_COUNT)) + +uint16_t AnaIn::values[NUM_SAMPLES*ANA_IN_COUNT]; + +/** +* Initialize ADC hardware and start DMA based conversion process +*/ +void AnaIn::Init(AnaInfo ins[]) +{ + uint8_t channel_array[16]; + + adc_power_off(ADC1); + adc_enable_scan_mode(ADC1); + adc_set_continuous_conversion_mode(ADC1); + adc_set_right_aligned(ADC1); + adc_set_sample_time_on_all_channels(ADC1, SAMPLE_TIME); + + adc_power_on(ADC1); + /* wait for adc starting up*/ + for (volatile int i = 0; i < 80000; i++); + + adc_reset_calibration(ADC1); + adc_calibrate(ADC1); + + for (int numChan = 0; numChan < ANA_IN_COUNT; numChan++) + { + gpio_set_mode(ins[numChan].port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, 1 << ins[numChan].pin); + channel_array[numChan] = AdcChFromPort(ins[numChan].port, ins[numChan].pin); + } + + adc_set_regular_sequence(ADC1, ANA_IN_COUNT, channel_array); + adc_enable_dma(ADC1); + + dma_set_peripheral_address(DMA1, ADC_DMA_CHAN, (uint32_t)&ADC_DR(ADC1)); + dma_set_memory_address(DMA1, ADC_DMA_CHAN, (uint32_t)values); + dma_set_peripheral_size(DMA1, ADC_DMA_CHAN, DMA_CCR_PSIZE_16BIT); + dma_set_memory_size(DMA1, ADC_DMA_CHAN, DMA_CCR_MSIZE_16BIT); + dma_set_number_of_data(DMA1, ADC_DMA_CHAN, NUM_SAMPLES * ANA_IN_COUNT); + dma_enable_memory_increment_mode(DMA1, ADC_DMA_CHAN); + dma_enable_circular_mode(DMA1, ADC_DMA_CHAN); + dma_enable_channel(DMA1, ADC_DMA_CHAN); + + adc_start_conversion_regular(ADC1); + adc_start_conversion_direct(ADC1); +} + +/** +* Get filtered value of given channel +* +* - NUM_SAMPLES = 1: Most recent raw value is returned +* - NUM_SAMPLES = 3: Median of last 3 values is returned +* - NUM_SAMPLES = 9: Median of last 3 medians is returned +* - NUM_SAMPLES = 12: Average of last 4 medians is returned +* +* @param[in] in channel index +* @return Filtered value +*/ +uint16_t AnaIn::Get(AnaIn::AnaIns in) +{ + #if NUM_SAMPLES == 1 + return values[in]; + #elif NUM_SAMPLES == 3 + uint16_t *curVal = &values[in]; + return MEDIAN3_FROM_ADC_ARRAY(curVal); + #elif NUM_SAMPLES == 9 + uint16_t *curVal = &values[in]; + uint16_t med[3]; + + for (int i = 0; i < 3; i++, curVal += 3*ANA_IN_COUNT) + { + med[i] = MEDIAN3_FROM_ADC_ARRAY(curVal); + } + + return median3(med[0], med[1], med[2]); + #elif NUM_SAMPLES == 12 + uint16_t *curVal = &values[in]; + uint16_t med[4]; + + for (int i = 0; i < 4; i++, curVal += 3*ANA_IN_COUNT) + { + med[i] = MEDIAN3_FROM_ADC_ARRAY(curVal); + } + + return (med[0] + med[1] + med[2] + med[3]) >> 2; + #else + #error NUM_SAMPLES must be 1, 3 or 9 + #endif +} + +int AnaIn::median3(int a, int b, int c) +{ + return MEDIAN3(a,b,c); +} + +uint8_t AnaIn::AdcChFromPort(uint32_t command_port, int command_bit) +{ + /* + PA0 ADC12_IN0 + PA1 ADC12_IN1 + PA2 ADC12_IN2 + PA3 ADC12_IN3 + PA4 ADC12_IN4 + PA5 ADC12_IN5 + PA6 ADC12_IN6 + PA7 ADC12_IN7 + PB0 ADC12_IN8 + PB1 ADC12_IN9 + PC0 ADC12_IN10 + PC1 ADC12_IN11 + PC2 ADC12_IN12 + PC3 ADC12_IN13 + PC4 ADC12_IN14 + PC5 ADC12_IN15 + temp ADC12_IN16 + */ + switch (command_port) + { + case GPIOA: /* port A */ + if (command_bit<8) return command_bit; + break; + case GPIOB: /* port B */ + if (command_bit<2) return command_bit+8; + break; + case GPIOC: /* port C */ + if (command_bit<6) return command_bit+10; + break; + } + return 16; +} diff --git a/src/digio.cpp b/src/digio.cpp new file mode 100644 index 0000000..df1ab71 --- /dev/null +++ b/src/digio.cpp @@ -0,0 +1,140 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2010 Johannes Huebner + * Copyright (C) 2010 Edward Cheeseman + * Copyright (C) 2009 Uwe Hermann + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "digio.h" + +#define DIG_IO_OFF 0 +#define DIG_IO_ON 1 + +struct IoInfo +{ + uint32_t port; + uint16_t pin; + PinMode::PinMode mode; +}; + +#define DIG_IO_ENTRY(name, port, pin, mode) { port, pin, mode }, +static struct IoInfo ios[] = +{ + DIG_IO_LIST + { 0, 0, PinMode::LAST } +}; + +/** +* Initialize pins +* @pre if runtime configured pins are present, configure them using Configure first +*/ +void DigIo::Init() +{ + const struct IoInfo *pCur; + + for (pCur = ios; pCur->mode != PinMode::LAST; pCur++) + { + uint8_t mode = GPIO_MODE_INPUT; + uint8_t cnf = GPIO_CNF_INPUT_PULL_UPDOWN; + uint16_t val = DIG_IO_OFF; + + switch (pCur->mode) + { + default: + case PinMode::INPUT_PD: + /* use defaults */ + break; + case PinMode::INPUT_PU: + val = DIG_IO_ON; + break; + case PinMode::INPUT_FLT: + cnf = GPIO_CNF_INPUT_FLOAT; + break; + case PinMode::INPUT_AIN: + cnf = GPIO_CNF_INPUT_ANALOG; + break; + case PinMode::OUTPUT: + mode = GPIO_MODE_OUTPUT_50_MHZ; + cnf = GPIO_CNF_OUTPUT_PUSHPULL; + break; + } + + gpio_set_mode(pCur->port, mode, cnf, pCur->pin); + if (DIG_IO_ON == val) + { + gpio_set(pCur->port, pCur->pin); + } + } +} + +/** Remap GPIO pin at runtime. Must be called before Init() + * @param[in] io io pin to reconfigure + * @param[in] port port to use for this pin + * @param[in] pin port-pin to use for this pin + * @param[in] mode pinmode to use + */ +void DigIo::Configure(Pin::DigPin io, uint32_t port, uint16_t pin, PinMode::PinMode mode) +{ + struct IoInfo *pIo = ios + io; + pIo->port = port; + pIo->pin = pin; + pIo->mode = mode; +} + +/** +* Get pin value +* +* @param[in] io pin index +* @return pin value +*/ +bool DigIo::Get(Pin::DigPin io) +{ + const struct IoInfo *pIo = ios + io; + return gpio_get(pIo->port, pIo->pin) > 0; +} + +/** +* Set pin high +* +* @param[in] io pin index +*/ +void DigIo::Set(Pin::DigPin io) +{ + const struct IoInfo *pIo = ios + io; + return gpio_set(pIo->port, pIo->pin); +} + +/** +* Set pin low +* +* @param[in] io pin index +*/ +void DigIo::Clear(Pin::DigPin io) +{ + const struct IoInfo *pIo = ios + io; + return gpio_clear(pIo->port, pIo->pin); +} + +/** +* Toggle pin +* +* @param[in] io pin index +*/ +void DigIo::Toggle(Pin::DigPin io) +{ + const struct IoInfo *pIo = ios + io; + gpio_toggle(pIo->port, pIo->pin); +} diff --git a/src/errormessage.cpp b/src/errormessage.cpp new file mode 100644 index 0000000..e37b2df --- /dev/null +++ b/src/errormessage.cpp @@ -0,0 +1,124 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "errormessage.h" +#include "printf.h" +#include "my_string.h" + +struct ErrorDescriptor +{ + const char* msg; + ERROR_TYPE type; +}; + +struct BufferEntry +{ + ERROR_MESSAGE_NUM msg; + uint32_t time; +}; + +#define ERROR_MESSAGE_ENTRY(id, type) { #id, type }, +static const struct ErrorDescriptor errorDescriptors[] = +{ + ERROR_MESSAGE_LIST +}; +#undef ERROR_MESSAGE_ENTRY + +#define EXPANDED_LIST ERROR_MESSAGE_ENTRY(NONE, ERROR_DISPLAY) ERROR_MESSAGE_LIST +#define ERROR_MESSAGE_ENTRY(id, type) __COUNTER__=id, +const char* errorListString = STRINGIFY(EXPANDED_LIST); +#undef ERROR_MESSAGE_ENTRY + +static const char* types[ERROR_LAST] = +{ + "STOP", + "DERATE", + "WARN" +}; + +struct BufferEntry errorBuffer[ERROR_BUF_SIZE] = { { ERROR_MESSAGE_LAST, 0 } }; + +uint32_t ErrorMessage::timeTick = 0; +uint32_t ErrorMessage::currentBufIdx = 0; +uint32_t ErrorMessage::lastPrintIdx = 0; +ERROR_MESSAGE_NUM ErrorMessage::lastError = ERROR_NONE; +bool ErrorMessage::posted[ERROR_MESSAGE_LAST] = { false }; + +/** Set timestamp for error message +* @param time Current timestamp, will be displayed as is in message */ +void ErrorMessage::SetTime(uint32_t time) +{ + timeTick = time; +} + +/** Post an error message. + Every message can only be posted once, then UnpostAll() must be called to post it again + @post Message is displayed and written to error memory + @param msg message number */ +void ErrorMessage::Post(ERROR_MESSAGE_NUM msg) +{ + if (!posted[msg] && timeTick > 0) + { + lastError = msg; + errorBuffer[currentBufIdx].msg = msg; + errorBuffer[currentBufIdx].time = timeTick; + posted[msg] = true; + currentBufIdx = (currentBufIdx + 1) % ERROR_BUF_SIZE; + } +} + +/** Unpost all error message, i.e. make them postable again. + Does not reset the error buffer */ +void ErrorMessage::UnpostAll() +{ + for (uint32_t i = 0; i < ERROR_MESSAGE_LAST; i++) + posted[i] = false; +} + +/** Print errors that have been posted since last print */ +void ErrorMessage::PrintNewErrors() +{ + while (lastPrintIdx != currentBufIdx) + { + PrintError(errorBuffer[lastPrintIdx].time, errorBuffer[lastPrintIdx].msg); + lastPrintIdx = (lastPrintIdx + 1) % ERROR_BUF_SIZE; + } +} + +ERROR_MESSAGE_NUM ErrorMessage::GetLastError() +{ + return lastError; +} + +/** Print all errors currently in error memory */ +void ErrorMessage::PrintAllErrors() +{ + if (errorBuffer[0].time == 0) + { + printf("No Errors\r\n"); + return; + } + + for (uint32_t i = 0; i < ERROR_BUF_SIZE && errorBuffer[i].time > 0; i++) + PrintError(errorBuffer[i].time, errorBuffer[i].msg); +} + +void ErrorMessage::PrintError(uint32_t time, ERROR_MESSAGE_NUM msg) +{ + printf("[%u]: %s - %s\r\n", time, types[errorDescriptors[msg].type], errorDescriptors[msg].msg); +} diff --git a/src/my_fp.c b/src/my_fp.c new file mode 100644 index 0000000..ae637b9 --- /dev/null +++ b/src/my_fp.c @@ -0,0 +1,141 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "my_string.h" +#include "my_fp.h" + +#define FRAC_MASK ((1 << FRAC_DIGITS) - 1) + +static s32fp log2_approx(s32fp x, int loopLimit); + +char* fp_itoa(char * buf, s32fp a) +{ + int sign = a < 0?-1:1; + int32_t nat = (sign * a) >> FRAC_DIGITS; + uint32_t frac = ((UTOA_FRACDEC * ((sign * a) & FRAC_MASK))) >> FRAC_DIGITS; + char *p = buf; + if (sign < 0) + { + *p = '-'; + p++; + } + p += my_ltoa(p, nat, 10); + *p = '.'; + p++; + for (uint32_t dec = UTOA_FRACDEC / 10; dec > 1; dec /= 10) + { + if ((frac / dec) == 0) + { + *p = '0'; + p++; + } + } + my_ltoa(p, frac, 10); + return buf; +} + +s32fp fp_atoi(const char *str) +{ + int nat = 0; + int frac = 0; + int div = 10; + int sign = 1; + if ('-' == *str) + { + sign = -1; + str++; + } + for (; *str >= '0' && *str <= '9'; str++) + { + nat *= 10; + nat += *str - '0'; + } + if (*str != 0) + { + for (str++; *str >= '0' && *str <= '9'; str++) + { + frac += FP_FROMINT(*str - '0') / div; + div *= 10; + } + } + + return sign * (FP_FROMINT(nat) + frac); +} + +u32fp fp_sqrt(u32fp rad) +{ + u32fp sqrt = rad >> (rad<1000?4:8); //Starting value for newton iteration + u32fp sqrtl; + sqrt = sqrt>FP_FROMINT(1)?sqrt:FP_FROMINT(1); //Must be > 0 + + do { + sqrtl = sqrt; + sqrt = (sqrt + FP_DIV(rad, sqrt)) >> 1; + } while ((sqrtl - sqrt) > 1); + + return sqrt; +} + +s32fp fp_ln(unsigned int x) +{ + int n = 0; + const s32fp ln2 = FP_FROMFLT(0.6931471806); + + if (x == 0) + { + return -1; + } + else + { //count leading zeros + uint32_t mask = 0xFFFFFFFF; + for (int i = 16; i > 0; i /= 2) + { + mask <<= i; + if ((x & mask) == 0) + { + n += i; + x <<= i; + } + } + } + + s32fp ln = FP_FROMINT(31 - n); + x >>= 32 - FRAC_DIGITS - 1; //will result in fixed point number in [1,2) + ln += log2_approx(x, 5); + ln = FP_MUL(ln2, ln); + return ln; +} + +static s32fp log2_approx(s32fp x, int loopLimit) +{ + int m = 0; + s32fp result = 0; + + if (loopLimit == 0) return FP_FROMINT(1); + if (x == FP_FROMINT(1)) return 0; + + while (x < FP_FROMINT(2)) + { + x = FP_MUL(x, x); + m++; + } + s32fp p = FRAC_FAC >> m; + result = FP_MUL(p, FP_FROMINT(1) + log2_approx(x / 2, loopLimit - 1)); + + return result; +} diff --git a/src/my_string.c b/src/my_string.c new file mode 100644 index 0000000..4acb89b --- /dev/null +++ b/src/my_string.c @@ -0,0 +1,142 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + #include "my_string.h" + +int my_strcmp(const char *str1, const char *str2) +{ + int res = 0; + for (; *str1 > 0 && *str2 > 0; str1++, str2++) + { + if (*str1 != *str2) + { + break; + } + } + if (*str1 != *str2) + { + res = 1; + } + return res; +} + +void my_strcat(char *str1, const char *str2) +{ + for (; *str1 > 0; str1++); + my_strcpy(str1, str2); +} + +void my_strcpy(char *str1, const char *str2) +{ + for (; *str2 > 0; str1++, str2++) + *str1 = *str2; + *str1 = 0; +} + +int my_strlen(const char *str) +{ + int len = 0; + for (; *str > 0; str++, len++); + return len; +} + +const char *my_strchr(const char *str, const char c) +{ + for (; *str > 0 && *str != c; str++); + return str; +} + +int my_ltoa(char *buf, int val, int base) +{ + char *start = buf; + char temp; + int len = 0; + + if (val < 0) + { + *buf = '-'; + buf++; + start++; + len++; + val = -val; + } + else if (0 == val) + { + *buf = '0'; + *(buf + 1) = 0; + return 1; + } + + for (; val > 0; val /= base, buf++, len++) + { + *buf = (val % base) + '0'; + } + *buf = 0; + buf--; + for (; buf > start; buf--, start++) + { + temp = *start; + *start = *buf; + *buf = temp; + } + return len; +} + +int my_atoi(const char *str) +{ + int Res = 0; + int sign = 1; + if ('-' == *str) + { + sign = -1; + str++; + } + for (; *str >= '0' && *str <= '9'; str++) + { + Res *= 10; + Res += *str - '0'; + } + + return sign * Res; +} + +char *my_trim(char *str) +{ + char *end; + + // Trim leading space + while (' ' == *str || '\n' == *str || '\r' == *str) str++; + + if(0 == *str) // All spaces? + return str; + + // Trim trailing space + end = str + my_strlen(str) - 1; + while(end > str && (' ' == *end || '\n' == *end || '\r' == *end)) end--; + + // Write new null terminator + *(end+1) = 0; + + return str; +} + +void memcpy32(int* target, int *source, int length) +{ + while (length--) + *target++ = *source++; +} diff --git a/src/param_save.cpp b/src/param_save.cpp new file mode 100644 index 0000000..b9d902e --- /dev/null +++ b/src/param_save.cpp @@ -0,0 +1,119 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include "params.h" +#include "param_save.h" +#include "hwdefs.h" + +#define NUM_PARAMS ((PARAM_BLKSIZE - 8) / sizeof(PARAM_ENTRY)) +#define PARAM_WORDS (PARAM_BLKSIZE / 4) + +typedef struct +{ + uint16_t key; + uint8_t dummy; + uint8_t flags; + uint32_t value; +} PARAM_ENTRY; + +typedef struct +{ + PARAM_ENTRY data[NUM_PARAMS]; + uint32_t crc; + uint32_t padding; +} PARAM_PAGE; + +/** +* Save parameters to flash +* +* @return CRC of parameter flash page +*/ +uint32_t parm_save() +{ + PARAM_PAGE parmPage; + unsigned int idx; + + crc_reset(); + + //Copy parameter values and keys to block structure + for (idx = 0; Param::IsParam((Param::PARAM_NUM)idx) && idx < NUM_PARAMS; idx++) + { + const Param::Attributes *pAtr = Param::GetAttrib((Param::PARAM_NUM)idx); + parmPage.data[idx].dummy = 0; + parmPage.data[idx].flags = (uint8_t)Param::GetFlag((Param::PARAM_NUM)idx); + parmPage.data[idx].key = pAtr->id; + parmPage.data[idx].value = Param::Get((Param::PARAM_NUM)idx); + crc_calculate(((uint32_t*)&parmPage.data[idx])[0]); //Treat 3 fields as uint32_t + crc_calculate(parmPage.data[idx].value); + } + //Pad the remaining space and the CRC calculcator with 1's + for (; idx < NUM_PARAMS; idx++) + { + parmPage.data[idx].dummy = 0xff; + parmPage.data[idx].flags = 0xff; + parmPage.data[idx].key = 0xffff; + parmPage.data[idx].value = 0xffffffff; + crc_calculate(0xffffffff); + parmPage.crc = crc_calculate(0xffffffff); + } + + parmPage.padding = 0xffffffff; + flash_unlock(); + flash_erase_page(PARAM_ADDRESS); + + for (idx = 0; idx < PARAM_WORDS; idx++) + { + uint32_t* pData = ((uint32_t*)&parmPage) + idx; + flash_program_word(PARAM_ADDRESS + idx * sizeof(uint32_t), *pData); + } + flash_lock(); + return parmPage.crc; +} + +/** +* Load parameters from flash +* +* @retval 0 Parameters loaded successfully +* @retval -1 CRC error, parameters not loaded +*/ +int parm_load() +{ + PARAM_PAGE *parmPage = (PARAM_PAGE *)PARAM_ADDRESS; + + crc_reset(); + uint32_t crc = crc_calculate_block(((uint32_t*)parmPage), (2 * NUM_PARAMS)); + + if (crc == parmPage->crc) + { + for (unsigned int idxPage = 0; idxPage < NUM_PARAMS; idxPage++) + { + Param::PARAM_NUM idx = Param::NumFromId(parmPage->data[idxPage].key); + if (idx != Param::PARAM_INVALID) + { + Param::SetFlt(idx, parmPage->data[idxPage].value); + Param::SetFlagsRaw(idx, parmPage->data[idxPage].flags); + } + } + return 0; + } + + return -1; +} diff --git a/src/params.cpp b/src/params.cpp new file mode 100644 index 0000000..afaf3ba --- /dev/null +++ b/src/params.cpp @@ -0,0 +1,224 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "params.h" +#include "my_string.h" + +namespace Param +{ + +#define PARAM_ENTRY(category, name, unit, min, max, def, id) { category, #name, unit, FP_FROMFLT(min), FP_FROMFLT(max), FP_FROMFLT(def), id }, +#define VALUE_ENTRY(name, unit, id) { 0, #name, unit, 0, 0, 0, id }, +static const Attributes attribs[] = +{ + PARAM_LIST +}; +#undef PARAM_ENTRY +#undef VALUE_ENTRY + +#define PARAM_ENTRY(category, name, unit, min, max, def, id) FP_FROMFLT(def), +#define VALUE_ENTRY(name, unit, id) 0, +static s32fp values[] = +{ + PARAM_LIST +}; +#undef PARAM_ENTRY +#undef VALUE_ENTRY + +#define PARAM_ENTRY(category, name, unit, min, max, def, id) 0, +#define VALUE_ENTRY(name, unit, id) 0, +static uint8_t flags[] = +{ + PARAM_LIST +}; +#undef PARAM_ENTRY +#undef VALUE_ENTRY + +/** +* Set a parameter +* +* @param[in] ParamNum Parameter index +* @param[in] ParamVal New value of parameter +* @return 0 if set ok, -1 if ParamVal outside of allowed range +*/ +int Set(PARAM_NUM ParamNum, s32fp ParamVal) +{ + char res = -1; + + if (ParamVal >= attribs[ParamNum].min && ParamVal <= attribs[ParamNum].max) + { + values[ParamNum] = ParamVal; + parm_Change(ParamNum); + res = 0; + } + return res; +} + +/** +* Get a parameters fixed point value +* +* @param[in] ParamNum Parameter index +* @return Parameters value +*/ +s32fp Get(PARAM_NUM ParamNum) +{ + return values[ParamNum]; +} + +/** +* Get a parameters unscaled digit value +* +* @param[in] ParamNum Parameter index +* @return Parameters value +*/ +int GetInt(PARAM_NUM ParamNum) +{ + return FP_TOINT(values[ParamNum]); +} + +/** +* Get a parameters boolean value, 1.00=True +* +* @param[in] ParamNum Parameter index +* @return Parameters value +*/ +bool GetBool(PARAM_NUM ParamNum) +{ + return FP_TOINT(values[ParamNum]) == 1; +} + +/** +* Set a parameters digit value +* +* @param[in] ParamNum Parameter index +* @param[in] ParamVal New value of parameter +*/ +void SetInt(PARAM_NUM ParamNum, int ParamVal) +{ + values[ParamNum] = FP_FROMINT(ParamVal); +} + +/** +* Set a parameters fixed point value +* +* @param[in] ParamNum Parameter index +* @param[in] ParamVal New value of parameter +*/ +void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal) +{ + values[ParamNum] = ParamVal; +} + +/** +* Get the paramater index from a parameter name +* +* @param[in] name Parameters name +* @return Parameter index if found, PARAM_INVALID otherwise +*/ +PARAM_NUM NumFromString(const char *name) +{ + PARAM_NUM paramNum = PARAM_INVALID; + const Attributes *pCurAtr = attribs; + + for (int i = 0; i < PARAM_LAST; i++, pCurAtr++) + { + if (0 == my_strcmp(pCurAtr->name, name)) + { + paramNum = (PARAM_NUM)i; + break; + } + } + return paramNum; +} + +/** +* Get the paramater index from a parameters unique id +* +* @param[in] id Parameters unique id +* @return Parameter index if found, PARAM_INVALID otherwise +*/ +PARAM_NUM NumFromId(uint32_t id) +{ + PARAM_NUM paramNum = PARAM_INVALID; + const Attributes *pCurAtr = attribs; + + for (int i = 0; i < PARAM_LAST; i++, pCurAtr++) + { + if (pCurAtr->id == id) + { + paramNum = (PARAM_NUM)i; + break; + } + } + return paramNum; +} + +/** +* Get the parameter attributes +* +* @param[in] ParamNum Parameter index +* @return Parameter attributes +*/ +const Attributes *GetAttrib(PARAM_NUM ParamNum) +{ + return &attribs[ParamNum]; +} + +/** Find out if ParamNum is a parameter or display value + * @retval 1 it is a parameter + * @retval 0 otherwise + */ +int IsParam(PARAM_NUM ParamNum) +{ + return attribs[ParamNum].min != attribs[ParamNum].max; +} + +/** Load default values for all parameters */ +void LoadDefaults() +{ + const Attributes *curAtr = attribs; + + for (int idx = 0; idx < PARAM_LAST; idx++, curAtr++) + { + if (curAtr->id > 0) + SetFlt((PARAM_NUM)idx, curAtr->def); + } +} + +void SetFlagsRaw(PARAM_NUM param, uint8_t rawFlags) +{ + flags[param] = rawFlags; +} + +void SetFlag(PARAM_NUM param, PARAM_FLAG flag) +{ + flags[param] |= (uint8_t)flag; +} + +void ClearFlag(PARAM_NUM param, PARAM_FLAG flag) +{ + flags[param] &= (uint8_t)~flag; +} + +PARAM_FLAG GetFlag(PARAM_NUM param) +{ + return (PARAM_FLAG)flags[param]; +} + +} diff --git a/src/printf.c b/src/printf.c new file mode 100644 index 0000000..9316a57 --- /dev/null +++ b/src/printf.c @@ -0,0 +1,281 @@ +/* + Copyright 2001, 2002 Georges Menie (www.menie.org) + stdarg version contributed by Christian Ettinger + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program 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. See the + GNU Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* + putchar is the only external dependency for this file, + if you have a working putchar, leave it commented out. + If not, uncomment the define below and + replace outbyte(c) by your own function call. + +#define putchar(c) outbyte(c) +*/ + +#include +#include "my_fp.h" + +static void printchar(char **str, int c) +{ + extern int putchar(int c); + + if (str) { + **str = c; + ++(*str); + } + else (void)putchar(c); +} + +#define PAD_RIGHT 1 +#define PAD_ZERO 2 + +static int prints(char **out, const char *string, int width, int pad) +{ + register int pc = 0, padchar = ' '; + + if (width > 0) { + register int len = 0; + register const char *ptr; + for (ptr = string; *ptr; ++ptr) ++len; + if (len >= width) width = 0; + else width -= len; + if (pad & PAD_ZERO) padchar = '0'; + } + if (!(pad & PAD_RIGHT)) { + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + } + for ( ; *string ; ++string) { + printchar (out, *string); + ++pc; + } + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + + return pc; +} + +/* the following should be enough for 32 bit int */ +#define PRINT_BUF_LEN 12 + +static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +{ + char print_buf[PRINT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; + + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints (out, print_buf, width, pad); + } + + if (sg && b == 10 && i < 0) { + neg = 1; + u = -i; + } + + s = print_buf + PRINT_BUF_LEN-1; + *s = '\0'; + + while (u) { + t = u % b; + if( t >= 10 ) + t += letbase - '0' - 10; + *--s = t + '0'; + u /= b; + } + + if (neg) { + if( width && (pad & PAD_ZERO) ) { + printchar (out, '-'); + ++pc; + --width; + } + else { + *--s = '-'; + } + } + + return pc + prints (out, s, width, pad); +} + +static int printfp(char **out, int i, int width, int pad) +{ + char print_buf[PRINT_BUF_LEN]; + + fp_itoa(print_buf, i); + + return prints (out, print_buf, width, pad); +} + +static int print(char **out, const char *format, va_list args ) +{ + register int width, pad; + register int pc = 0; + char scr[2]; + + for (; *format != 0; ++format) { + if (*format == '%') { + ++format; + width = pad = 0; + if (*format == '\0') break; + if (*format == '%') goto out; + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + for ( ; *format >= '0' && *format <= '9'; ++format) { + width *= 10; + width += *format - '0'; + } + if( *format == 's' ) { + register char *s = (char *)va_arg( args, int ); + pc += prints (out, s?s:"(null)", width, pad); + continue; + } + if( *format == 'd' ) { + pc += printi (out, va_arg( args, int ), 10, 1, width, pad, 'a'); + continue; + } + if( *format == 'x' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'a'); + continue; + } + if( *format == 'X' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'A'); + continue; + } + if( *format == 'u' ) { + pc += printi (out, va_arg( args, int ), 10, 0, width, pad, 'a'); + continue; + } + if ( *format == 'f' ) { + pc += printfp (out, va_arg( args, int ), width, pad); + continue; + } + if( *format == 'c' ) { + /* char are converted to int then pushed on the stack */ + scr[0] = (char)va_arg( args, int ); + scr[1] = '\0'; + pc += prints (out, scr, width, pad); + continue; + } + } + else { + out: + printchar (out, *format); + ++pc; + } + } + if (out) **out = '\0'; + va_end( args ); + return pc; +} + +int printf(const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( 0, format, args ); +} + +int sprintf(char *out, const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( &out, format, args ); +} + +#ifdef TEST_PRINTF +int main(void) +{ + char *ptr = "Hello world!"; + char *np = 0; + int i = 5; + unsigned int bs = sizeof(int)*8; + int mi; + char buf[80]; + + mi = (1 << (bs-1)) + 1; + printf("%s\n", ptr); + printf("printf test\n"); + printf("%s is null pointer\n", np); + printf("%d = 5\n", i); + printf("%d = - max int\n", mi); + printf("char %c = 'a'\n", 'a'); + printf("hex %x = ff\n", 0xff); + printf("hex %02x = 00\n", 0); + printf("signed %d = unsigned %u = hex %x\n", -3, -3, -3); + printf("%d %s(s)%", 0, "message"); + printf("\n"); + printf("%d %s(s) with %%\n", 0, "message"); + sprintf(buf, "justif: \"%-10s\"\n", "left"); printf("%s", buf); + sprintf(buf, "justif: \"%10s\"\n", "right"); printf("%s", buf); + sprintf(buf, " 3: %04d zero padded\n", 3); printf("%s", buf); + sprintf(buf, " 3: %-4d left justif.\n", 3); printf("%s", buf); + sprintf(buf, " 3: %4d right justif.\n", 3); printf("%s", buf); + sprintf(buf, "-3: %04d zero padded\n", -3); printf("%s", buf); + sprintf(buf, "-3: %-4d left justif.\n", -3); printf("%s", buf); + sprintf(buf, "-3: %4d right justif.\n", -3); printf("%s", buf); + + return 0; +} + +/* + * if you compile this file with + * gcc -Wall $(YOUR_C_OPTIONS) -DTEST_PRINTF -c printf.c + * you will get a normal warning: + * printf.c:214: warning: spurious trailing `%' in format + * this line is testing an invalid % at the end of the format string. + * + * this should display (on 32bit int machine) : + * + * Hello world! + * printf test + * (null) is null pointer + * 5 = 5 + * -2147483647 = - max int + * char a = 'a' + * hex ff = ff + * hex 00 = 00 + * signed -3 = unsigned 4294967293 = hex fffffffd + * 0 message(s) + * 0 message(s) with % + * justif: "left " + * justif: " right" + * 3: 0003 zero padded + * 3: 3 left justif. + * 3: 3 right justif. + * -3: -003 zero padded + * -3: -3 left justif. + * -3: -3 right justif. + */ + +#endif + diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp new file mode 100644 index 0000000..67ec572 --- /dev/null +++ b/src/stm32_can.cpp @@ -0,0 +1,646 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2016 Nail Güzel + * Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include "hwdefs.h" +#include "my_string.h" +#include "my_math.h" +#include +#include +#include +#include +#include +#include +#include "stm32_can.h" + +#define MAX_MESSAGES 10 +#define MAX_ITEMS_PER_MESSAGE 8 +#define MAX_USER_MESSAGES 10 +#define IDS_PER_BANK 4 +#define SENDBUFFER_LEN 20 +#define SDO_WRITE 0x40 +#define SDO_READ 0x22 +#define SDO_ABORT 0x80 +#define SDO_WRITE_REPLY 0x23 +#define SDO_READ_REPLY 0x43 +#define SDO_ERR_INVIDX 0x06020000 +#define SDO_ERR_RANGE 0x06090030 +#define SENDMAP_ADDRESS CANMAP_ADDRESS +#define RECVMAP_ADDRESS (CANMAP_ADDRESS + sizeof(canSendMap)) +#define CRC_ADDRESS (CANMAP_ADDRESS + sizeof(canSendMap) + sizeof(canRecvMap)) +#define SENDMAP_WORDS (sizeof(canSendMap) / sizeof(uint32_t)) +#define RECVMAP_WORDS (sizeof(canRecvMap) / sizeof(uint32_t)) +#define CANID_UNSET 0xffff +#define NUMBITS_LASTMARKER -1 +#define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->canId < CANID_UNSET; c++) +#define forEachPosMap(c,m) for (CANPOS *c = m->items; (c - m->items) < MAX_ITEMS_PER_MESSAGE && c->numBits > 0; c++) + +#if (2 *((MAX_ITEMS_PER_MESSAGE * 6 + 2) * MAX_MESSAGES + 2) + 4) > FLASH_PAGE_SIZE +#error CANMAP will not fit in one flash page +#endif + +typedef struct +{ + uint16_t mapParam; + s16fp gain; + uint8_t offsetBits; + int8_t numBits; +} CANPOS; + +typedef struct +{ + uint16_t canId; + CANPOS items[MAX_ITEMS_PER_MESSAGE]; +} CANIDMAP; + +typedef struct +{ + uint8_t cmd; + uint16_t index; + uint8_t subIndex; + uint32_t data; +} __attribute__((packed)) CAN_SDO; + +typedef struct +{ + uint32_t ts1; + uint32_t ts2; + uint32_t prescaler; +} CANSPEED; + +typedef struct +{ + uint16_t id; + uint32_t data[2]; +} SENDBUFFER; + +static void ProcessSDO(uint32_t data[2]); +static void ClearMap(CANIDMAP *canMap); +static int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param); +static int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); +static uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len); +static int LoadFromFlash(); +static CANIDMAP *FindById(CANIDMAP *canMap, int canId); +static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param); +static void ReplaceParamEnumByUid(CANIDMAP *canMap); +static void ReplaceParamUidByEnum(CANIDMAP *canMap); +static void ConfigureFilters(); + +static CANIDMAP canSendMap[MAX_MESSAGES]; +static CANIDMAP canRecvMap[MAX_MESSAGES]; +static uint32_t lastRxTimestamp = 0; +static SENDBUFFER sendBuffer[SENDBUFFER_LEN]; +static int sendCnt = 0; +static void (*recvCallback)(uint32_t, uint32_t*) = 0; +static uint16_t userIds[MAX_USER_MESSAGES]; +static int nextUserMessageIndex = 0; + +static const CANSPEED canSpeed[Can::BaudLast] = +{ + { CAN_BTR_TS1_9TQ, CAN_BTR_TS2_6TQ, 9 }, //250kbps + { CAN_BTR_TS1_4TQ, CAN_BTR_TS2_3TQ, 9 }, //500kbps + { CAN_BTR_TS1_5TQ, CAN_BTR_TS2_3TQ, 5 }, //800kbps + { CAN_BTR_TS1_6TQ, CAN_BTR_TS2_5TQ, 3 }, //1000kbps +}; + +int Can::AddSend(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) +{ + return Add(canSendMap, param, canId, offset, length, gain); +} + +int Can::AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) +{ + int res = Add(canRecvMap, param, canId, offset, length, gain); + ConfigureFilters(); + return res; +} + +void Can::SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)) +{ + recvCallback = recv; +} + +bool Can::RegisterUserMessage(int canId) +{ + if (nextUserMessageIndex < MAX_USER_MESSAGES) + { + userIds[nextUserMessageIndex] = canId; + nextUserMessageIndex++; + ConfigureFilters(); + return true; + } + return false; +} + +bool Can::FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx) +{ + rx = false; + bool done = false; + + for (CANIDMAP *map = canSendMap; !done; map = canRecvMap) + { + forEachCanMap(curMap, map) + { + forEachPosMap(curPos, curMap) + { + if (curPos->mapParam == param) + { + canId = curMap->canId; + offset = curPos->offsetBits; + length = curPos->numBits; + gain = curPos->gain; + return true; + } + } + } + done = rx; + rx = true; + } + return false; +} + +void Can::Save() +{ + uint32_t crc; + crc_reset(); + + flash_unlock(); + flash_set_ws(2); + flash_erase_page(CANMAP_ADDRESS); + + ReplaceParamEnumByUid(canSendMap); + ReplaceParamEnumByUid(canRecvMap); + + SaveToFlash(SENDMAP_ADDRESS, (uint32_t *)canSendMap, SENDMAP_WORDS); + crc = SaveToFlash(RECVMAP_ADDRESS, (uint32_t *)canRecvMap, RECVMAP_WORDS); + SaveToFlash(CRC_ADDRESS, &crc, 1); + flash_lock(); + + ReplaceParamUidByEnum(canSendMap); + ReplaceParamUidByEnum(canRecvMap); +} + +void Can::SendAll() +{ + forEachCanMap(curMap, canSendMap) + { + uint32_t data[2] = { 0 }; //Had an issue with uint64_t, otherwise would have used that + + forEachPosMap(curPos, curMap) + { + s32fp val = FP_MUL(Param::Get((Param::PARAM_NUM)curPos->mapParam), curPos->gain); + + val &= ((1 << curPos->numBits) - 1); + + if (curPos->offsetBits > 31) + { + data[1] |= val << (curPos->offsetBits - 32); + } + else + { + data[0] |= val << curPos->offsetBits; + } + } + + Send(curMap->canId, data); + } +} + +void Can::Clear(void) +{ + ClearMap(canSendMap); + ClearMap(canRecvMap); +} + +int Can::Remove(Param::PARAM_NUM param) +{ + int removed = RemoveFromMap(canSendMap, param); + removed += RemoveFromMap(canRecvMap, param); + + return removed; +} + +void Can::Init(enum baudrates baudrate) +{ + Clear(); + LoadFromFlash(); + + AFIO_MAPR |= AFIO_MAPR_CAN1_REMAP_PORTA; + + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN1_RX, GPIO_MODE_INPUT, + GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_RX); + gpio_set(GPIO_BANK_CAN1_RX, GPIO_CAN1_RX); + + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN1_TX, GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_TX); + + + // Reset CAN + can_reset(CAN1); + + SetBaudrate(baudrate); + ConfigureFilters(); + // Enable CAN RX interrupt. + can_enable_irq(CAN1, CAN_IER_FMPIE0); +} + +void Can::SetBaudrate(enum baudrates baudrate) +{ + // CAN cell init. + // Setting the bitrate to 250KBit. APB1 = 36MHz, + // prescaler = 9 -> 4MHz time quanta frequency. + // 1tq sync + 9tq bit segment1 (TS1) + 6tq bit segment2 (TS2) = + // 16time quanto per bit period, therefor 4MHz/16 = 250kHz + // + can_init(CAN1, + false, // TTCM: Time triggered comm mode? + true, // ABOM: Automatic bus-off management? + false, // AWUM: Automatic wakeup mode? + false, // NART: No automatic retransmission? + false, // RFLM: Receive FIFO locked mode? + false, // TXFP: Transmit FIFO priority? + CAN_BTR_SJW_1TQ, + canSpeed[baudrate].ts1, + canSpeed[baudrate].ts2, + canSpeed[baudrate].prescaler, // BRP+1: Baud rate prescaler + false, + false); +} + +uint32_t Can::GetLastRxTimestamp() +{ + return lastRxTimestamp; +} + +void Can::Send(uint32_t canId, uint32_t data[2]) +{ + can_disable_irq(CAN1, CAN_IER_TMEIE); + + if (can_transmit(CAN1, canId, false, false, 8, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN) + { + /* enqueue in send buffer if all TX mailboxes are full */ + sendBuffer[sendCnt].id = canId; + sendBuffer[sendCnt].data[0] = data[0]; + sendBuffer[sendCnt].data[1] = data[1]; + sendCnt++; + } + + if (sendCnt > 0) + { + can_enable_irq(CAN1, CAN_IER_TMEIE); + } +} + +extern "C" void usb_lp_can_rx0_isr(void) +{ + uint32_t id; + bool ext, rtr; + uint8_t length, fmi; + uint32_t data[2]; + + for (int fifo = 0; fifo < 2; fifo++) + { + while (can_receive(CAN1, fifo, true, &id, &ext, &rtr, &fmi, &length, (uint8_t*)data, NULL) > 0) + { + //printf("fifo: %d, id: %x, len: %d, data[0]: %x, data[1]: %x\r\n", fifo, id, length, data[0], data[1]); + if (id == 0x601 && length == 8) //SDO request, nodeid=1 + { + ProcessSDO(data); + } + else + { + CANIDMAP *recvMap = FindById(canRecvMap, id); + + if (0 != recvMap) + { + forEachPosMap(curPos, recvMap) + { + s32fp val; + + if (curPos->offsetBits > 31) + { + val = FP_FROMINT((data[1] >> (curPos->offsetBits - 32)) & ((1 << curPos->numBits) - 1)); + } + else + { + val = FP_FROMINT((data[0] >> curPos->offsetBits) & ((1 << curPos->numBits) - 1)); + } + val = FP_MUL(val, curPos->gain); + + if (Param::IsParam((Param::PARAM_NUM)curPos->mapParam)) + Param::Set((Param::PARAM_NUM)curPos->mapParam, val); + else + Param::SetFlt((Param::PARAM_NUM)curPos->mapParam, val); + } + lastRxTimestamp = rtc_get_counter_val(); + } + else //Now it must be a user message, as filters block everything else + { + recvCallback(id, data); + } + } + } + } +} + +extern "C" void usb_hp_can_tx_isr() +{ + while (sendCnt > 0 && can_transmit(CAN1, sendBuffer[sendCnt - 1].id, false, false, 8, (uint8_t*)sendBuffer[sendCnt - 1].data) >= 0) + sendCnt--; + + if (sendCnt == 0) + { + can_disable_irq(CAN1, CAN_IER_TMEIE); + } +} + +//http://www.byteme.org.uk/canopenparent/canopen/sdo-service-data-objects-canopen/ +static void ProcessSDO(uint32_t data[2]) +{ + CAN_SDO *sdo = (CAN_SDO*)data; + if (sdo->index == 0x2000 && sdo->subIndex < Param::PARAM_LAST) + { + if (sdo->cmd == SDO_WRITE) + { + if (Param::Set((Param::PARAM_NUM)sdo->subIndex, sdo->data) == 0) + { + sdo->cmd = SDO_WRITE_REPLY; + } + else + { + sdo->cmd = SDO_ABORT; + sdo->data = SDO_ERR_RANGE; + } + } + else if (sdo->cmd == SDO_READ) + { + sdo->data = Param::Get((Param::PARAM_NUM)sdo->subIndex); + sdo->cmd = SDO_READ_REPLY; + } + } + else if (sdo->index >= 0x3000 && sdo->index < 0x4800 && sdo->subIndex < Param::PARAM_LAST) + { + if (sdo->cmd == SDO_WRITE) + { + int result; + int offset = sdo->data & 0xFF; + int len = (sdo->data >> 8) & 0xFF; + s32fp gain = sdo->data >> 16; + + if ((sdo->index & 0x4000) == 0x4000) + { + result = Can::AddRecv((Param::PARAM_NUM)sdo->subIndex, sdo->index & 0x7FF, offset, len, gain); + } + else + { + result = Can::AddSend((Param::PARAM_NUM)sdo->subIndex, sdo->index & 0x7FF, offset, len, gain); + } + + if (result >= 0) + { + sdo->cmd = SDO_WRITE_REPLY; + } + else + { + sdo->cmd = SDO_ABORT; + sdo->data = SDO_ERR_RANGE; + } + } + } + else + { + sdo->cmd = SDO_ABORT; + sdo->data = SDO_ERR_INVIDX; + } + Can::Send(0x581, data); +} + +static void SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) +{ + can_filter_id_list_16bit_init( + filterId, + idList[0] << 5, //left align + idList[1] << 5, + idList[2] << 5, + idList[3] << 5, + filterId & 1, + true); + idIndex = 0; + filterId++; + idList[0] = idList[1] = idList[2] = idList[3] = 0; +} + +static void ConfigureFilters() +{ + uint16_t idList[IDS_PER_BANK] = { 0x601, 0, 0, 0 }; + int idIndex = 1; + int filterId = 0; + + for (int i = 0; i < nextUserMessageIndex; i++) + { + idList[idIndex] = userIds[i]; + idIndex++; + + if (idIndex == IDS_PER_BANK) + { + SetFilterBank(idIndex, filterId, idList); + } + } + + forEachCanMap(curMap, canRecvMap) + { + idList[idIndex] = curMap->canId; + idIndex++; + + if (idIndex == IDS_PER_BANK) + { + SetFilterBank(idIndex, filterId, idList); + } + } + //loop terminates before adding last set of filters + if (idIndex > 0) + { + SetFilterBank(idIndex, filterId, idList); + } +} + +static int LoadFromFlash() +{ + uint32_t* data = (uint32_t *)CANMAP_ADDRESS; + uint32_t storedCrc = *(uint32_t*)CRC_ADDRESS; + uint32_t crc; + + crc_reset(); + crc = crc_calculate_block(data, SENDMAP_WORDS + RECVMAP_WORDS); + + if (storedCrc == crc) + { + memcpy32((int*)canSendMap, (int*)SENDMAP_ADDRESS, SENDMAP_WORDS); + memcpy32((int*)canRecvMap, (int*)RECVMAP_ADDRESS, RECVMAP_WORDS); + ReplaceParamUidByEnum(canSendMap); + ReplaceParamUidByEnum(canRecvMap); + return 1; + } + return 0; +} + +static int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param) +{ + CANIDMAP copyMap[MAX_MESSAGES]; + + ClearMap(copyMap); + int removed = CopyIdMapExcept(canMap, copyMap, param); + ClearMap(canMap); + CopyIdMapExcept(copyMap, canMap, param); + + return removed; +} + +static int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) +{ + if (canId > 0x7ff) return CAN_ERR_INVALID_ID; + if (offset > 63) return CAN_ERR_INVALID_OFS; + if (length > 32) return CAN_ERR_INVALID_LEN; + + CANIDMAP *existingMap = FindById(canMap, canId); + + if (0 == existingMap) + { + existingMap = FindById(canMap, CANID_UNSET); + if (0 == existingMap) + return CAN_ERR_MAXMESSAGES; + + existingMap->canId = canId; + } + + CANPOS* freeItem = existingMap->items; + + for (; freeItem->numBits > 0; freeItem++); + + if (freeItem->numBits == NUMBITS_LASTMARKER) + return CAN_ERR_MAXITEMS; + + freeItem->mapParam = param; + freeItem->gain = gain; + freeItem->offsetBits = offset; + freeItem->numBits = length; + + int count = 0; + + forEachCanMap(curMap, canMap) + count++; + + return count; +} + +static void ClearMap(CANIDMAP *canMap) +{ + for (int i = 0; i < MAX_MESSAGES; i++) + { + canMap[i].canId = CANID_UNSET; + + for (int j = 0; j < MAX_ITEMS_PER_MESSAGE; j++) + { + canMap[i].items[j].numBits = 0; + } + } +} + +static CANIDMAP *FindById(CANIDMAP *canMap, int canId) +{ + for (int i = 0; i < MAX_MESSAGES; i++) + { + if (canMap[i].canId == canId) + return &canMap[i]; + } + return 0; +} + +static uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len) +{ + uint32_t crc = 0; + + for (int idx = 0; idx < len; idx++) + { + crc = crc_calculate(*data); + flash_program_word(baseAddress + idx * sizeof(uint32_t), *data); + data++; + } + + return crc; +} + +static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param) +{ + int i = 0, removed = 0; + + forEachCanMap(curMap, source) + { + bool discardId = true; + int j = 0; + + forEachPosMap(curPos, curMap) + { + if (curPos->mapParam != param) + { + discardId = false; + dest[i].items[j] = *curPos; + j++; + } + else + { + removed++; + } + } + + if (!discardId) + { + dest[i].canId = curMap->canId; + i++; + } + } + + return removed; +} + +static void ReplaceParamEnumByUid(CANIDMAP *canMap) +{ + forEachCanMap(curMap, canMap) + { + forEachPosMap(curPos, curMap) + { + const Param::Attributes* attr = Param::GetAttrib((Param::PARAM_NUM)curPos->mapParam); + curPos->mapParam = (uint16_t)attr->id; + } + } +} + +static void ReplaceParamUidByEnum(CANIDMAP *canMap) +{ + forEachCanMap(curMap, canMap) + { + forEachPosMap(curPos, curMap) + { + Param::PARAM_NUM param = Param::NumFromId(curPos->mapParam); + curPos->mapParam = param; + } + } +} diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp new file mode 100644 index 0000000..e0c4e3c --- /dev/null +++ b/src/stm32scheduler.cpp @@ -0,0 +1,99 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2017 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "stm32scheduler.h" + +/* return CCRc of TIMt */ +#define TIM_CCR(t,c) (*(volatile uint32_t *)(&TIM_CCR1(t) + (c))) + +const enum tim_oc_id Stm32Scheduler::ocMap[MAX_TASKS] = { TIM_OC1, TIM_OC2, TIM_OC3, TIM_OC4 }; + +Stm32Scheduler::Stm32Scheduler(uint32_t timer) +{ + this->timer = timer; + /* Setup timers upcounting and auto preload enable */ + timer_enable_preload(timer); + timer_direction_up(timer); + /* Set prescaler to count at 100 kHz = 72 MHz/7200 - 1 */ + timer_set_prescaler(timer, 719); + /* Maximum counter value */ + timer_set_period(timer, 0xFFFF); + + for (int i = 0; i < MAX_TASKS; i++) + { + functions[i] = nofunc; + periods[i] = 0xFFFF; + } + + nextTask = 0; +} + +void Stm32Scheduler::AddTask(void (*function)(void), uint16_t period) +{ + if (nextTask >= MAX_TASKS) return; + /* Disable timer */ + timer_disable_counter(timer); + + timer_set_oc_mode(timer, ocMap[nextTask], TIM_OCM_ACTIVE); + timer_set_oc_value(timer, ocMap[nextTask], 0); + + /* Assign task function and period */ + functions[nextTask] = function; + periods [nextTask] = period; + + /* Enable interrupt for that channel */ + timer_enable_irq(timer, TIM_DIER_CC1IE << nextTask); + + /* Reset counter */ + timer_set_counter(timer, 0); + + /* Enable timer */ + timer_enable_counter(timer); + + nextTask++; +} + +void Stm32Scheduler::Run() +{ + for (int i = 0; i < MAX_TASKS; i++) + { + if (timer_get_flag(timer, TIM_SR_CC1IF << i)) + { + uint16_t start = timer_get_counter(timer); + timer_clear_flag(timer, TIM_SR_CC1IF << i); + TIM_CCR(timer, i) += periods[i]; + functions[i](); + execTicks[i] = timer_get_counter(timer) - start; + } + } +} + +int Stm32Scheduler::GetCpuLoad() +{ + int totalLoad = 0; + for (int i = 0; i < MAX_TASKS; i++) + { + int load = (1000 * execTicks[i]) / periods[i]; + totalLoad += load; + } + return totalLoad; +} + +void Stm32Scheduler::nofunc() +{ +} diff --git a/src/terminal.c b/src/terminal.c new file mode 100644 index 0000000..a586c1c --- /dev/null +++ b/src/terminal.c @@ -0,0 +1,176 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "my_string.h" +#include +#include +#include +#include +#include "terminal.h" +#include "hwdefs.h" +#include + +static const TERM_CMD *CmdLookup(char *buf); +static void term_send(uint32_t usart, const char *str); +static void ResetDMA(); + +extern const TERM_CMD TermCmds[]; +static char inBuf[TERM_BUFSIZE]; +static char outBuf[2][TERM_BUFSIZE]; //double buffering + +void term_Init() +{ + ResetDMA(); +} + +/** Run the terminal */ +void term_Run() +{ + char args[TERM_BUFSIZE]; + const TERM_CMD *pCurCmd = NULL; + int lastIdx = 0; + + while (1) + { + int numRcvd = dma_get_number_of_data(DMA1, TERM_USART_DMARX); + int currentIdx = TERM_BUFSIZE - numRcvd; + + if (0 == numRcvd) + ResetDMA(); + + while (lastIdx < currentIdx) //echo + usart_send_blocking(TERM_USART, inBuf[lastIdx++]); + + if (currentIdx > 0) + { + if (inBuf[currentIdx - 1] == '\n' || inBuf[currentIdx - 1] == '\r') + { + inBuf[currentIdx] = 0; + lastIdx = 0; + char *space = (char*)my_strchr(inBuf, ' '); + + if (0 == *space) //No args after command, look for end of line + { + space = (char*)my_strchr(inBuf, '\n'); + args[0] = 0; + } + else //There are arguments, copy everything behind the space + { + my_strcpy(args, space + 1); + } + + if (0 == *space) //No \n found? try \r + space = (char*)my_strchr(inBuf, '\r'); + + *space = 0; + pCurCmd = CmdLookup(inBuf); + ResetDMA(); + + if (NULL != pCurCmd) + { + usart_wait_send_ready(TERM_USART); + pCurCmd->CmdFunc(args); + } + else if (currentIdx > 1) + { + term_send(TERM_USART, "Unknown command sequence\r\n"); + } + } + else if (inBuf[0] == '!' && NULL != pCurCmd) + { + ResetDMA(); + lastIdx = 0; + pCurCmd->CmdFunc(args); + } + } + } /* while(1) */ +} /* term_Run */ + +/* + * Revision 1 hardware can only use synchronous sending as the DMA channel is + * occupied by the encoder timer (TIM3, channel 3). + * All other hardware can use DMA for seamless sending of data. We use double + * buffering, so while one buffer is sent by DMA we can prepare the other + * buffer to go next. +*/ +int putchar(int c) +{ + static uint32_t curIdx = 0, curBuf = 0, first = 1; + + if (hwRev == HW_REV1) + { + usart_send_blocking(TERM_USART, c); + } + else if (c == '\n' || curIdx == (TERM_BUFSIZE - 1)) + { + outBuf[curBuf][curIdx] = c; + + while (!dma_get_interrupt_flag(DMA1, TERM_USART_DMATX, DMA_TCIF) && !first); + + dma_disable_channel(DMA1, TERM_USART_DMATX); + dma_set_number_of_data(DMA1, TERM_USART_DMATX, curIdx + 1); + dma_set_memory_address(DMA1, TERM_USART_DMATX, (uint32_t)outBuf[curBuf]); + dma_clear_interrupt_flags(DMA1, TERM_USART_DMATX, DMA_TCIF); + dma_enable_channel(DMA1, TERM_USART_DMATX); + + curBuf = !curBuf; //switch buffers + first = 0; //only needed once so we don't get stuck in the while loop above + curIdx = 0; + } + else + { + outBuf[curBuf][curIdx] = c; + curIdx++; + } + return 0; +} + +static void ResetDMA() +{ + dma_disable_channel(DMA1, TERM_USART_DMARX); + dma_set_memory_address(DMA1, TERM_USART_DMARX, (uint32_t)inBuf); + dma_set_number_of_data(DMA1, TERM_USART_DMARX, TERM_BUFSIZE); + dma_enable_channel(DMA1, TERM_USART_DMARX); +} + +static const TERM_CMD *CmdLookup(char *buf) +{ + const TERM_CMD *pCmd = TermCmds; + + for (; NULL != pCmd->cmd; pCmd++) + { + if (0 == my_strcmp(buf, pCmd->cmd)) + { + break; + } + } + if (NULL == pCmd->cmd) + { + pCmd = NULL; + } + return pCmd; +} + +static void term_send(uint32_t usart, const char *str) +{ + for (;*str > 0; str++) + usart_send_blocking(usart, *str); +} + + From f575109a82b6c3a841c368e4c9882cafcf5a85e6 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 4 Nov 2019 23:41:27 +0100 Subject: [PATCH 03/83] Added files and merged changes --- include/digio.h | 62 +- include/foc.h | 26 + include/fu.h | 27 + include/my_math.h | 1 + include/picontroller.h | 83 ++ include/sine_core.h | 2108 ++++++++++++++++++++++++++++++++++++++++ src/digio.cpp | 138 +-- src/errormessage.cpp | 3 +- src/foc.cpp | 157 +++ src/fu.cpp | 60 ++ src/picontroller.cpp | 42 + src/sine_core.cpp | 199 ++++ src/stm32scheduler.cpp | 6 +- 13 files changed, 2786 insertions(+), 126 deletions(-) create mode 100644 include/foc.h create mode 100644 include/fu.h create mode 100644 include/picontroller.h create mode 100644 include/sine_core.h create mode 100644 src/foc.cpp create mode 100644 src/fu.cpp create mode 100644 src/picontroller.cpp create mode 100644 src/sine_core.cpp diff --git a/include/digio.h b/include/digio.h index d2e9259..eccdf5b 100644 --- a/include/digio.h +++ b/include/digio.h @@ -16,25 +16,55 @@ namespace PinMode { }; } -#define DIG_IO_ENTRY(name, port, pin, mode) name, -namespace Pin { - enum DigPin - { - DIG_IO_LIST - DIG_IO_LAST - }; -} -#undef DIG_IO_ENTRY - class DigIo { public: - static void Init(void); - static void Configure(Pin::DigPin io, uint32_t port, uint16_t pin, PinMode::PinMode mode); - static bool Get(Pin::DigPin io); - static void Set(Pin::DigPin io); - static void Clear(Pin::DigPin io); - static void Toggle(Pin::DigPin io); + #define DIG_IO_ENTRY(name, port, pin, mode) static DigIo name; + DIG_IO_LIST + #undef DIG_IO_ENTRY + + /** Map GPIO pin object to hardware pin. + * @param[in] port port to use for this pin + * @param[in] pin port-pin to use for this pin + * @param[in] mode pinmode to use + */ + void Configure(uint32_t port, uint16_t pin, PinMode::PinMode pinMode); + + /** + * Get pin value + * + * @param[in] io pin index + * @return pin value + */ + bool Get() { return gpio_get(_port, _pin) > 0; } + + /** + * Set pin high + * + * @param[in] io pin index + */ + void Set() { gpio_set(_port, _pin); } + + /** + * Set pin low + * + * @param[in] io pin index + */ + void Clear() { gpio_clear(_port, _pin); } + + /** + * Toggle pin + * + * @param[in] io pin index + */ + void Toggle() { gpio_toggle(_port, _pin); } + +private: + uint32_t _port; + uint16_t _pin; }; +//Configure all digio objects from the given list +#define DIG_IO_ENTRY(name, port, pin, mode) DigIo::name.Configure(port, pin, mode); +#define DIG_IO_CONFIGURE(l) l #endif // DIGIO_H_INCLUDED diff --git a/include/foc.h b/include/foc.h new file mode 100644 index 0000000..36033c0 --- /dev/null +++ b/include/foc.h @@ -0,0 +1,26 @@ +#ifndef FOC_H +#define FOC_H + +#include +#include "my_fp.h" + +class FOC +{ + public: + static void ParkClarke(s32fp il1, s32fp il2, uint16_t angle); + static int32_t GetQLimit(int32_t maxVd); + static int32_t GetTotalVoltage(int32_t ud, int32_t uq); + static void InvParkClarke(int32_t ud, int32_t uq, uint16_t angle); + static void Mtpa(int32_t is, int32_t& idref, int32_t& iqref); + static int32_t GetMaximumModulationIndex(); + static s32fp id; + static s32fp iq; + static int32_t DutyCycles[3]; + + protected: + private: + static uint32_t sqrt(uint32_t rad); + static u32fp fpsqrt(u32fp rad); +}; + +#endif // FOC_H diff --git a/include/fu.h b/include/fu.h new file mode 100644 index 0000000..08aa31e --- /dev/null +++ b/include/fu.h @@ -0,0 +1,27 @@ +#ifndef FU_H_INCLUDED +#define FU_H_INCLUDED + +#include +#include "my_fp.h" + +class MotorVoltage +{ +public: + static void SetBoost(uint32_t boost); + static void SetWeakeningFrq(u32fp frq); + static void SetMaxAmp(uint32_t maxAmp); + static void SetMinFrq(u32fp frq); + static uint32_t GetAmp(u32fp frq); + static uint32_t GetAmpPerc(u32fp frq, u32fp perc); + +private: + static void CalcFac(); + static uint32_t boost; + static u32fp fac; + static uint32_t maxAmp; + static u32fp endFrq; + static u32fp minFrq; + static u32fp maxFrq; +}; + +#endif // FU_H_INCLUDED diff --git a/include/my_math.h b/include/my_math.h index 35b8ff8..c753b90 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -9,5 +9,6 @@ #define IIRFILTER(l,n,c) (((n) + ((l) << (c)) - (l)) >> (c)) #define MEDIAN3(a,b,c) ((a) > (b) ? ((b) > (c) ? (b) : ((a) > (c) ? (c) : (a))) \ : ((a) > (c) ? (a) : ((b) > (c) ? (c) : (b)))) +#define CHK_BIPOLAR_OFS(ofs) ((ofs < (2048 - 512)) || (ofs > (2048 + 512))) #endif // MY_MATH_H_INCLUDED diff --git a/include/picontroller.h b/include/picontroller.h new file mode 100644 index 0000000..4f17fff --- /dev/null +++ b/include/picontroller.h @@ -0,0 +1,83 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2018 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef PIREGULATOR_H +#define PIREGULATOR_H + +#include "my_fp.h" + +class PiController +{ + public: + /** Default constructor */ + PiController(); + + /** Set regulator proportional and integral gain. + * \param kp New value to set for proportional gain + * \param ki New value for integral gain + */ + void SetGains(int kp, int ki) + { + this->kp = kp; + this->ki = ki; + } + + /** Set regulator target set point + * \param val regulator target + */ + void SetRef(s32fp val) { refVal = val; } + + void SetOffset(int32_t ofs) { offset = ofs; } + + s32fp GetRef() { return refVal; } + + /** Set maximum regulator output + * \param val actuator saturation value + * \post the integrator will stop once this value is surpassed. The output is NOT limited! + */ + void SetMinMaxY(int32_t valMin, int32_t valMax) { minY = valMin; maxY = valMax; } + + /** Set calling frequency + * \param val New value to set + */ + void SetCallingFrequency(int val) { frequency = val; } + + /** Run regulator to obtain a new actuator value + * \param curVal currently measured value + * \return new actuator value + */ + int32_t Run(s32fp curVal); + + /** Reset integrator to 0 */ + void ResetIntegrator() { esum = 0; } + + protected: + + private: + int32_t kp; //!< Member variable "kp" + int32_t ki; //!< Member variable "ki" + s32fp esum; //!< Member variable "esum" + s32fp refVal; + int32_t offset; + int32_t frequency; //!< Calling frequency + int32_t maxY; + int32_t minY; + int32_t y; //!< Member variable "y" +}; + +#endif // PIREGULATOR_H diff --git a/include/sine_core.h b/include/sine_core.h new file mode 100644 index 0000000..a71bd30 --- /dev/null +++ b/include/sine_core.h @@ -0,0 +1,2108 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2012 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef SINE_CORE_H_INCLUDED +#define SINE_CORE_H_INCLUDED + +#include "my_fp.h" + +class SineCore +{ + public: + static void Calc(uint16_t angle); + static s32fp Sine(uint16_t angle); + static s32fp Cosine(uint16_t angle); + static uint16_t Atan2(int32_t cos, int32_t sin); + static void SetAmp(uint32_t amp); + static uint32_t GetAmp(); + static void SetMinPulseWidth(uint32_t minWidth); + static int32_t CalcSVPWMOffset(int32_t a, int32_t b, int32_t c); + static uint32_t DutyCycles[3]; + static uint32_t Offset; + static const int BITS; + static const uint16_t MAXAMP; + + private: + static int32_t SineLookup(uint16_t Arg); + static int32_t MultiplyAmplitude(uint16_t Amplitude, int32_t Baseval); + static int32_t min(int32_t a, int32_t b); + static int32_t max(int32_t a, int32_t b); + + /** Minimum pulse width in normalized digits */ + static uint32_t minPulse; + static uint32_t ampl; + static const int16_t SinTab[]; /* sine LUT */ + static const uint16_t ZERO_OFFSET; +}; + +/* Domain of lookup function */ +#define SINLU_ARGDIGITS 16 +#define SINLU_ONEREV (1U << SINLU_ARGDIGITS) + +#define SINTAB \ +0 ,\ +101 ,\ +201 ,\ +302 ,\ +402 ,\ +503 ,\ +603 ,\ +704 ,\ +804 ,\ +905 ,\ +1005 ,\ +1106 ,\ +1206 ,\ +1307 ,\ +1407 ,\ +1507 ,\ +1608 ,\ +1708 ,\ +1809 ,\ +1909 ,\ +2009 ,\ +2110 ,\ +2210 ,\ +2310 ,\ +2410 ,\ +2511 ,\ +2611 ,\ +2711 ,\ +2811 ,\ +2911 ,\ +3012 ,\ +3112 ,\ +3212 ,\ +3312 ,\ +3412 ,\ +3512 ,\ +3612 ,\ +3712 ,\ +3811 ,\ +3911 ,\ +4011 ,\ +4111 ,\ +4210 ,\ +4310 ,\ +4410 ,\ +4509 ,\ +4609 ,\ +4708 ,\ +4808 ,\ +4907 ,\ +5007 ,\ +5106 ,\ +5205 ,\ +5305 ,\ +5404 ,\ +5503 ,\ +5602 ,\ +5701 ,\ +5800 ,\ +5899 ,\ +5998 ,\ +6096 ,\ +6195 ,\ +6294 ,\ +6393 ,\ +6491 ,\ +6590 ,\ +6688 ,\ +6786 ,\ +6885 ,\ +6983 ,\ +7081 ,\ +7179 ,\ +7277 ,\ +7375 ,\ +7473 ,\ +7571 ,\ +7669 ,\ +7767 ,\ +7864 ,\ +7962 ,\ +8059 ,\ +8157 ,\ +8254 ,\ +8351 ,\ +8448 ,\ +8545 ,\ +8642 ,\ +8739 ,\ +8836 ,\ +8933 ,\ +9030 ,\ +9126 ,\ +9223 ,\ +9319 ,\ +9416 ,\ +9512 ,\ +9608 ,\ +9704 ,\ +9800 ,\ +9896 ,\ +9992 ,\ +10087 ,\ +10183 ,\ +10278 ,\ +10374 ,\ +10469 ,\ +10564 ,\ +10659 ,\ +10754 ,\ +10849 ,\ +10944 ,\ +11039 ,\ +11133 ,\ +11228 ,\ +11322 ,\ +11417 ,\ +11511 ,\ +11605 ,\ +11699 ,\ +11793 ,\ +11886 ,\ +11980 ,\ +12074 ,\ +12167 ,\ +12260 ,\ +12353 ,\ +12446 ,\ +12539 ,\ +12632 ,\ +12725 ,\ +12817 ,\ +12910 ,\ +13002 ,\ +13094 ,\ +13187 ,\ +13279 ,\ +13370 ,\ +13462 ,\ +13554 ,\ +13645 ,\ +13736 ,\ +13828 ,\ +13919 ,\ +14010 ,\ +14101 ,\ +14191 ,\ +14282 ,\ +14372 ,\ +14462 ,\ +14553 ,\ +14643 ,\ +14732 ,\ +14822 ,\ +14912 ,\ +15001 ,\ +15090 ,\ +15180 ,\ +15269 ,\ +15358 ,\ +15446 ,\ +15535 ,\ +15623 ,\ +15712 ,\ +15800 ,\ +15888 ,\ +15976 ,\ +16063 ,\ +16151 ,\ +16238 ,\ +16325 ,\ +16413 ,\ +16499 ,\ +16586 ,\ +16673 ,\ +16759 ,\ +16846 ,\ +16932 ,\ +17018 ,\ +17104 ,\ +17189 ,\ +17275 ,\ +17360 ,\ +17445 ,\ +17530 ,\ +17615 ,\ +17700 ,\ +17784 ,\ +17869 ,\ +17953 ,\ +18037 ,\ +18121 ,\ +18204 ,\ +18288 ,\ +18371 ,\ +18454 ,\ +18537 ,\ +18620 ,\ +18703 ,\ +18785 ,\ +18868 ,\ +18950 ,\ +19032 ,\ +19113 ,\ +19195 ,\ +19276 ,\ +19357 ,\ +19438 ,\ +19519 ,\ +19600 ,\ +19680 ,\ +19761 ,\ +19841 ,\ +19921 ,\ +20000 ,\ +20080 ,\ +20159 ,\ +20238 ,\ +20317 ,\ +20396 ,\ +20475 ,\ +20553 ,\ +20631 ,\ +20709 ,\ +20787 ,\ +20865 ,\ +20942 ,\ +21019 ,\ +21096 ,\ +21173 ,\ +21250 ,\ +21326 ,\ +21403 ,\ +21479 ,\ +21554 ,\ +21630 ,\ +21705 ,\ +21781 ,\ +21856 ,\ +21930 ,\ +22005 ,\ +22079 ,\ +22154 ,\ +22227 ,\ +22301 ,\ +22375 ,\ +22448 ,\ +22521 ,\ +22594 ,\ +22667 ,\ +22739 ,\ +22812 ,\ +22884 ,\ +22956 ,\ +23027 ,\ +23099 ,\ +23170 ,\ +23241 ,\ +23311 ,\ +23382 ,\ +23452 ,\ +23522 ,\ +23592 ,\ +23662 ,\ +23731 ,\ +23801 ,\ +23870 ,\ +23938 ,\ +24007 ,\ +24075 ,\ +24143 ,\ +24211 ,\ +24279 ,\ +24346 ,\ +24413 ,\ +24480 ,\ +24547 ,\ +24613 ,\ +24680 ,\ +24746 ,\ +24811 ,\ +24877 ,\ +24942 ,\ +25007 ,\ +25072 ,\ +25137 ,\ +25201 ,\ +25265 ,\ +25329 ,\ +25393 ,\ +25456 ,\ +25519 ,\ +25582 ,\ +25645 ,\ +25708 ,\ +25770 ,\ +25832 ,\ +25893 ,\ +25955 ,\ +26016 ,\ +26077 ,\ +26138 ,\ +26198 ,\ +26259 ,\ +26319 ,\ +26378 ,\ +26438 ,\ +26497 ,\ +26556 ,\ +26615 ,\ +26674 ,\ +26732 ,\ +26790 ,\ +26848 ,\ +26905 ,\ +26962 ,\ +27019 ,\ +27076 ,\ +27133 ,\ +27189 ,\ +27245 ,\ +27300 ,\ +27356 ,\ +27411 ,\ +27466 ,\ +27521 ,\ +27575 ,\ +27629 ,\ +27683 ,\ +27737 ,\ +27790 ,\ +27843 ,\ +27896 ,\ +27949 ,\ +28001 ,\ +28053 ,\ +28105 ,\ +28157 ,\ +28208 ,\ +28259 ,\ +28310 ,\ +28360 ,\ +28411 ,\ +28460 ,\ +28510 ,\ +28560 ,\ +28609 ,\ +28658 ,\ +28706 ,\ +28755 ,\ +28803 ,\ +28850 ,\ +28898 ,\ +28945 ,\ +28992 ,\ +29039 ,\ +29085 ,\ +29131 ,\ +29177 ,\ +29223 ,\ +29268 ,\ +29313 ,\ +29358 ,\ +29403 ,\ +29447 ,\ +29491 ,\ +29534 ,\ +29578 ,\ +29621 ,\ +29664 ,\ +29706 ,\ +29749 ,\ +29791 ,\ +29832 ,\ +29874 ,\ +29915 ,\ +29956 ,\ +29997 ,\ +30037 ,\ +30077 ,\ +30117 ,\ +30156 ,\ +30195 ,\ +30234 ,\ +30273 ,\ +30311 ,\ +30349 ,\ +30387 ,\ +30424 ,\ +30462 ,\ +30498 ,\ +30535 ,\ +30571 ,\ +30607 ,\ +30643 ,\ +30679 ,\ +30714 ,\ +30749 ,\ +30783 ,\ +30818 ,\ +30852 ,\ +30885 ,\ +30919 ,\ +30952 ,\ +30985 ,\ +31017 ,\ +31050 ,\ +31082 ,\ +31113 ,\ +31145 ,\ +31176 ,\ +31206 ,\ +31237 ,\ +31267 ,\ +31297 ,\ +31327 ,\ +31356 ,\ +31385 ,\ +31414 ,\ +31442 ,\ +31470 ,\ +31498 ,\ +31526 ,\ +31553 ,\ +31580 ,\ +31607 ,\ +31633 ,\ +31659 ,\ +31685 ,\ +31710 ,\ +31736 ,\ +31760 ,\ +31785 ,\ +31809 ,\ +31833 ,\ +31857 ,\ +31880 ,\ +31903 ,\ +31926 ,\ +31949 ,\ +31971 ,\ +31993 ,\ +32014 ,\ +32036 ,\ +32057 ,\ +32077 ,\ +32098 ,\ +32118 ,\ +32137 ,\ +32157 ,\ +32176 ,\ +32195 ,\ +32213 ,\ +32232 ,\ +32250 ,\ +32267 ,\ +32285 ,\ +32302 ,\ +32318 ,\ +32335 ,\ +32351 ,\ +32367 ,\ +32382 ,\ +32397 ,\ +32412 ,\ +32427 ,\ +32441 ,\ +32455 ,\ +32469 ,\ +32482 ,\ +32495 ,\ +32508 ,\ +32521 ,\ +32533 ,\ +32545 ,\ +32556 ,\ +32567 ,\ +32578 ,\ +32589 ,\ +32599 ,\ +32609 ,\ +32619 ,\ +32628 ,\ +32637 ,\ +32646 ,\ +32655 ,\ +32663 ,\ +32671 ,\ +32678 ,\ +32685 ,\ +32692 ,\ +32699 ,\ +32705 ,\ +32711 ,\ +32717 ,\ +32722 ,\ +32728 ,\ +32732 ,\ +32737 ,\ +32741 ,\ +32745 ,\ +32748 ,\ +32752 ,\ +32755 ,\ +32757 ,\ +32759 ,\ +32761 ,\ +32763 ,\ +32765 ,\ +32766 ,\ +32766 ,\ +32767 ,\ +32767 ,\ +32767 ,\ +32766 ,\ +32766 ,\ +32765 ,\ +32763 ,\ +32761 ,\ +32759 ,\ +32757 ,\ +32755 ,\ +32752 ,\ +32748 ,\ +32745 ,\ +32741 ,\ +32737 ,\ +32732 ,\ +32728 ,\ +32722 ,\ +32717 ,\ +32711 ,\ +32705 ,\ +32699 ,\ +32692 ,\ +32685 ,\ +32678 ,\ +32671 ,\ +32663 ,\ +32655 ,\ +32646 ,\ +32637 ,\ +32628 ,\ +32619 ,\ +32609 ,\ +32599 ,\ +32589 ,\ +32578 ,\ +32567 ,\ +32556 ,\ +32545 ,\ +32533 ,\ +32521 ,\ +32508 ,\ +32495 ,\ +32482 ,\ +32469 ,\ +32455 ,\ +32441 ,\ +32427 ,\ +32412 ,\ +32397 ,\ +32382 ,\ +32367 ,\ +32351 ,\ +32335 ,\ +32318 ,\ +32302 ,\ +32285 ,\ +32267 ,\ +32250 ,\ +32232 ,\ +32213 ,\ +32195 ,\ +32176 ,\ +32157 ,\ +32137 ,\ +32118 ,\ +32098 ,\ +32077 ,\ +32057 ,\ +32036 ,\ +32014 ,\ +31993 ,\ +31971 ,\ +31949 ,\ +31926 ,\ +31903 ,\ +31880 ,\ +31857 ,\ +31833 ,\ +31809 ,\ +31785 ,\ +31760 ,\ +31736 ,\ +31710 ,\ +31685 ,\ +31659 ,\ +31633 ,\ +31607 ,\ +31580 ,\ +31553 ,\ +31526 ,\ +31498 ,\ +31470 ,\ +31442 ,\ +31414 ,\ +31385 ,\ +31356 ,\ +31327 ,\ +31297 ,\ +31267 ,\ +31237 ,\ +31206 ,\ +31176 ,\ +31145 ,\ +31113 ,\ +31082 ,\ +31050 ,\ +31017 ,\ +30985 ,\ +30952 ,\ +30919 ,\ +30885 ,\ +30852 ,\ +30818 ,\ +30783 ,\ +30749 ,\ +30714 ,\ +30679 ,\ +30643 ,\ +30607 ,\ +30571 ,\ +30535 ,\ +30498 ,\ +30462 ,\ +30424 ,\ +30387 ,\ +30349 ,\ +30311 ,\ +30273 ,\ +30234 ,\ +30195 ,\ +30156 ,\ +30117 ,\ +30077 ,\ +30037 ,\ +29997 ,\ +29956 ,\ +29915 ,\ +29874 ,\ +29832 ,\ +29791 ,\ +29749 ,\ +29706 ,\ +29664 ,\ +29621 ,\ +29578 ,\ +29534 ,\ +29491 ,\ +29447 ,\ +29403 ,\ +29358 ,\ +29313 ,\ +29268 ,\ +29223 ,\ +29177 ,\ +29131 ,\ +29085 ,\ +29039 ,\ +28992 ,\ +28945 ,\ +28898 ,\ +28850 ,\ +28803 ,\ +28755 ,\ +28706 ,\ +28658 ,\ +28609 ,\ +28560 ,\ +28510 ,\ +28460 ,\ +28411 ,\ +28360 ,\ +28310 ,\ +28259 ,\ +28208 ,\ +28157 ,\ +28105 ,\ +28053 ,\ +28001 ,\ +27949 ,\ +27896 ,\ +27843 ,\ +27790 ,\ +27737 ,\ +27683 ,\ +27629 ,\ +27575 ,\ +27521 ,\ +27466 ,\ +27411 ,\ +27356 ,\ +27300 ,\ +27245 ,\ +27189 ,\ +27133 ,\ +27076 ,\ +27019 ,\ +26962 ,\ +26905 ,\ +26848 ,\ +26790 ,\ +26732 ,\ +26674 ,\ +26615 ,\ +26556 ,\ +26497 ,\ +26438 ,\ +26378 ,\ +26319 ,\ +26259 ,\ +26198 ,\ +26138 ,\ +26077 ,\ +26016 ,\ +25955 ,\ +25893 ,\ +25832 ,\ +25770 ,\ +25708 ,\ +25645 ,\ +25582 ,\ +25519 ,\ +25456 ,\ +25393 ,\ +25329 ,\ +25265 ,\ +25201 ,\ +25137 ,\ +25072 ,\ +25007 ,\ +24942 ,\ +24877 ,\ +24811 ,\ +24746 ,\ +24680 ,\ +24613 ,\ +24547 ,\ +24480 ,\ +24413 ,\ +24346 ,\ +24279 ,\ +24211 ,\ +24143 ,\ +24075 ,\ +24007 ,\ +23938 ,\ +23870 ,\ +23801 ,\ +23731 ,\ +23662 ,\ +23592 ,\ +23522 ,\ +23452 ,\ +23382 ,\ +23311 ,\ +23241 ,\ +23170 ,\ +23099 ,\ +23027 ,\ +22956 ,\ +22884 ,\ +22812 ,\ +22739 ,\ +22667 ,\ +22594 ,\ +22521 ,\ +22448 ,\ +22375 ,\ +22301 ,\ +22227 ,\ +22154 ,\ +22079 ,\ +22005 ,\ +21930 ,\ +21856 ,\ +21781 ,\ +21705 ,\ +21630 ,\ +21554 ,\ +21479 ,\ +21403 ,\ +21326 ,\ +21250 ,\ +21173 ,\ +21096 ,\ +21019 ,\ +20942 ,\ +20865 ,\ +20787 ,\ +20709 ,\ +20631 ,\ +20553 ,\ +20475 ,\ +20396 ,\ +20317 ,\ +20238 ,\ +20159 ,\ +20080 ,\ +20000 ,\ +19921 ,\ +19841 ,\ +19761 ,\ +19680 ,\ +19600 ,\ +19519 ,\ +19438 ,\ +19357 ,\ +19276 ,\ +19195 ,\ +19113 ,\ +19032 ,\ +18950 ,\ +18868 ,\ +18785 ,\ +18703 ,\ +18620 ,\ +18537 ,\ +18454 ,\ +18371 ,\ +18288 ,\ +18204 ,\ +18121 ,\ +18037 ,\ +17953 ,\ +17869 ,\ +17784 ,\ +17700 ,\ +17615 ,\ +17530 ,\ +17445 ,\ +17360 ,\ +17275 ,\ +17189 ,\ +17104 ,\ +17018 ,\ +16932 ,\ +16846 ,\ +16759 ,\ +16673 ,\ +16586 ,\ +16499 ,\ +16413 ,\ +16325 ,\ +16238 ,\ +16151 ,\ +16063 ,\ +15976 ,\ +15888 ,\ +15800 ,\ +15712 ,\ +15623 ,\ +15535 ,\ +15446 ,\ +15358 ,\ +15269 ,\ +15180 ,\ +15090 ,\ +15001 ,\ +14912 ,\ +14822 ,\ +14732 ,\ +14643 ,\ +14553 ,\ +14462 ,\ +14372 ,\ +14282 ,\ +14191 ,\ +14101 ,\ +14010 ,\ +13919 ,\ +13828 ,\ +13736 ,\ +13645 ,\ +13554 ,\ +13462 ,\ +13370 ,\ +13279 ,\ +13187 ,\ +13094 ,\ +13002 ,\ +12910 ,\ +12817 ,\ +12725 ,\ +12632 ,\ +12539 ,\ +12446 ,\ +12353 ,\ +12260 ,\ +12167 ,\ +12074 ,\ +11980 ,\ +11886 ,\ +11793 ,\ +11699 ,\ +11605 ,\ +11511 ,\ +11417 ,\ +11322 ,\ +11228 ,\ +11133 ,\ +11039 ,\ +10944 ,\ +10849 ,\ +10754 ,\ +10659 ,\ +10564 ,\ +10469 ,\ +10374 ,\ +10278 ,\ +10183 ,\ +10087 ,\ +9992 ,\ +9896 ,\ +9800 ,\ +9704 ,\ +9608 ,\ +9512 ,\ +9416 ,\ +9319 ,\ +9223 ,\ +9126 ,\ +9030 ,\ +8933 ,\ +8836 ,\ +8739 ,\ +8642 ,\ +8545 ,\ +8448 ,\ +8351 ,\ +8254 ,\ +8157 ,\ +8059 ,\ +7962 ,\ +7864 ,\ +7767 ,\ +7669 ,\ +7571 ,\ +7473 ,\ +7375 ,\ +7277 ,\ +7179 ,\ +7081 ,\ +6983 ,\ +6885 ,\ +6786 ,\ +6688 ,\ +6590 ,\ +6491 ,\ +6393 ,\ +6294 ,\ +6195 ,\ +6096 ,\ +5998 ,\ +5899 ,\ +5800 ,\ +5701 ,\ +5602 ,\ +5503 ,\ +5404 ,\ +5305 ,\ +5205 ,\ +5106 ,\ +5007 ,\ +4907 ,\ +4808 ,\ +4708 ,\ +4609 ,\ +4509 ,\ +4410 ,\ +4310 ,\ +4210 ,\ +4111 ,\ +4011 ,\ +3911 ,\ +3811 ,\ +3712 ,\ +3612 ,\ +3512 ,\ +3412 ,\ +3312 ,\ +3212 ,\ +3112 ,\ +3012 ,\ +2911 ,\ +2811 ,\ +2711 ,\ +2611 ,\ +2511 ,\ +2410 ,\ +2310 ,\ +2210 ,\ +2110 ,\ +2009 ,\ +1909 ,\ +1809 ,\ +1708 ,\ +1608 ,\ +1507 ,\ +1407 ,\ +1307 ,\ +1206 ,\ +1106 ,\ +1005 ,\ +905 ,\ +804 ,\ +704 ,\ +603 ,\ +503 ,\ +402 ,\ +302 ,\ +201 ,\ +101 ,\ +0 ,\ +-101 ,\ +-201 ,\ +-302 ,\ +-402 ,\ +-503 ,\ +-603 ,\ +-704 ,\ +-804 ,\ +-905 ,\ +-1005 ,\ +-1106 ,\ +-1206 ,\ +-1307 ,\ +-1407 ,\ +-1507 ,\ +-1608 ,\ +-1708 ,\ +-1809 ,\ +-1909 ,\ +-2009 ,\ +-2110 ,\ +-2210 ,\ +-2310 ,\ +-2410 ,\ +-2511 ,\ +-2611 ,\ +-2711 ,\ +-2811 ,\ +-2911 ,\ +-3012 ,\ +-3112 ,\ +-3212 ,\ +-3312 ,\ +-3412 ,\ +-3512 ,\ +-3612 ,\ +-3712 ,\ +-3811 ,\ +-3911 ,\ +-4011 ,\ +-4111 ,\ +-4210 ,\ +-4310 ,\ +-4410 ,\ +-4509 ,\ +-4609 ,\ +-4708 ,\ +-4808 ,\ +-4907 ,\ +-5007 ,\ +-5106 ,\ +-5205 ,\ +-5305 ,\ +-5404 ,\ +-5503 ,\ +-5602 ,\ +-5701 ,\ +-5800 ,\ +-5899 ,\ +-5998 ,\ +-6096 ,\ +-6195 ,\ +-6294 ,\ +-6393 ,\ +-6491 ,\ +-6590 ,\ +-6688 ,\ +-6786 ,\ +-6885 ,\ +-6983 ,\ +-7081 ,\ +-7179 ,\ +-7277 ,\ +-7375 ,\ +-7473 ,\ +-7571 ,\ +-7669 ,\ +-7767 ,\ +-7864 ,\ +-7962 ,\ +-8059 ,\ +-8157 ,\ +-8254 ,\ +-8351 ,\ +-8448 ,\ +-8545 ,\ +-8642 ,\ +-8739 ,\ +-8836 ,\ +-8933 ,\ +-9030 ,\ +-9126 ,\ +-9223 ,\ +-9319 ,\ +-9416 ,\ +-9512 ,\ +-9608 ,\ +-9704 ,\ +-9800 ,\ +-9896 ,\ +-9992 ,\ +-10087 ,\ +-10183 ,\ +-10278 ,\ +-10374 ,\ +-10469 ,\ +-10564 ,\ +-10659 ,\ +-10754 ,\ +-10849 ,\ +-10944 ,\ +-11039 ,\ +-11133 ,\ +-11228 ,\ +-11322 ,\ +-11417 ,\ +-11511 ,\ +-11605 ,\ +-11699 ,\ +-11793 ,\ +-11886 ,\ +-11980 ,\ +-12074 ,\ +-12167 ,\ +-12260 ,\ +-12353 ,\ +-12446 ,\ +-12539 ,\ +-12632 ,\ +-12725 ,\ +-12817 ,\ +-12910 ,\ +-13002 ,\ +-13094 ,\ +-13187 ,\ +-13279 ,\ +-13370 ,\ +-13462 ,\ +-13554 ,\ +-13645 ,\ +-13736 ,\ +-13828 ,\ +-13919 ,\ +-14010 ,\ +-14101 ,\ +-14191 ,\ +-14282 ,\ +-14372 ,\ +-14462 ,\ +-14553 ,\ +-14643 ,\ +-14732 ,\ +-14822 ,\ +-14912 ,\ +-15001 ,\ +-15090 ,\ +-15180 ,\ +-15269 ,\ +-15358 ,\ +-15446 ,\ +-15535 ,\ +-15623 ,\ +-15712 ,\ +-15800 ,\ +-15888 ,\ +-15976 ,\ +-16063 ,\ +-16151 ,\ +-16238 ,\ +-16325 ,\ +-16413 ,\ +-16499 ,\ +-16586 ,\ +-16673 ,\ +-16759 ,\ +-16846 ,\ +-16932 ,\ +-17018 ,\ +-17104 ,\ +-17189 ,\ +-17275 ,\ +-17360 ,\ +-17445 ,\ +-17530 ,\ +-17615 ,\ +-17700 ,\ +-17784 ,\ +-17869 ,\ +-17953 ,\ +-18037 ,\ +-18121 ,\ +-18204 ,\ +-18288 ,\ +-18371 ,\ +-18454 ,\ +-18537 ,\ +-18620 ,\ +-18703 ,\ +-18785 ,\ +-18868 ,\ +-18950 ,\ +-19032 ,\ +-19113 ,\ +-19195 ,\ +-19276 ,\ +-19357 ,\ +-19438 ,\ +-19519 ,\ +-19600 ,\ +-19680 ,\ +-19761 ,\ +-19841 ,\ +-19921 ,\ +-20000 ,\ +-20080 ,\ +-20159 ,\ +-20238 ,\ +-20317 ,\ +-20396 ,\ +-20475 ,\ +-20553 ,\ +-20631 ,\ +-20709 ,\ +-20787 ,\ +-20865 ,\ +-20942 ,\ +-21019 ,\ +-21096 ,\ +-21173 ,\ +-21250 ,\ +-21326 ,\ +-21403 ,\ +-21479 ,\ +-21554 ,\ +-21630 ,\ +-21705 ,\ +-21781 ,\ +-21856 ,\ +-21930 ,\ +-22005 ,\ +-22079 ,\ +-22154 ,\ +-22227 ,\ +-22301 ,\ +-22375 ,\ +-22448 ,\ +-22521 ,\ +-22594 ,\ +-22667 ,\ +-22739 ,\ +-22812 ,\ +-22884 ,\ +-22956 ,\ +-23027 ,\ +-23099 ,\ +-23170 ,\ +-23241 ,\ +-23311 ,\ +-23382 ,\ +-23452 ,\ +-23522 ,\ +-23592 ,\ +-23662 ,\ +-23731 ,\ +-23801 ,\ +-23870 ,\ +-23938 ,\ +-24007 ,\ +-24075 ,\ +-24143 ,\ +-24211 ,\ +-24279 ,\ +-24346 ,\ +-24413 ,\ +-24480 ,\ +-24547 ,\ +-24613 ,\ +-24680 ,\ +-24746 ,\ +-24811 ,\ +-24877 ,\ +-24942 ,\ +-25007 ,\ +-25072 ,\ +-25137 ,\ +-25201 ,\ +-25265 ,\ +-25329 ,\ +-25393 ,\ +-25456 ,\ +-25519 ,\ +-25582 ,\ +-25645 ,\ +-25708 ,\ +-25770 ,\ +-25832 ,\ +-25893 ,\ +-25955 ,\ +-26016 ,\ +-26077 ,\ +-26138 ,\ +-26198 ,\ +-26259 ,\ +-26319 ,\ +-26378 ,\ +-26438 ,\ +-26497 ,\ +-26556 ,\ +-26615 ,\ +-26674 ,\ +-26732 ,\ +-26790 ,\ +-26848 ,\ +-26905 ,\ +-26962 ,\ +-27019 ,\ +-27076 ,\ +-27133 ,\ +-27189 ,\ +-27245 ,\ +-27300 ,\ +-27356 ,\ +-27411 ,\ +-27466 ,\ +-27521 ,\ +-27575 ,\ +-27629 ,\ +-27683 ,\ +-27737 ,\ +-27790 ,\ +-27843 ,\ +-27896 ,\ +-27949 ,\ +-28001 ,\ +-28053 ,\ +-28105 ,\ +-28157 ,\ +-28208 ,\ +-28259 ,\ +-28310 ,\ +-28360 ,\ +-28411 ,\ +-28460 ,\ +-28510 ,\ +-28560 ,\ +-28609 ,\ +-28658 ,\ +-28706 ,\ +-28755 ,\ +-28803 ,\ +-28850 ,\ +-28898 ,\ +-28945 ,\ +-28992 ,\ +-29039 ,\ +-29085 ,\ +-29131 ,\ +-29177 ,\ +-29223 ,\ +-29268 ,\ +-29313 ,\ +-29358 ,\ +-29403 ,\ +-29447 ,\ +-29491 ,\ +-29534 ,\ +-29578 ,\ +-29621 ,\ +-29664 ,\ +-29706 ,\ +-29749 ,\ +-29791 ,\ +-29832 ,\ +-29874 ,\ +-29915 ,\ +-29956 ,\ +-29997 ,\ +-30037 ,\ +-30077 ,\ +-30117 ,\ +-30156 ,\ +-30195 ,\ +-30234 ,\ +-30273 ,\ +-30311 ,\ +-30349 ,\ +-30387 ,\ +-30424 ,\ +-30462 ,\ +-30498 ,\ +-30535 ,\ +-30571 ,\ +-30607 ,\ +-30643 ,\ +-30679 ,\ +-30714 ,\ +-30749 ,\ +-30783 ,\ +-30818 ,\ +-30852 ,\ +-30885 ,\ +-30919 ,\ +-30952 ,\ +-30985 ,\ +-31017 ,\ +-31050 ,\ +-31082 ,\ +-31113 ,\ +-31145 ,\ +-31176 ,\ +-31206 ,\ +-31237 ,\ +-31267 ,\ +-31297 ,\ +-31327 ,\ +-31356 ,\ +-31385 ,\ +-31414 ,\ +-31442 ,\ +-31470 ,\ +-31498 ,\ +-31526 ,\ +-31553 ,\ +-31580 ,\ +-31607 ,\ +-31633 ,\ +-31659 ,\ +-31685 ,\ +-31710 ,\ +-31736 ,\ +-31760 ,\ +-31785 ,\ +-31809 ,\ +-31833 ,\ +-31857 ,\ +-31880 ,\ +-31903 ,\ +-31926 ,\ +-31949 ,\ +-31971 ,\ +-31993 ,\ +-32014 ,\ +-32036 ,\ +-32057 ,\ +-32077 ,\ +-32098 ,\ +-32118 ,\ +-32137 ,\ +-32157 ,\ +-32176 ,\ +-32195 ,\ +-32213 ,\ +-32232 ,\ +-32250 ,\ +-32267 ,\ +-32285 ,\ +-32302 ,\ +-32318 ,\ +-32335 ,\ +-32351 ,\ +-32367 ,\ +-32382 ,\ +-32397 ,\ +-32412 ,\ +-32427 ,\ +-32441 ,\ +-32455 ,\ +-32469 ,\ +-32482 ,\ +-32495 ,\ +-32508 ,\ +-32521 ,\ +-32533 ,\ +-32545 ,\ +-32556 ,\ +-32567 ,\ +-32578 ,\ +-32589 ,\ +-32599 ,\ +-32609 ,\ +-32619 ,\ +-32628 ,\ +-32637 ,\ +-32646 ,\ +-32655 ,\ +-32663 ,\ +-32671 ,\ +-32678 ,\ +-32685 ,\ +-32692 ,\ +-32699 ,\ +-32705 ,\ +-32711 ,\ +-32717 ,\ +-32722 ,\ +-32728 ,\ +-32732 ,\ +-32737 ,\ +-32741 ,\ +-32745 ,\ +-32748 ,\ +-32752 ,\ +-32755 ,\ +-32757 ,\ +-32759 ,\ +-32761 ,\ +-32763 ,\ +-32765 ,\ +-32766 ,\ +-32766 ,\ +-32767 ,\ +-32767 ,\ +-32767 ,\ +-32766 ,\ +-32766 ,\ +-32765 ,\ +-32763 ,\ +-32761 ,\ +-32759 ,\ +-32757 ,\ +-32755 ,\ +-32752 ,\ +-32748 ,\ +-32745 ,\ +-32741 ,\ +-32737 ,\ +-32732 ,\ +-32728 ,\ +-32722 ,\ +-32717 ,\ +-32711 ,\ +-32705 ,\ +-32699 ,\ +-32692 ,\ +-32685 ,\ +-32678 ,\ +-32671 ,\ +-32663 ,\ +-32655 ,\ +-32646 ,\ +-32637 ,\ +-32628 ,\ +-32619 ,\ +-32609 ,\ +-32599 ,\ +-32589 ,\ +-32578 ,\ +-32567 ,\ +-32556 ,\ +-32545 ,\ +-32533 ,\ +-32521 ,\ +-32508 ,\ +-32495 ,\ +-32482 ,\ +-32469 ,\ +-32455 ,\ +-32441 ,\ +-32427 ,\ +-32412 ,\ +-32397 ,\ +-32382 ,\ +-32367 ,\ +-32351 ,\ +-32335 ,\ +-32318 ,\ +-32302 ,\ +-32285 ,\ +-32267 ,\ +-32250 ,\ +-32232 ,\ +-32213 ,\ +-32195 ,\ +-32176 ,\ +-32157 ,\ +-32137 ,\ +-32118 ,\ +-32098 ,\ +-32077 ,\ +-32057 ,\ +-32036 ,\ +-32014 ,\ +-31993 ,\ +-31971 ,\ +-31949 ,\ +-31926 ,\ +-31903 ,\ +-31880 ,\ +-31857 ,\ +-31833 ,\ +-31809 ,\ +-31785 ,\ +-31760 ,\ +-31736 ,\ +-31710 ,\ +-31685 ,\ +-31659 ,\ +-31633 ,\ +-31607 ,\ +-31580 ,\ +-31553 ,\ +-31526 ,\ +-31498 ,\ +-31470 ,\ +-31442 ,\ +-31414 ,\ +-31385 ,\ +-31356 ,\ +-31327 ,\ +-31297 ,\ +-31267 ,\ +-31237 ,\ +-31206 ,\ +-31176 ,\ +-31145 ,\ +-31113 ,\ +-31082 ,\ +-31050 ,\ +-31017 ,\ +-30985 ,\ +-30952 ,\ +-30919 ,\ +-30885 ,\ +-30852 ,\ +-30818 ,\ +-30783 ,\ +-30749 ,\ +-30714 ,\ +-30679 ,\ +-30643 ,\ +-30607 ,\ +-30571 ,\ +-30535 ,\ +-30498 ,\ +-30462 ,\ +-30424 ,\ +-30387 ,\ +-30349 ,\ +-30311 ,\ +-30273 ,\ +-30234 ,\ +-30195 ,\ +-30156 ,\ +-30117 ,\ +-30077 ,\ +-30037 ,\ +-29997 ,\ +-29956 ,\ +-29915 ,\ +-29874 ,\ +-29832 ,\ +-29791 ,\ +-29749 ,\ +-29706 ,\ +-29664 ,\ +-29621 ,\ +-29578 ,\ +-29534 ,\ +-29491 ,\ +-29447 ,\ +-29403 ,\ +-29358 ,\ +-29313 ,\ +-29268 ,\ +-29223 ,\ +-29177 ,\ +-29131 ,\ +-29085 ,\ +-29039 ,\ +-28992 ,\ +-28945 ,\ +-28898 ,\ +-28850 ,\ +-28803 ,\ +-28755 ,\ +-28706 ,\ +-28658 ,\ +-28609 ,\ +-28560 ,\ +-28510 ,\ +-28460 ,\ +-28411 ,\ +-28360 ,\ +-28310 ,\ +-28259 ,\ +-28208 ,\ +-28157 ,\ +-28105 ,\ +-28053 ,\ +-28001 ,\ +-27949 ,\ +-27896 ,\ +-27843 ,\ +-27790 ,\ +-27737 ,\ +-27683 ,\ +-27629 ,\ +-27575 ,\ +-27521 ,\ +-27466 ,\ +-27411 ,\ +-27356 ,\ +-27300 ,\ +-27245 ,\ +-27189 ,\ +-27133 ,\ +-27076 ,\ +-27019 ,\ +-26962 ,\ +-26905 ,\ +-26848 ,\ +-26790 ,\ +-26732 ,\ +-26674 ,\ +-26615 ,\ +-26556 ,\ +-26497 ,\ +-26438 ,\ +-26378 ,\ +-26319 ,\ +-26259 ,\ +-26198 ,\ +-26138 ,\ +-26077 ,\ +-26016 ,\ +-25955 ,\ +-25893 ,\ +-25832 ,\ +-25770 ,\ +-25708 ,\ +-25645 ,\ +-25582 ,\ +-25519 ,\ +-25456 ,\ +-25393 ,\ +-25329 ,\ +-25265 ,\ +-25201 ,\ +-25137 ,\ +-25072 ,\ +-25007 ,\ +-24942 ,\ +-24877 ,\ +-24811 ,\ +-24746 ,\ +-24680 ,\ +-24613 ,\ +-24547 ,\ +-24480 ,\ +-24413 ,\ +-24346 ,\ +-24279 ,\ +-24211 ,\ +-24143 ,\ +-24075 ,\ +-24007 ,\ +-23938 ,\ +-23870 ,\ +-23801 ,\ +-23731 ,\ +-23662 ,\ +-23592 ,\ +-23522 ,\ +-23452 ,\ +-23382 ,\ +-23311 ,\ +-23241 ,\ +-23170 ,\ +-23099 ,\ +-23027 ,\ +-22956 ,\ +-22884 ,\ +-22812 ,\ +-22739 ,\ +-22667 ,\ +-22594 ,\ +-22521 ,\ +-22448 ,\ +-22375 ,\ +-22301 ,\ +-22227 ,\ +-22154 ,\ +-22079 ,\ +-22005 ,\ +-21930 ,\ +-21856 ,\ +-21781 ,\ +-21705 ,\ +-21630 ,\ +-21554 ,\ +-21479 ,\ +-21403 ,\ +-21326 ,\ +-21250 ,\ +-21173 ,\ +-21096 ,\ +-21019 ,\ +-20942 ,\ +-20865 ,\ +-20787 ,\ +-20709 ,\ +-20631 ,\ +-20553 ,\ +-20475 ,\ +-20396 ,\ +-20317 ,\ +-20238 ,\ +-20159 ,\ +-20080 ,\ +-20000 ,\ +-19921 ,\ +-19841 ,\ +-19761 ,\ +-19680 ,\ +-19600 ,\ +-19519 ,\ +-19438 ,\ +-19357 ,\ +-19276 ,\ +-19195 ,\ +-19113 ,\ +-19032 ,\ +-18950 ,\ +-18868 ,\ +-18785 ,\ +-18703 ,\ +-18620 ,\ +-18537 ,\ +-18454 ,\ +-18371 ,\ +-18288 ,\ +-18204 ,\ +-18121 ,\ +-18037 ,\ +-17953 ,\ +-17869 ,\ +-17784 ,\ +-17700 ,\ +-17615 ,\ +-17530 ,\ +-17445 ,\ +-17360 ,\ +-17275 ,\ +-17189 ,\ +-17104 ,\ +-17018 ,\ +-16932 ,\ +-16846 ,\ +-16759 ,\ +-16673 ,\ +-16586 ,\ +-16499 ,\ +-16413 ,\ +-16325 ,\ +-16238 ,\ +-16151 ,\ +-16063 ,\ +-15976 ,\ +-15888 ,\ +-15800 ,\ +-15712 ,\ +-15623 ,\ +-15535 ,\ +-15446 ,\ +-15358 ,\ +-15269 ,\ +-15180 ,\ +-15090 ,\ +-15001 ,\ +-14912 ,\ +-14822 ,\ +-14732 ,\ +-14643 ,\ +-14553 ,\ +-14462 ,\ +-14372 ,\ +-14282 ,\ +-14191 ,\ +-14101 ,\ +-14010 ,\ +-13919 ,\ +-13828 ,\ +-13736 ,\ +-13645 ,\ +-13554 ,\ +-13462 ,\ +-13370 ,\ +-13279 ,\ +-13187 ,\ +-13094 ,\ +-13002 ,\ +-12910 ,\ +-12817 ,\ +-12725 ,\ +-12632 ,\ +-12539 ,\ +-12446 ,\ +-12353 ,\ +-12260 ,\ +-12167 ,\ +-12074 ,\ +-11980 ,\ +-11886 ,\ +-11793 ,\ +-11699 ,\ +-11605 ,\ +-11511 ,\ +-11417 ,\ +-11322 ,\ +-11228 ,\ +-11133 ,\ +-11039 ,\ +-10944 ,\ +-10849 ,\ +-10754 ,\ +-10659 ,\ +-10564 ,\ +-10469 ,\ +-10374 ,\ +-10278 ,\ +-10183 ,\ +-10087 ,\ +-9992 ,\ +-9896 ,\ +-9800 ,\ +-9704 ,\ +-9608 ,\ +-9512 ,\ +-9416 ,\ +-9319 ,\ +-9223 ,\ +-9126 ,\ +-9030 ,\ +-8933 ,\ +-8836 ,\ +-8739 ,\ +-8642 ,\ +-8545 ,\ +-8448 ,\ +-8351 ,\ +-8254 ,\ +-8157 ,\ +-8059 ,\ +-7962 ,\ +-7864 ,\ +-7767 ,\ +-7669 ,\ +-7571 ,\ +-7473 ,\ +-7375 ,\ +-7277 ,\ +-7179 ,\ +-7081 ,\ +-6983 ,\ +-6885 ,\ +-6786 ,\ +-6688 ,\ +-6590 ,\ +-6491 ,\ +-6393 ,\ +-6294 ,\ +-6195 ,\ +-6096 ,\ +-5998 ,\ +-5899 ,\ +-5800 ,\ +-5701 ,\ +-5602 ,\ +-5503 ,\ +-5404 ,\ +-5305 ,\ +-5205 ,\ +-5106 ,\ +-5007 ,\ +-4907 ,\ +-4808 ,\ +-4708 ,\ +-4609 ,\ +-4509 ,\ +-4410 ,\ +-4310 ,\ +-4210 ,\ +-4111 ,\ +-4011 ,\ +-3911 ,\ +-3811 ,\ +-3712 ,\ +-3612 ,\ +-3512 ,\ +-3412 ,\ +-3312 ,\ +-3212 ,\ +-3112 ,\ +-3012 ,\ +-2911 ,\ +-2811 ,\ +-2711 ,\ +-2611 ,\ +-2511 ,\ +-2410 ,\ +-2310 ,\ +-2210 ,\ +-2110 ,\ +-2009 ,\ +-1909 ,\ +-1809 ,\ +-1708 ,\ +-1608 ,\ +-1507 ,\ +-1407 ,\ +-1307 ,\ +-1206 ,\ +-1106 ,\ +-1005 ,\ +-905 ,\ +-804 ,\ +-704 ,\ +-603 ,\ +-503 ,\ +-402 ,\ +-302 ,\ +-201 ,\ +-101 + + +#endif // SINE_CORE_H_INCLUDED diff --git a/src/digio.cpp b/src/digio.cpp index df1ab71..714f59b 100644 --- a/src/digio.cpp +++ b/src/digio.cpp @@ -23,118 +23,44 @@ #define DIG_IO_OFF 0 #define DIG_IO_ON 1 -struct IoInfo -{ - uint32_t port; - uint16_t pin; - PinMode::PinMode mode; -}; +#undef DIG_IO_ENTRY +#define DIG_IO_ENTRY(name, port, pin, mode) DigIo DigIo::name; +DIG_IO_LIST -#define DIG_IO_ENTRY(name, port, pin, mode) { port, pin, mode }, -static struct IoInfo ios[] = +void DigIo::Configure(uint32_t port, uint16_t pin, PinMode::PinMode pinMode) { - DIG_IO_LIST - { 0, 0, PinMode::LAST } -}; + uint8_t mode = GPIO_MODE_INPUT; + uint8_t cnf = GPIO_CNF_INPUT_PULL_UPDOWN; + uint16_t val = DIG_IO_OFF; -/** -* Initialize pins -* @pre if runtime configured pins are present, configure them using Configure first -*/ -void DigIo::Init() -{ - const struct IoInfo *pCur; + _port = port; + _pin = pin; - for (pCur = ios; pCur->mode != PinMode::LAST; pCur++) + switch (pinMode) { - uint8_t mode = GPIO_MODE_INPUT; - uint8_t cnf = GPIO_CNF_INPUT_PULL_UPDOWN; - uint16_t val = DIG_IO_OFF; - - switch (pCur->mode) - { - default: - case PinMode::INPUT_PD: - /* use defaults */ - break; - case PinMode::INPUT_PU: - val = DIG_IO_ON; - break; - case PinMode::INPUT_FLT: - cnf = GPIO_CNF_INPUT_FLOAT; - break; - case PinMode::INPUT_AIN: - cnf = GPIO_CNF_INPUT_ANALOG; - break; - case PinMode::OUTPUT: - mode = GPIO_MODE_OUTPUT_50_MHZ; - cnf = GPIO_CNF_OUTPUT_PUSHPULL; - break; - } - - gpio_set_mode(pCur->port, mode, cnf, pCur->pin); - if (DIG_IO_ON == val) - { - gpio_set(pCur->port, pCur->pin); - } + default: + case PinMode::INPUT_PD: + /* use defaults */ + break; + case PinMode::INPUT_PU: + val = DIG_IO_ON; + break; + case PinMode::INPUT_FLT: + cnf = GPIO_CNF_INPUT_FLOAT; + break; + case PinMode::INPUT_AIN: + cnf = GPIO_CNF_INPUT_ANALOG; + break; + case PinMode::OUTPUT: + mode = GPIO_MODE_OUTPUT_50_MHZ; + cnf = GPIO_CNF_OUTPUT_PUSHPULL; + break; } -} - -/** Remap GPIO pin at runtime. Must be called before Init() - * @param[in] io io pin to reconfigure - * @param[in] port port to use for this pin - * @param[in] pin port-pin to use for this pin - * @param[in] mode pinmode to use - */ -void DigIo::Configure(Pin::DigPin io, uint32_t port, uint16_t pin, PinMode::PinMode mode) -{ - struct IoInfo *pIo = ios + io; - pIo->port = port; - pIo->pin = pin; - pIo->mode = mode; -} - -/** -* Get pin value -* -* @param[in] io pin index -* @return pin value -*/ -bool DigIo::Get(Pin::DigPin io) -{ - const struct IoInfo *pIo = ios + io; - return gpio_get(pIo->port, pIo->pin) > 0; -} - -/** -* Set pin high -* -* @param[in] io pin index -*/ -void DigIo::Set(Pin::DigPin io) -{ - const struct IoInfo *pIo = ios + io; - return gpio_set(pIo->port, pIo->pin); -} -/** -* Set pin low -* -* @param[in] io pin index -*/ -void DigIo::Clear(Pin::DigPin io) -{ - const struct IoInfo *pIo = ios + io; - return gpio_clear(pIo->port, pIo->pin); + gpio_set_mode(port, mode, cnf, pin); + if (DIG_IO_ON == val) + { + gpio_set(port, pin); + } } -/** -* Toggle pin -* -* @param[in] io pin index -*/ -void DigIo::Toggle(Pin::DigPin io) -{ - const struct IoInfo *pIo = ios + io; - gpio_toggle(pIo->port, pIo->pin); -} diff --git a/src/errormessage.cpp b/src/errormessage.cpp index e37b2df..4f845c2 100644 --- a/src/errormessage.cpp +++ b/src/errormessage.cpp @@ -35,7 +35,8 @@ struct BufferEntry #define ERROR_MESSAGE_ENTRY(id, type) { #id, type }, static const struct ErrorDescriptor errorDescriptors[] = { - ERROR_MESSAGE_LIST + { "", ERROR_LAST }, + ERROR_MESSAGE_LIST }; #undef ERROR_MESSAGE_ENTRY diff --git a/src/foc.cpp b/src/foc.cpp new file mode 100644 index 0000000..23e5c6c --- /dev/null +++ b/src/foc.cpp @@ -0,0 +1,157 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#define CST_DIGITS 15 +#include "my_fp.h" +#include "my_math.h" +#include "foc.h" +#include "sine_core.h" + +#define SQRT3 FP_FROMFLT(1.732050807568877293527446315059) + +static const s32fp fluxLinkage = FP_FROMFLT(0.09); +static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); +static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-bit left shift because otherwise it can't be represented +static const s32fp lqminusld = FP_FROMFLT(0.0058); +static const u32fp sqrt3 = SQRT3; +static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) +static const s32fp sqrt3inv2 = 2*sqrt3inv1; //2/sqrt(2) +static const s32fp zeroOffset = FP_FROMINT(1); +static const int32_t modMax = FP_DIV(FP_FROMINT(2U), sqrt3); +static const int32_t modMaxPow2 = modMax * modMax; +static int32_t minPulse = 1000; +static int32_t maxPulse = FP_FROMINT(2) - 1000; + +s32fp FOC::id; +s32fp FOC::iq; +s32fp FOC::DutyCycles[3]; + +/** @brief Transform current to rotor system using Clarke and Park transformation + * @post flux producing (id) and torque producing (iq) current are written + * to FOC::id and FOC::iq + */ +void FOC::ParkClarke(s32fp il1, s32fp il2, uint16_t angle) +{ + s32fp sin = SineCore::Sine(angle); + s32fp cos = SineCore::Cosine(angle); + //Clarke transformation + s32fp ia = il1; + s32fp ib = FP_MUL(sqrt3inv1, il1) + FP_MUL(sqrt3inv2, il2); + //Park transformation + id = FP_MUL(cos, ia) + FP_MUL(sin, ib); + iq = FP_MUL(cos, ib) - FP_MUL(sin, ia); +} + +void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref) +{ + int32_t isSquared = is * is; + int32_t sign = is < 0 ? -1 : 1; + s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 10)); + idref = FP_TOINT(FP_DIV(fluxLinkage - term1, lqminusld)); + iqref = sign * (int32_t)sqrt(isSquared - idref * idref); +} + +int32_t FOC::GetQLimit(int32_t ud) +{ + return sqrt(modMaxPow2 - ud * ud); +} + +/** \brief Returns the resulting modulation index from uq and ud + * + * \param ud d voltage modulation index + * \param uq q voltage modulation index + * \return sqrt(ud²+uq²) + * + */ +int32_t FOC::GetTotalVoltage(int32_t ud, int32_t uq) +{ + return sqrt((uint32_t)(ud * ud) + (uint32_t)(uq * uq)); +} + +void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) +{ + s32fp sin = SineCore::Sine(angle); + s32fp cos = SineCore::Cosine(angle); + + //Inverse Park transformation + s32fp ua = (cos * ud - sin * uq) >> CST_DIGITS; + s32fp ub = (cos * uq + sin * ud) >> CST_DIGITS; + //Inverse Clarke transformation + DutyCycles[0] = ua; + DutyCycles[1] = (-ua + FP_MUL(SQRT3, ub)) / 2; + DutyCycles[2] = (-ua - FP_MUL(SQRT3, ub)) / 2; + + int32_t offset = SineCore::CalcSVPWMOffset(DutyCycles[0], DutyCycles[1], DutyCycles[2]); + + for (int i = 0; i < 3; i++) + { + /* subtract it from all 3 phases -> no difference in phase-to-phase voltage */ + DutyCycles[i] -= offset; + /* Shift above 0 */ + DutyCycles[i] += zeroOffset; + /* Short pulse suppression */ + if (DutyCycles[i] < minPulse) + { + DutyCycles[i] = 0U; + } + else if (DutyCycles[i] > maxPulse) + { + DutyCycles[i] = FP_FROMINT(2); + } + } +} + +int32_t FOC::GetMaximumModulationIndex() +{ + return modMax; +} + +uint32_t FOC::sqrt(uint32_t rad) +{ + uint32_t radshift = (rad < 10000 ? 5 : (rad < 10000000 ? 9 : (rad < 1000000000 ? 13 : 15))); + uint32_t sqrt = (rad >> radshift) + 1; //Starting value for newton iteration + uint32_t sqrtl; + + do { + sqrtl = sqrt; + sqrt = (sqrt + rad / sqrt) / 2; + } while ((sqrtl - sqrt) > 1); + + return sqrt; +} + +#define R1 FP_FROMFLT(0.03) +#define S1 FP_FROMFLT(0.15) +#define R2 FP_FROMFLT(0.5) +#define S2 FP_FROMFLT(0.5) +#define S3 FP_FROMFLT(1) +#define RADSTART(x) x < R1 ? S1 : (x < R2 ? S2 : S3) + +u32fp FOC::fpsqrt(u32fp rad) +{ + u32fp sqrt = RADSTART(rad); + u32fp sqrtl; + + do { + sqrtl = sqrt; + sqrt = (sqrt + FP_DIV(rad, sqrt)) >> 1; + } while ((sqrtl - sqrt) > 1); + + return sqrt; +} + diff --git a/src/fu.cpp b/src/fu.cpp new file mode 100644 index 0000000..463645f --- /dev/null +++ b/src/fu.cpp @@ -0,0 +1,60 @@ +#include "fu.h" + +uint32_t MotorVoltage::boost = 0; +u32fp MotorVoltage::fac; +uint32_t MotorVoltage::maxAmp; +u32fp MotorVoltage::endFrq = 1; //avoid division by 0 when not set +u32fp MotorVoltage::minFrq; + +/** Set 0 Hz boost to overcome winding resistance */ +void MotorVoltage::SetBoost(uint32_t boost /**< amplitude in digit */) +{ + MotorVoltage::boost = boost; + CalcFac(); +} + +/** Set frequency where the full amplitude is to be provided */ +void MotorVoltage::SetWeakeningFrq(u32fp frq) +{ + endFrq = frq; + CalcFac(); +} + +/** Get amplitude for a given frequency */ +uint32_t MotorVoltage::GetAmp(u32fp frq) +{ + return MotorVoltage::GetAmpPerc(frq, FP_FROMINT(100)); +} + +/** Get amplitude for given frequency multiplied with given percentage */ +uint32_t MotorVoltage::GetAmpPerc(u32fp frq, u32fp perc) +{ + uint32_t amp = FP_MUL(perc, (FP_TOINT(FP_MUL(fac, frq)) + boost)) / 100; + if (frq < minFrq) + { + amp = 0; + } + if (amp > maxAmp) + { + amp = maxAmp; + } + + return amp; +} + +void MotorVoltage::SetMaxAmp(uint32_t maxAmp) +{ + MotorVoltage::maxAmp = maxAmp; + CalcFac(); +} + +void MotorVoltage::SetMinFrq(u32fp frq) +{ + minFrq = frq; +} + +/** Calculate slope of u/f */ +void MotorVoltage::CalcFac() +{ + fac = FP_DIV(FP_FROMINT(maxAmp - boost), endFrq); +} diff --git a/src/picontroller.cpp b/src/picontroller.cpp new file mode 100644 index 0000000..61c9043 --- /dev/null +++ b/src/picontroller.cpp @@ -0,0 +1,42 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2018 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "picontroller.h" +#include "my_math.h" + +PiController::PiController() + : kp(0), ki(0), esum(0), refVal(0), frequency(1), maxY(0), minY(0), y(0) +{ +} + +int32_t PiController::Run(s32fp curVal) +{ + s32fp err = refVal - curVal; + + if (y < maxY && y > minY) + { + esum += err; + } + + y = offset + FP_TOINT(err * kp + (esum / frequency) * ki); + + y = MAX(y, minY); + y = MIN(y, maxY); + + return y; +} diff --git a/src/sine_core.cpp b/src/sine_core.cpp new file mode 100644 index 0000000..2ed315d --- /dev/null +++ b/src/sine_core.cpp @@ -0,0 +1,199 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2012 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + /** @addtogroup G_sine Sine wave generation + * @{ + */ +#include "sine_core.h" + +#define SINTAB_ARGDIGITS 11 +#define SINTAB_ENTRIES (1 << SINTAB_ARGDIGITS) +/* Value range of sine lookup table */ +#define SINTAB_MAX (1 << BITS) +#define BRAD_PI (1 << (BITS - 1)) + +#define PHASE_SHIFT90 ((uint32_t)( SINLU_ONEREV / 4)) +#define PHASE_SHIFT120 ((uint32_t)( SINLU_ONEREV / 3)) +#define PHASE_SHIFT240 ((uint32_t)(2 * (SINLU_ONEREV / 3))) + +uint32_t SineCore::minPulse = 0; +uint32_t SineCore::ampl = 0; +const int16_t SineCore::SinTab[] = { SINTAB };/* sine LUT */ +const uint16_t SineCore::ZERO_OFFSET = SINTAB_MAX / 2; +const int SineCore::BITS = 16; +const uint16_t SineCore::MAXAMP = 37813; +uint32_t SineCore::DutyCycles[3]; + +/** Calculate the next dutycyles. + * This function is meant to be called by your timer interrupt handler + */ +void SineCore::Calc(uint16_t angle) +{ + int32_t Ofs; + uint32_t Idx; + + int32_t sine[3]; + + /* 1. Calculate sine */ + sine[0] = SineLookup(angle); + sine[1] = SineLookup((angle + PHASE_SHIFT120) & 0xFFFF); + sine[2] = SineLookup((angle + PHASE_SHIFT240) & 0xFFFF); + + for (Idx = 0; Idx < 3; Idx++) + { + /* 2. Set desired amplitude */ + sine[Idx] = MultiplyAmplitude(ampl, sine[Idx]); + } + + /* 3. Calculate the offset of SVPWM */ + Ofs = CalcSVPWMOffset(sine[0], sine[1], sine[2]); + + for (Idx = 0; Idx < 3; Idx++) + { + /* 4. subtract it from all 3 phases -> no difference in phase-to-phase voltage */ + sine[Idx] -= Ofs; + /* Shift above 0 */ + DutyCycles[Idx] = sine[Idx] + ZERO_OFFSET; + /* Short pulse supression */ + if (DutyCycles[Idx] < minPulse) + { + DutyCycles[Idx] = 0U; + } + else if (DutyCycles[Idx] > (SINTAB_MAX - minPulse)) + { + DutyCycles[Idx] = SINTAB_MAX; + } + } +} + +s32fp SineCore::Sine(uint16_t angle) +{ + return SineLookup(angle); +} + +s32fp SineCore::Cosine(uint16_t angle) +{ + return SineLookup((PHASE_SHIFT90 + angle) & 0xFFFF); +} + +//Found here: http://www.coranac.com/documents/arctangent/ +uint16_t SineCore::Atan2(int32_t x, int32_t y) +{ + if(y==0) + return (x>=0 ? 0 : BRAD_PI); + + static const int fixShift = 15; + int phi = 0, t, t2, dphi; + + if (y < 0) + { + x = -x; + y = -y; + phi += 4; + } + if (x <= 0) + { + int temp = x; + x = y; + y = -temp; + phi += 2; + } + if (x <= y) + { + int temp = y - x; + x = x + y; + y = temp; + phi += 1; + } + + phi *= BRAD_PI/4; + + t= (y << fixShift) / x; + t2= -t*t>>fixShift; + + dphi= 0x0470; + dphi= 0x1029 + (t2*dphi>>fixShift); + dphi= 0x1F0B + (t2*dphi>>fixShift); + dphi= 0x364C + (t2*dphi>>fixShift); + dphi= 0xA2FC + (t2*dphi>>fixShift); + dphi= dphi*t>>fixShift; + + return phi + ((dphi+2)>>2); +} + +/** Set amplitude of the synthesized sine wave */ +void SineCore::SetAmp(uint32_t amp /**< amplitude in digit. Largest value is 37813 */) +{ + ampl = amp; +} + +uint32_t SineCore::GetAmp() +{ + return ampl; +} + +/** Sets the minimum pulse width in normalized digits. + * @post duty cylcles shorter than minWidth are supressed, both on the negative and the positive pulse + */ +void SineCore::SetMinPulseWidth(uint32_t minWidth) +{ + minPulse = minWidth; +} + +/* Performs a lookup in the sine table */ +/* 0 = 0, 2Pi = 65535 */ +int32_t SineCore::SineLookup(uint16_t Arg) +{ + /* No interpolation for now */ + /* We divide arg by 2^(SINTAB_ARGDIGITS) */ + /* No we can directly address the lookup table */ + Arg >>= SINLU_ARGDIGITS - SINTAB_ARGDIGITS; + return (int32_t)SinTab[Arg]; +} + +/* 0 = 0, 1 = 32767 */ +int32_t SineCore::MultiplyAmplitude(uint16_t Amplitude, int32_t Baseval) +{ + int32_t temp = (int32_t)((uint32_t)Amplitude * Baseval); + /* Divide by 32768 */ + /* -> Allow overmodulation, for SVPWM or FTPWM */ + temp >>= (BITS - 1); + return temp; +} + + +int32_t SineCore::CalcSVPWMOffset(int32_t a, int32_t b, int32_t c) +{ + int32_t Minimum = min(min(a, b), c); + int32_t Maximum = max(max(a, b), c); + int32_t Offset = Minimum + Maximum; + + return (Offset >> 1); +} + +int32_t SineCore::min(int32_t a, int32_t b) +{ + return (a <= b)?a:b; +} + +int32_t SineCore::max(int32_t a, int32_t b) +{ + return (a >= b)?a:b; +} + +/** @} */ diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp index e0c4e3c..d12d7ae 100644 --- a/src/stm32scheduler.cpp +++ b/src/stm32scheduler.cpp @@ -29,8 +29,8 @@ Stm32Scheduler::Stm32Scheduler(uint32_t timer) /* Setup timers upcounting and auto preload enable */ timer_enable_preload(timer); timer_direction_up(timer); - /* Set prescaler to count at 100 kHz = 72 MHz/7200 - 1 */ - timer_set_prescaler(timer, 719); + /* Set prescaler to count at 10 kHz = 72 MHz/7200 - 1 */ + timer_set_prescaler(timer, 7199); /* Maximum counter value */ timer_set_period(timer, 0xFFFF); @@ -54,7 +54,7 @@ void Stm32Scheduler::AddTask(void (*function)(void), uint16_t period) /* Assign task function and period */ functions[nextTask] = function; - periods [nextTask] = period; + periods [nextTask] = period * 10; /* Enable interrupt for that channel */ timer_enable_irq(timer, TIM_DIER_CC1IE << nextTask); From 243c92fb446fefa02f21dbbd04238cda698ec231 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 4 Nov 2019 23:51:54 +0100 Subject: [PATCH 04/83] dynamic anain array --- include/anain.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/include/anain.h b/include/anain.h index e35ed9d..dd6b978 100644 --- a/include/anain.h +++ b/include/anain.h @@ -26,8 +26,6 @@ class AnaIn static uint16_t Get(AnaIn::AnaIns); private: - - static const AnaInfo ins[]; static uint16_t values[]; @@ -36,9 +34,9 @@ class AnaIn }; #define ANA_IN_ENTRY(name, port, pin) { port, pin }, -/** Usage: AnaIn::AnaInfo analogInputs[] = ANA_IN_ARRAY; +/** Usage: AnaIn::AnaInfo analogInputs[] = ANA_IN_ARRAY(ANA_IN_LIST); * AnaIn::Init(analogInputs); */ -#define ANA_IN_ARRAY { ANA_IN_LIST } +#define ANA_IN_ARRAY(l) { l } #endif // ANAIO_H_INCLUDED From 103cbec2b6026666134a9d2926ec1b136ba1d64f Mon Sep 17 00:00:00 2001 From: johannes Date: Tue, 5 Nov 2019 12:48:52 +0100 Subject: [PATCH 05/83] Added function to iterate all CAN messages --- include/stm32_can.h | 1 + src/stm32_can.cpp | 112 ++++++++++++++++++++++++++++++++++++++++- src/stm32scheduler.cpp | 8 +-- 3 files changed, 115 insertions(+), 6 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index 9129f50..899a018 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -48,6 +48,7 @@ class Can static int AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); static int Remove(Param::PARAM_NUM param); static bool FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx); + static void IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, bool)); }; diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 67ec572..ce87f44 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -21,6 +21,7 @@ #include "hwdefs.h" #include "my_string.h" #include "my_math.h" +#include "printf.h" #include #include #include @@ -101,13 +102,14 @@ static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM pa static void ReplaceParamEnumByUid(CANIDMAP *canMap); static void ReplaceParamUidByEnum(CANIDMAP *canMap); static void ConfigureFilters(); +static void DummyCallback(uint32_t i, uint32_t* d) { i=i; d=d; } static CANIDMAP canSendMap[MAX_MESSAGES]; static CANIDMAP canRecvMap[MAX_MESSAGES]; static uint32_t lastRxTimestamp = 0; static SENDBUFFER sendBuffer[SENDBUFFER_LEN]; static int sendCnt = 0; -static void (*recvCallback)(uint32_t, uint32_t*) = 0; +static void (*recvCallback)(uint32_t, uint32_t*) = DummyCallback; static uint16_t userIds[MAX_USER_MESSAGES]; static int nextUserMessageIndex = 0; @@ -119,11 +121,41 @@ static const CANSPEED canSpeed[Can::BaudLast] = { CAN_BTR_TS1_6TQ, CAN_BTR_TS2_5TQ, 3 }, //1000kbps }; +/** \brief Add periodic CAN message + * + * \param param Parameter index of parameter to be sent + * \param canId CAN identifier of generated message + * \param offset bit offset within the 64 message bits + * \param length number of bits + * \param gain Fixed point gain to be multiplied before sending + * \return success: number of active messages + * Fault: + * - CAN_ERR_INVALID_ID ID was > 0x7ff + * - CAN_ERR_INVALID_OFS Offset > 63 + * - CAN_ERR_INVALID_LEN Length > 32 + * - CAN_ERR_MAXMESSAGES Already 10 send messages defined + * - CAN_ERR_MAXITEMS Already 8 items in message + */ int Can::AddSend(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) { return Add(canSendMap, param, canId, offset, length, gain); } +/** \brief Map data from CAN bus to parameter + * + * \param param Parameter index of parameter to be received + * \param canId CAN identifier of consumed message + * \param offset bit offset within the 64 message bits + * \param length number of bits + * \param gain Fixed point gain to be multiplied after receiving + * \return success: number of active messages + * Fault: + * - CAN_ERR_INVALID_ID ID was > 0x7ff + * - CAN_ERR_INVALID_OFS Offset > 63 + * - CAN_ERR_INVALID_LEN Length > 32 + * - CAN_ERR_MAXMESSAGES Already 10 receive messages defined + * - CAN_ERR_MAXITEMS Already 8 items in message + */ int Can::AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) { int res = Add(canRecvMap, param, canId, offset, length, gain); @@ -131,11 +163,21 @@ int Can::AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16f return res; } +/** \brief Set function to be called for user handled CAN messages + * + * \param recv Function pointer to void func(uint32_t, uint32_t[2]) - ID, Data + */ void Can::SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)) { recvCallback = recv; } +/** \brief Add CAN Id to user message list + * \post Receive callback will be called when a message with this Id id received + * \param canId CAN identifier of message to be user handled + * \return true: success, false: already 10 messages registered + * + */ bool Can::RegisterUserMessage(int canId) { if (nextUserMessageIndex < MAX_USER_MESSAGES) @@ -148,6 +190,16 @@ bool Can::RegisterUserMessage(int canId) return false; } +/** \brief Find first occurence of parameter in CAN map and output its mapping info + * + * \param[in] param Index of parameter to be looked up + * \param[out] canId CAN identifier that the parameter is mapped to + * \param[out] offset bit offset that the parameter is mapped to + * \param[out] length number of bits that the parameter is mapped to + * \param[out] gain Parameter gain + * \param[out] rx true: Parameter is received via CAN, false: sent via CAN + * \return true: parameter is mapped, false: not mapped + */ bool Can::FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx) { rx = false; @@ -175,6 +227,8 @@ bool Can::FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, return false; } +/** \brief Save CAN mapping to flash + */ void Can::Save() { uint32_t crc; @@ -196,6 +250,8 @@ void Can::Save() ReplaceParamUidByEnum(canRecvMap); } +/** \brief Send all defined messages + */ void Can::SendAll() { forEachCanMap(curMap, canSendMap) @@ -222,12 +278,21 @@ void Can::SendAll() } } -void Can::Clear(void) +/** \brief Clear all defined messages + */ +void Can::Clear() { ClearMap(canSendMap); ClearMap(canRecvMap); + ConfigureFilters(); } +/** \brief Remove all occurences of given parameter from CAN map + * + * \param param Parameter index to be removed + * \return int number of removed items + * + */ int Can::Remove(Param::PARAM_NUM param) { int removed = RemoveFromMap(canSendMap, param); @@ -236,6 +301,12 @@ int Can::Remove(Param::PARAM_NUM param) return removed; } +/** \brief Init can hardware with given baud rate + * + * \param baudrate enum baudrates + * \return void + * + */ void Can::Init(enum baudrates baudrate) { Clear(); @@ -262,6 +333,12 @@ void Can::Init(enum baudrates baudrate) can_enable_irq(CAN1, CAN_IER_FMPIE0); } +/** \brief Set baud rate to given value + * + * \param baudrate enum baudrates + * \return void + * + */ void Can::SetBaudrate(enum baudrates baudrate) { // CAN cell init. @@ -285,11 +362,23 @@ void Can::SetBaudrate(enum baudrates baudrate) false); } +/** \brief Get RTC time when last message was received + * + * \return uint32_t RTC time + * + */ uint32_t Can::GetLastRxTimestamp() { return lastRxTimestamp; } +/** \brief Send a user defined CAN message + * + * \param canId uint32_t + * \param data[2] uint32_t + * \return void + * + */ void Can::Send(uint32_t canId, uint32_t data[2]) { can_disable_irq(CAN1, CAN_IER_TMEIE); @@ -309,6 +398,25 @@ void Can::Send(uint32_t canId, uint32_t data[2]) } } +void Can::IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, bool)) +{ + bool done = false, rx = false; + + for (CANIDMAP *map = canSendMap; !done; map = canRecvMap) + { + forEachCanMap(curMap, map) + { + forEachPosMap(curPos, curMap) + { + callback((Param::PARAM_NUM)curPos->mapParam, curMap->canId, curPos->offsetBits, curPos->numBits, curPos->gain, rx); + } + } + done = rx; + rx = true; + } +} + +/****************** Private methods and ISRs ********************/ extern "C" void usb_lp_can_rx0_isr(void) { uint32_t id; diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp index d12d7ae..5aab385 100644 --- a/src/stm32scheduler.cpp +++ b/src/stm32scheduler.cpp @@ -29,8 +29,8 @@ Stm32Scheduler::Stm32Scheduler(uint32_t timer) /* Setup timers upcounting and auto preload enable */ timer_enable_preload(timer); timer_direction_up(timer); - /* Set prescaler to count at 10 kHz = 72 MHz/7200 - 1 */ - timer_set_prescaler(timer, 7199); + /* Set prescaler to count at 100 kHz = 72 MHz/7200 - 1 */ + timer_set_prescaler(timer, 719); /* Maximum counter value */ timer_set_period(timer, 0xFFFF); @@ -54,7 +54,7 @@ void Stm32Scheduler::AddTask(void (*function)(void), uint16_t period) /* Assign task function and period */ functions[nextTask] = function; - periods [nextTask] = period * 10; + periods [nextTask] = period * 100; /* Enable interrupt for that channel */ timer_enable_irq(timer, TIM_DIER_CC1IE << nextTask); @@ -88,7 +88,7 @@ int Stm32Scheduler::GetCpuLoad() int totalLoad = 0; for (int i = 0; i < MAX_TASKS; i++) { - int load = (1000 * execTicks[i]) / periods[i]; + int load = (10 * execTicks[i]) / periods[i]; totalLoad += load; } return totalLoad; From 4c2590d99f1e77a2df7dfaf652c0828c3d6adcc1 Mon Sep 17 00:00:00 2001 From: johannes Date: Sun, 10 Nov 2019 21:04:28 +0100 Subject: [PATCH 06/83] Improved anti-windup, preventing locked integrator --- include/picontroller.h | 1 - src/picontroller.cpp | 18 +++++++----------- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/include/picontroller.h b/include/picontroller.h index 4f17fff..d8e3857 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -77,7 +77,6 @@ class PiController int32_t frequency; //!< Calling frequency int32_t maxY; int32_t minY; - int32_t y; //!< Member variable "y" }; #endif // PIREGULATOR_H diff --git a/src/picontroller.cpp b/src/picontroller.cpp index 61c9043..ca61c90 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -20,7 +20,7 @@ #include "my_math.h" PiController::PiController() - : kp(0), ki(0), esum(0), refVal(0), frequency(1), maxY(0), minY(0), y(0) + : kp(0), ki(0), esum(0), refVal(0), frequency(1), maxY(0), minY(0) { } @@ -28,15 +28,11 @@ int32_t PiController::Run(s32fp curVal) { s32fp err = refVal - curVal; - if (y < maxY && y > minY) - { - esum += err; - } + esum += err; + int32_t y = offset + FP_TOINT(err * kp + (esum / frequency) * ki); + int32_t ylim = MAX(y, minY); + ylim = MIN(ylim, maxY); + esum += ((ylim - y) * frequency) / (ki + 1); //anti windup - y = offset + FP_TOINT(err * kp + (esum / frequency) * ki); - - y = MAX(y, minY); - y = MIN(y, maxY); - - return y; + return ylim; } From 530c56929f88af7e3a9e7e4c34f4f7044e583985 Mon Sep 17 00:00:00 2001 From: johannes Date: Sun, 19 Jan 2020 18:08:38 +0100 Subject: [PATCH 07/83] Limiting offset the maximum output Ignoring parameters with ID==0 when loading --- include/picontroller.h | 7 ++++++- src/param_save.cpp | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/include/picontroller.h b/include/picontroller.h index d8e3857..1a8445c 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -20,6 +20,7 @@ #define PIREGULATOR_H #include "my_fp.h" +#include "my_math.h" class PiController { @@ -42,7 +43,11 @@ class PiController */ void SetRef(s32fp val) { refVal = val; } - void SetOffset(int32_t ofs) { offset = ofs; } + void SetOffset(int32_t ofs) + { + offset = MAX(minY, ofs); + offset = MIN(maxY, offset); + } s32fp GetRef() { return refVal; } diff --git a/src/param_save.cpp b/src/param_save.cpp index b9d902e..b8cdc73 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -106,7 +106,7 @@ int parm_load() for (unsigned int idxPage = 0; idxPage < NUM_PARAMS; idxPage++) { Param::PARAM_NUM idx = Param::NumFromId(parmPage->data[idxPage].key); - if (idx != Param::PARAM_INVALID) + if (idx != Param::PARAM_INVALID && parmPage->data[idxPage].key > 0) { Param::SetFlt(idx, parmPage->data[idxPage].value); Param::SetFlagsRaw(idx, parmPage->data[idxPage].flags); From daa6887e07570ef511328a273f72832f93540e4b Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 6 Feb 2020 12:13:06 +0100 Subject: [PATCH 08/83] Removed offset, allow setting proportional and integral gain separately --- include/picontroller.h | 10 +++------- src/picontroller.cpp | 2 +- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/include/picontroller.h b/include/picontroller.h index 1a8445c..69ed5e7 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -38,17 +38,14 @@ class PiController this->ki = ki; } + void SetProportionalGain(int kp) { this->kp = kp; } + void SetIntegralGain(int ki) { this->ki = ki; } + /** Set regulator target set point * \param val regulator target */ void SetRef(s32fp val) { refVal = val; } - void SetOffset(int32_t ofs) - { - offset = MAX(minY, ofs); - offset = MIN(maxY, offset); - } - s32fp GetRef() { return refVal; } /** Set maximum regulator output @@ -78,7 +75,6 @@ class PiController int32_t ki; //!< Member variable "ki" s32fp esum; //!< Member variable "esum" s32fp refVal; - int32_t offset; int32_t frequency; //!< Calling frequency int32_t maxY; int32_t minY; diff --git a/src/picontroller.cpp b/src/picontroller.cpp index ca61c90..394badf 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -29,7 +29,7 @@ int32_t PiController::Run(s32fp curVal) s32fp err = refVal - curVal; esum += err; - int32_t y = offset + FP_TOINT(err * kp + (esum / frequency) * ki); + int32_t y = FP_TOINT(err * kp + (esum / frequency) * ki); int32_t ylim = MAX(y, minY); ylim = MIN(ylim, maxY); esum += ((ylim - y) * frequency) / (ki + 1); //anti windup From 6cb3a84a2e91a836891397a076966937226d72b7 Mon Sep 17 00:00:00 2001 From: johannes Date: Tue, 14 Apr 2020 11:01:50 +0200 Subject: [PATCH 09/83] More comments --- src/foc.cpp | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index 23e5c6c..cb74d75 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -23,6 +23,12 @@ #include "sine_core.h" #define SQRT3 FP_FROMFLT(1.732050807568877293527446315059) +#define R1 FP_FROMFLT(0.03) +#define S1 FP_FROMFLT(0.15) +#define R2 FP_FROMFLT(0.5) +#define S2 FP_FROMFLT(0.5) +#define S3 FP_FROMFLT(1) +#define RADSTART(x) x < R1 ? S1 : (x < R2 ? S2 : S3) static const s32fp fluxLinkage = FP_FROMFLT(0.09); static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); @@ -57,6 +63,14 @@ void FOC::ParkClarke(s32fp il1, s32fp il2, uint16_t angle) iq = FP_MUL(cos, ib) - FP_MUL(sin, ia); } +/** \brief distribute motor current in magnetic torque and reluctance torque with the least total current + * + * \param is int32_t total motor current + * \param[out] idref int32_t& resulting direct current reference + * \param[out] iqref int32_t& resulting quadrature current reference + * \return void + * + */ void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref) { int32_t isSquared = is * is; @@ -83,6 +97,14 @@ int32_t FOC::GetTotalVoltage(int32_t ud, int32_t uq) return sqrt((uint32_t)(ud * ud) + (uint32_t)(uq * uq)); } +/** \brief Calculate duty cycles for generating ud and uq at given angle + * + * \param ud int32_t direct voltage + * \param uq int32_t quadrature voltage + * \param angle uint16_t rotor angle + * \return void + * + */ void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) { s32fp sin = SineCore::Sine(angle); @@ -135,13 +157,6 @@ uint32_t FOC::sqrt(uint32_t rad) return sqrt; } -#define R1 FP_FROMFLT(0.03) -#define S1 FP_FROMFLT(0.15) -#define R2 FP_FROMFLT(0.5) -#define S2 FP_FROMFLT(0.5) -#define S3 FP_FROMFLT(1) -#define RADSTART(x) x < R1 ? S1 : (x < R2 ? S2 : S3) - u32fp FOC::fpsqrt(u32fp rad) { u32fp sqrt = RADSTART(rad); From 64128168b855b17d961b9e392616b2aa43fabe3d Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 20 Apr 2020 11:13:13 +0200 Subject: [PATCH 10/83] Made CAN class instantiatable for 2 CAN interfaces --- include/stm32_can.h | 85 +++++++++++--- src/stm32_can.cpp | 270 ++++++++++++++++++++++++-------------------- 2 files changed, 220 insertions(+), 135 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index 899a018..90e901e 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -27,6 +27,9 @@ #define CAN_ERR_MAXMESSAGES -4 #define CAN_ERR_MAXITEMS -5 +class CANIDMAP; +class SENDBUFFER; + class Can { public: @@ -35,20 +38,74 @@ class Can Baud250, Baud500, Baud800, Baud1000, BaudLast }; - static void Clear(void); - static void Init(enum baudrates baudrate); - static void SetBaudrate(enum baudrates baudrate); - static void Send(uint32_t canId, uint32_t data[2]); - static void SendAll(); - static void Save(); - static void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)); - static bool RegisterUserMessage(int canId); - static uint32_t GetLastRxTimestamp(); - static int AddSend(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); - static int AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); - static int Remove(Param::PARAM_NUM param); - static bool FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx); - static void IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, bool)); + Can(uint32_t baseAddr, enum baudrates baudrate); + void Clear(void); + void SetBaudrate(enum baudrates baudrate); + void Send(uint32_t canId, uint32_t data[2]); + void SendAll(); + void Save(); + void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)); + bool RegisterUserMessage(int canId); + uint32_t GetLastRxTimestamp(); + int AddSend(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); + int AddRecv(Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); + int Remove(Param::PARAM_NUM param); + bool FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, s32fp& gain, bool& rx); + void IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, bool)); + void HandleRx(int fifo); + void HandleTx(); + static Can* GetInterface(int index); + +private: + static const int MAX_ITEMS_PER_MESSAGE = 8; + static const int MAX_MESSAGES = 10; + static const int SENDBUFFER_LEN = 20; + static const int MAX_USER_MESSAGES = 10; + + struct CANPOS + { + uint16_t mapParam; + s16fp gain; + uint8_t offsetBits; + int8_t numBits; + }; + + struct CANIDMAP + { + uint16_t canId; + CANPOS items[MAX_ITEMS_PER_MESSAGE]; + }; + + struct SENDBUFFER + { + uint16_t id; + uint32_t data[2]; + }; + + CANIDMAP canSendMap[MAX_MESSAGES]; + CANIDMAP canRecvMap[MAX_MESSAGES]; + uint32_t lastRxTimestamp; + SENDBUFFER sendBuffer[SENDBUFFER_LEN]; + int sendCnt; + void (*recvCallback)(uint32_t, uint32_t*); + uint16_t userIds[MAX_USER_MESSAGES]; + int nextUserMessageIndex; + uint32_t canDev; + + void ProcessSDO(uint32_t data[2]); + void ClearMap(CANIDMAP *canMap); + int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param); + int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); + uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len); + int LoadFromFlash(); + CANIDMAP *FindById(CANIDMAP *canMap, int canId); + int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param); + void ReplaceParamEnumByUid(CANIDMAP *canMap); + void ReplaceParamUidByEnum(CANIDMAP *canMap); + void ConfigureFilters(); + void SetFilterBank(int& idIndex, int& filterId, uint16_t* idList); + + static Can* interfaces[]; }; diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index ce87f44..d874857 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -28,13 +28,12 @@ #include #include #include +#include +#include #include "stm32_can.h" -#define MAX_MESSAGES 10 -#define MAX_ITEMS_PER_MESSAGE 8 -#define MAX_USER_MESSAGES 10 +#define MAX_INTERFACES 2 #define IDS_PER_BANK 4 -#define SENDBUFFER_LEN 20 #define SDO_WRITE 0x40 #define SDO_READ 0x22 #define SDO_ABORT 0x80 @@ -56,63 +55,24 @@ #error CANMAP will not fit in one flash page #endif -typedef struct -{ - uint16_t mapParam; - s16fp gain; - uint8_t offsetBits; - int8_t numBits; -} CANPOS; - -typedef struct -{ - uint16_t canId; - CANPOS items[MAX_ITEMS_PER_MESSAGE]; -} CANIDMAP; - -typedef struct +struct CAN_SDO { uint8_t cmd; uint16_t index; uint8_t subIndex; uint32_t data; -} __attribute__((packed)) CAN_SDO; +} __attribute__((packed)); -typedef struct +struct CANSPEED { uint32_t ts1; uint32_t ts2; uint32_t prescaler; -} CANSPEED; - -typedef struct -{ - uint16_t id; - uint32_t data[2]; -} SENDBUFFER; - -static void ProcessSDO(uint32_t data[2]); -static void ClearMap(CANIDMAP *canMap); -static int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param); -static int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain); -static uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len); -static int LoadFromFlash(); -static CANIDMAP *FindById(CANIDMAP *canMap, int canId); -static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param); -static void ReplaceParamEnumByUid(CANIDMAP *canMap); -static void ReplaceParamUidByEnum(CANIDMAP *canMap); -static void ConfigureFilters(); -static void DummyCallback(uint32_t i, uint32_t* d) { i=i; d=d; } +}; -static CANIDMAP canSendMap[MAX_MESSAGES]; -static CANIDMAP canRecvMap[MAX_MESSAGES]; -static uint32_t lastRxTimestamp = 0; -static SENDBUFFER sendBuffer[SENDBUFFER_LEN]; -static int sendCnt = 0; -static void (*recvCallback)(uint32_t, uint32_t*) = DummyCallback; -static uint16_t userIds[MAX_USER_MESSAGES]; -static int nextUserMessageIndex = 0; +Can* Can::interfaces[MAX_INTERFACES]; +static void DummyCallback(uint32_t i, uint32_t* d) { i=i; d=d; } static const CANSPEED canSpeed[Can::BaudLast] = { { CAN_BTR_TS1_9TQ, CAN_BTR_TS2_6TQ, 9 }, //250kbps @@ -302,35 +262,65 @@ int Can::Remove(Param::PARAM_NUM param) } /** \brief Init can hardware with given baud rate + * Initializes the following sub systems: + * - CAN hardware itself + * - Appropriate GPIO pins (non-remapped) + * - Enables appropriate interrupts in NVIC * + * \param baseAddr base address of CAN peripheral, CAN1 or CAN2 * \param baudrate enum baudrates * \return void * */ -void Can::Init(enum baudrates baudrate) +Can::Can(uint32_t baseAddr, enum baudrates baudrate) + : lastRxTimestamp(0), sendCnt(0), recvCallback(DummyCallback), nextUserMessageIndex(0), canDev(baseAddr) { Clear(); LoadFromFlash(); - AFIO_MAPR |= AFIO_MAPR_CAN1_REMAP_PORTA; - - // Configure CAN pin: RX (input pull-up). - gpio_set_mode(GPIO_BANK_CAN1_RX, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_RX); - gpio_set(GPIO_BANK_CAN1_RX, GPIO_CAN1_RX); - - // Configure CAN pin: TX.- - gpio_set_mode(GPIO_BANK_CAN1_TX, GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_TX); - + switch (baseAddr) + { + case CAN1: + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN1_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_RX); + gpio_set(GPIO_BANK_CAN1_RX, GPIO_CAN1_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN1_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_TX); + //CAN1 RX and TX IRQs + nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ); //CAN RX + nvic_set_priority(NVIC_USB_LP_CAN_RX0_IRQ, 0xf << 4); //lowest priority + nvic_enable_irq(NVIC_CAN_RX1_IRQ); //CAN RX + nvic_set_priority(NVIC_CAN_RX1_IRQ, 0xf << 4); //lowest priority + nvic_enable_irq(NVIC_USB_HP_CAN_TX_IRQ); //CAN TX + nvic_set_priority(NVIC_USB_HP_CAN_TX_IRQ, 0xf << 4); //lowest priority + interfaces[0] = this; + break; + case CAN2: + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN2_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN2_RX); + gpio_set(GPIO_BANK_CAN2_RX, GPIO_CAN2_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN2_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN2_TX); + + //CAN2 RX and TX IRQs + nvic_enable_irq(NVIC_CAN2_RX0_IRQ); //CAN RX + nvic_set_priority(NVIC_CAN2_RX0_IRQ, 0xf << 4); //lowest priority + nvic_enable_irq(NVIC_CAN2_RX1_IRQ); //CAN RX + nvic_set_priority(NVIC_CAN2_RX1_IRQ, 0xf << 4); //lowest priority + nvic_enable_irq(NVIC_CAN2_TX_IRQ); //CAN RX + nvic_set_priority(NVIC_CAN2_TX_IRQ, 0xf << 4); //lowest priority + interfaces[1] = this; + break; + } // Reset CAN - can_reset(CAN1); + can_reset(canDev); SetBaudrate(baudrate); ConfigureFilters(); - // Enable CAN RX interrupt. - can_enable_irq(CAN1, CAN_IER_FMPIE0); + // Enable CAN RX interrupts. + can_enable_irq(canDev, CAN_IER_FMPIE0); + can_enable_irq(canDev, CAN_IER_FMPIE1); } /** \brief Set baud rate to given value @@ -347,11 +337,11 @@ void Can::SetBaudrate(enum baudrates baudrate) // 1tq sync + 9tq bit segment1 (TS1) + 6tq bit segment2 (TS2) = // 16time quanto per bit period, therefor 4MHz/16 = 250kHz // - can_init(CAN1, + can_init(canDev, false, // TTCM: Time triggered comm mode? true, // ABOM: Automatic bus-off management? false, // AWUM: Automatic wakeup mode? - false, // NART: No automatic retransmission? + true, // NART: No automatic retransmission? false, // RFLM: Receive FIFO locked mode? false, // TXFP: Transmit FIFO priority? CAN_BTR_SJW_1TQ, @@ -381,9 +371,9 @@ uint32_t Can::GetLastRxTimestamp() */ void Can::Send(uint32_t canId, uint32_t data[2]) { - can_disable_irq(CAN1, CAN_IER_TMEIE); + can_disable_irq(canDev, CAN_IER_TMEIE); - if (can_transmit(CAN1, canId, false, false, 8, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN) + if (can_transmit(canDev, canId, false, false, 8, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN) { /* enqueue in send buffer if all TX mailboxes are full */ sendBuffer[sendCnt].id = canId; @@ -394,7 +384,7 @@ void Can::Send(uint32_t canId, uint32_t data[2]) if (sendCnt > 0) { - can_enable_irq(CAN1, CAN_IER_TMEIE); + can_enable_irq(canDev, CAN_IER_TMEIE); } } @@ -416,72 +406,79 @@ void Can::IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, } } -/****************** Private methods and ISRs ********************/ -extern "C" void usb_lp_can_rx0_isr(void) +Can* Can::GetInterface(int index) { - uint32_t id; + if (index < MAX_INTERFACES) + { + return interfaces[index]; + } + return 0; +} + +void Can::HandleRx(int fifo) +{ + uint32_t id; bool ext, rtr; uint8_t length, fmi; uint32_t data[2]; - for (int fifo = 0; fifo < 2; fifo++) + while (can_receive(canDev, fifo, true, &id, &ext, &rtr, &fmi, &length, (uint8_t*)data, NULL) > 0) { - while (can_receive(CAN1, fifo, true, &id, &ext, &rtr, &fmi, &length, (uint8_t*)data, NULL) > 0) + //printf("fifo: %d, id: %x, len: %d, data[0]: %x, data[1]: %x\r\n", fifo, id, length, data[0], data[1]); + if (id == 0x601 && length == 8) //SDO request, nodeid=1 { - //printf("fifo: %d, id: %x, len: %d, data[0]: %x, data[1]: %x\r\n", fifo, id, length, data[0], data[1]); - if (id == 0x601 && length == 8) //SDO request, nodeid=1 - { - ProcessSDO(data); - } - else - { - CANIDMAP *recvMap = FindById(canRecvMap, id); + ProcessSDO(data); + } + else + { + CANIDMAP *recvMap = FindById(canRecvMap, id); - if (0 != recvMap) + if (0 != recvMap) + { + forEachPosMap(curPos, recvMap) { - forEachPosMap(curPos, recvMap) + s32fp val; + + if (curPos->offsetBits > 31) { - s32fp val; - - if (curPos->offsetBits > 31) - { - val = FP_FROMINT((data[1] >> (curPos->offsetBits - 32)) & ((1 << curPos->numBits) - 1)); - } - else - { - val = FP_FROMINT((data[0] >> curPos->offsetBits) & ((1 << curPos->numBits) - 1)); - } - val = FP_MUL(val, curPos->gain); - - if (Param::IsParam((Param::PARAM_NUM)curPos->mapParam)) - Param::Set((Param::PARAM_NUM)curPos->mapParam, val); - else - Param::SetFlt((Param::PARAM_NUM)curPos->mapParam, val); + val = FP_FROMINT((data[1] >> (curPos->offsetBits - 32)) & ((1 << curPos->numBits) - 1)); } - lastRxTimestamp = rtc_get_counter_val(); - } - else //Now it must be a user message, as filters block everything else - { - recvCallback(id, data); + else + { + val = FP_FROMINT((data[0] >> curPos->offsetBits) & ((1 << curPos->numBits) - 1)); + } + val = FP_MUL(val, curPos->gain); + + if (Param::IsParam((Param::PARAM_NUM)curPos->mapParam)) + Param::Set((Param::PARAM_NUM)curPos->mapParam, val); + else + Param::SetFlt((Param::PARAM_NUM)curPos->mapParam, val); } + lastRxTimestamp = rtc_get_counter_val(); + } + else //Now it must be a user message, as filters block everything else + { + recvCallback(id, data); } } } } -extern "C" void usb_hp_can_tx_isr() +void Can::HandleTx() { - while (sendCnt > 0 && can_transmit(CAN1, sendBuffer[sendCnt - 1].id, false, false, 8, (uint8_t*)sendBuffer[sendCnt - 1].data) >= 0) + while (sendCnt > 0 && can_transmit(canDev, sendBuffer[sendCnt - 1].id, false, false, 8, (uint8_t*)sendBuffer[sendCnt - 1].data) >= 0) sendCnt--; if (sendCnt == 0) { - can_disable_irq(CAN1, CAN_IER_TMEIE); + can_disable_irq(canDev, CAN_IER_TMEIE); } } +/****************** Private methods and ISRs ********************/ + //http://www.byteme.org.uk/canopenparent/canopen/sdo-service-data-objects-canopen/ -static void ProcessSDO(uint32_t data[2]) +void Can::ProcessSDO(uint32_t data[2]) { CAN_SDO *sdo = (CAN_SDO*)data; if (sdo->index == 0x2000 && sdo->subIndex < Param::PARAM_LAST) @@ -541,7 +538,7 @@ static void ProcessSDO(uint32_t data[2]) Can::Send(0x581, data); } -static void SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) +void Can::SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) { can_filter_id_list_16bit_init( filterId, @@ -556,11 +553,11 @@ static void SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) idList[0] = idList[1] = idList[2] = idList[3] = 0; } -static void ConfigureFilters() +void Can::ConfigureFilters() { uint16_t idList[IDS_PER_BANK] = { 0x601, 0, 0, 0 }; int idIndex = 1; - int filterId = 0; + int filterId = canDev == CAN1 ? 0 : ((CAN_FMR(CAN2) >> 8) & 0x3F); for (int i = 0; i < nextUserMessageIndex; i++) { @@ -590,7 +587,7 @@ static void ConfigureFilters() } } -static int LoadFromFlash() +int Can::LoadFromFlash() { uint32_t* data = (uint32_t *)CANMAP_ADDRESS; uint32_t storedCrc = *(uint32_t*)CRC_ADDRESS; @@ -610,7 +607,7 @@ static int LoadFromFlash() return 0; } -static int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param) +int Can::RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param) { CANIDMAP copyMap[MAX_MESSAGES]; @@ -622,7 +619,7 @@ static int RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param) return removed; } -static int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) +int Can::Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, int length, s16fp gain) { if (canId > 0x7ff) return CAN_ERR_INVALID_ID; if (offset > 63) return CAN_ERR_INVALID_OFS; @@ -659,7 +656,7 @@ static int Add(CANIDMAP *canMap, Param::PARAM_NUM param, int canId, int offset, return count; } -static void ClearMap(CANIDMAP *canMap) +void Can::ClearMap(CANIDMAP *canMap) { for (int i = 0; i < MAX_MESSAGES; i++) { @@ -672,7 +669,7 @@ static void ClearMap(CANIDMAP *canMap) } } -static CANIDMAP *FindById(CANIDMAP *canMap, int canId) +Can::CANIDMAP* Can::FindById(CANIDMAP *canMap, int canId) { for (int i = 0; i < MAX_MESSAGES; i++) { @@ -682,7 +679,7 @@ static CANIDMAP *FindById(CANIDMAP *canMap, int canId) return 0; } -static uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len) +uint32_t Can::SaveToFlash(uint32_t baseAddress, uint32_t* data, int len) { uint32_t crc = 0; @@ -696,7 +693,7 @@ static uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len) return crc; } -static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param) +int Can::CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param) { int i = 0, removed = 0; @@ -729,7 +726,7 @@ static int CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM pa return removed; } -static void ReplaceParamEnumByUid(CANIDMAP *canMap) +void Can::ReplaceParamEnumByUid(CANIDMAP *canMap) { forEachCanMap(curMap, canMap) { @@ -741,7 +738,7 @@ static void ReplaceParamEnumByUid(CANIDMAP *canMap) } } -static void ReplaceParamUidByEnum(CANIDMAP *canMap) +void Can::ReplaceParamUidByEnum(CANIDMAP *canMap) { forEachCanMap(curMap, canMap) { @@ -752,3 +749,34 @@ static void ReplaceParamUidByEnum(CANIDMAP *canMap) } } } + +/* Interrupt service routines */ +extern "C" void usb_lp_can_rx0_isr(void) +{ + Can::GetInterface(0)->HandleRx(0); +} + +extern "C" void can_rx1_isr() +{ + Can::GetInterface(0)->HandleRx(1); +} + +extern "C" void usb_hp_can_tx_isr() +{ + Can::GetInterface(0)->HandleTx(); +} + +extern "C" void can2_rx0_isr() +{ + Can::GetInterface(1)->HandleRx(0); +} + +extern "C" void can2_rx1_isr() +{ + Can::GetInterface(1)->HandleRx(1); +} + +extern "C" void can2_tx_isr() +{ + Can::GetInterface(1)->HandleTx(); +} From 74c6d5b9f45b53cc4acd489ccaeceb2350b1039c Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 20 Apr 2020 11:16:19 +0200 Subject: [PATCH 11/83] Refactored AnaIn --- include/anain.h | 32 ++++++++++++++++++-------------- src/anain.cpp | 36 ++++++++++++++++++++++-------------- 2 files changed, 40 insertions(+), 28 deletions(-) diff --git a/include/anain.h b/include/anain.h index dd6b978..c6872d4 100644 --- a/include/anain.h +++ b/include/anain.h @@ -8,12 +8,14 @@ class AnaIn { public: - #define ANA_IN_ENTRY(name, port, pin) name, - enum AnaIns - { - ANA_IN_LIST - ANA_IN_COUNT - }; + AnaIn(int chan): firstValue(&values[chan]) {} + + #define ANA_IN_ENTRY(name, port, pin) static AnaIn name; + ANA_IN_LIST + #undef ANA_IN_ENTRY + + #define ANA_IN_ENTRY(name, port, pin) +1 + static const int ANA_IN_COUNT = ANA_IN_LIST; #undef ANA_IN_ENTRY struct AnaInfo @@ -22,21 +24,23 @@ class AnaIn uint16_t pin; }; - static void Init(AnaInfo ins[]); - static uint16_t Get(AnaIn::AnaIns); + static void Start(); + void Configure(uint32_t port, uint8_t pin); + uint16_t Get(); + uint16_t GetIndex() { return firstValue - values; } private: - static const AnaInfo ins[]; static uint16_t values[]; + static uint8_t channel_array[]; static uint8_t AdcChFromPort(uint32_t command_port, int command_bit); static int median3(int a, int b, int c); -}; -#define ANA_IN_ENTRY(name, port, pin) { port, pin }, -/** Usage: AnaIn::AnaInfo analogInputs[] = ANA_IN_ARRAY(ANA_IN_LIST); - * AnaIn::Init(analogInputs); */ -#define ANA_IN_ARRAY(l) { l } + uint16_t* const firstValue; +}; +//Configure all AnaIn objects from the given list +#define ANA_IN_ENTRY(name, port, pin) AnaIn::name.Configure(port, pin); +#define ANA_IN_CONFIGURE(l) l #endif // ANAIO_H_INCLUDED diff --git a/src/anain.cpp b/src/anain.cpp index 183a249..0fabb29 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -27,15 +27,19 @@ #define ADC_DMA_CHAN 1 #define MEDIAN3_FROM_ADC_ARRAY(a) median3(*a, *(a + ANA_IN_COUNT), *(a + 2*ANA_IN_COUNT)) +uint8_t AnaIn::channel_array[ANA_IN_COUNT]; uint16_t AnaIn::values[NUM_SAMPLES*ANA_IN_COUNT]; +#undef ANA_IN_ENTRY +#define ANA_IN_ENTRY(name, port, pin) AnaIn AnaIn::name(__COUNTER__); +ANA_IN_LIST +#undef ANA_IN_ENTRY + /** * Initialize ADC hardware and start DMA based conversion process */ -void AnaIn::Init(AnaInfo ins[]) +void AnaIn::Start() { - uint8_t channel_array[16]; - adc_power_off(ADC1); adc_enable_scan_mode(ADC1); adc_set_continuous_conversion_mode(ADC1); @@ -49,11 +53,11 @@ void AnaIn::Init(AnaInfo ins[]) adc_reset_calibration(ADC1); adc_calibrate(ADC1); - for (int numChan = 0; numChan < ANA_IN_COUNT; numChan++) + /*for (int numChan = 0; numChan < ANA_IN_COUNT; numChan++) { gpio_set_mode(ins[numChan].port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, 1 << ins[numChan].pin); channel_array[numChan] = AdcChFromPort(ins[numChan].port, ins[numChan].pin); - } + }*/ adc_set_regular_sequence(ADC1, ANA_IN_COUNT, channel_array); adc_enable_dma(ADC1); @@ -71,6 +75,12 @@ void AnaIn::Init(AnaInfo ins[]) adc_start_conversion_direct(ADC1); } +void AnaIn::Configure(uint32_t port, uint8_t pin) +{ + gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, 1 << pin); + channel_array[GetIndex()] = AdcChFromPort(port, pin); +} + /** * Get filtered value of given channel * @@ -79,18 +89,16 @@ void AnaIn::Init(AnaInfo ins[]) * - NUM_SAMPLES = 9: Median of last 3 medians is returned * - NUM_SAMPLES = 12: Average of last 4 medians is returned * -* @param[in] in channel index * @return Filtered value */ -uint16_t AnaIn::Get(AnaIn::AnaIns in) +uint16_t AnaIn::Get() { #if NUM_SAMPLES == 1 - return values[in]; + return *firstValue; #elif NUM_SAMPLES == 3 - uint16_t *curVal = &values[in]; - return MEDIAN3_FROM_ADC_ARRAY(curVal); + return MEDIAN3_FROM_ADC_ARRAY(firstValue); #elif NUM_SAMPLES == 9 - uint16_t *curVal = &values[in]; + uint16_t *curVal = firstValue; uint16_t med[3]; for (int i = 0; i < 3; i++, curVal += 3*ANA_IN_COUNT) @@ -98,9 +106,9 @@ uint16_t AnaIn::Get(AnaIn::AnaIns in) med[i] = MEDIAN3_FROM_ADC_ARRAY(curVal); } - return median3(med[0], med[1], med[2]); + return MEDIAN3(med[0], med[1], med[2]); #elif NUM_SAMPLES == 12 - uint16_t *curVal = &values[in]; + uint16_t *curVal = firstValue; uint16_t med[4]; for (int i = 0; i < 4; i++, curVal += 3*ANA_IN_COUNT) @@ -110,7 +118,7 @@ uint16_t AnaIn::Get(AnaIn::AnaIns in) return (med[0] + med[1] + med[2] + med[3]) >> 2; #else - #error NUM_SAMPLES must be 1, 3 or 9 + #error NUM_SAMPLES must be 1, 3, 9 or 12 #endif } From ed0c7234b86ac76158880e2be4802e91d5f7dbe3 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 6 May 2020 16:51:41 +0200 Subject: [PATCH 12/83] oops, CAN NART was still enabled --- src/anain.cpp | 6 ------ src/stm32_can.cpp | 2 +- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/src/anain.cpp b/src/anain.cpp index 0fabb29..3557d64 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -53,12 +53,6 @@ void AnaIn::Start() adc_reset_calibration(ADC1); adc_calibrate(ADC1); - /*for (int numChan = 0; numChan < ANA_IN_COUNT; numChan++) - { - gpio_set_mode(ins[numChan].port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, 1 << ins[numChan].pin); - channel_array[numChan] = AdcChFromPort(ins[numChan].port, ins[numChan].pin); - }*/ - adc_set_regular_sequence(ADC1, ANA_IN_COUNT, channel_array); adc_enable_dma(ADC1); diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index d874857..e159240 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -341,7 +341,7 @@ void Can::SetBaudrate(enum baudrates baudrate) false, // TTCM: Time triggered comm mode? true, // ABOM: Automatic bus-off management? false, // AWUM: Automatic wakeup mode? - true, // NART: No automatic retransmission? + false, // NART: No automatic retransmission? false, // RFLM: Receive FIFO locked mode? false, // TXFP: Transmit FIFO priority? CAN_BTR_SJW_1TQ, From b9751207b4853c244b107c895d5da83919862ae7 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 13 May 2020 10:21:00 +0200 Subject: [PATCH 13/83] make hwRev dependency in terminal optional --- src/terminal.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/terminal.c b/src/terminal.c index a586c1c..430c5b5 100644 --- a/src/terminal.c +++ b/src/terminal.c @@ -113,11 +113,14 @@ int putchar(int c) { static uint32_t curIdx = 0, curBuf = 0, first = 1; +#ifdef UARTDMABLOCKED if (hwRev == HW_REV1) { usart_send_blocking(TERM_USART, c); } - else if (c == '\n' || curIdx == (TERM_BUFSIZE - 1)) + else +#endif + if (c == '\n' || curIdx == (TERM_BUFSIZE - 1)) { outBuf[curBuf][curIdx] = c; From a9e7ff7eccd38596d5d43e73aa865e58b3f3df94 Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 17 Oct 2020 23:11:30 +0200 Subject: [PATCH 14/83] Added memset32 function --- include/my_string.h | 1 + src/my_string.c | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/include/my_string.h b/include/my_string.h index fe24a55..3d4c8e8 100644 --- a/include/my_string.h +++ b/include/my_string.h @@ -37,6 +37,7 @@ int my_ltoa(char *buf, int val, int base); int my_atoi(const char *str); char *my_trim(char *str); void memcpy32(int* target, int *source, int length); +void memset32(int* target, int value, int length); void my_strcpy(char *str1, const char *str2); #ifdef __cplusplus diff --git a/src/my_string.c b/src/my_string.c index 4acb89b..015664e 100644 --- a/src/my_string.c +++ b/src/my_string.c @@ -140,3 +140,9 @@ void memcpy32(int* target, int *source, int length) while (length--) *target++ = *source++; } + +void memset32(int* target, int value, int length) +{ + while (length--) + *target++ = value; +} From 5372bfd159f0c0d5bc48149f2f6c0886431e717b Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 17 Oct 2020 23:11:38 +0200 Subject: [PATCH 15/83] Simplified param_save --- src/param_save.cpp | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/src/param_save.cpp b/src/param_save.cpp index b8cdc73..7fc94b3 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -22,6 +22,7 @@ #include "params.h" #include "param_save.h" #include "hwdefs.h" +#include "my_string.h" #define NUM_PARAMS ((PARAM_BLKSIZE - 8) / sizeof(PARAM_ENTRY)) #define PARAM_WORDS (PARAM_BLKSIZE / 4) @@ -52,30 +53,18 @@ uint32_t parm_save() unsigned int idx; crc_reset(); + memset32((int*)&parmPage, 0xFFFFFFFF, PARAM_WORDS); //Copy parameter values and keys to block structure for (idx = 0; Param::IsParam((Param::PARAM_NUM)idx) && idx < NUM_PARAMS; idx++) { const Param::Attributes *pAtr = Param::GetAttrib((Param::PARAM_NUM)idx); - parmPage.data[idx].dummy = 0; parmPage.data[idx].flags = (uint8_t)Param::GetFlag((Param::PARAM_NUM)idx); parmPage.data[idx].key = pAtr->id; parmPage.data[idx].value = Param::Get((Param::PARAM_NUM)idx); - crc_calculate(((uint32_t*)&parmPage.data[idx])[0]); //Treat 3 fields as uint32_t - crc_calculate(parmPage.data[idx].value); - } - //Pad the remaining space and the CRC calculcator with 1's - for (; idx < NUM_PARAMS; idx++) - { - parmPage.data[idx].dummy = 0xff; - parmPage.data[idx].flags = 0xff; - parmPage.data[idx].key = 0xffff; - parmPage.data[idx].value = 0xffffffff; - crc_calculate(0xffffffff); - parmPage.crc = crc_calculate(0xffffffff); } - parmPage.padding = 0xffffffff; + parmPage.crc = crc_calculate_block(((uint32_t*)&parmPage), (2 * NUM_PARAMS)); flash_unlock(); flash_erase_page(PARAM_ADDRESS); From 72653729f4b4cc1a8d1f08aebdd97fb23083a680 Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 17 Oct 2020 23:11:45 +0200 Subject: [PATCH 16/83] Added boot loader pin init parameter structures --- include/stm32_loader.h | 46 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 include/stm32_loader.h diff --git a/include/stm32_loader.h b/include/stm32_loader.h new file mode 100644 index 0000000..4e75121 --- /dev/null +++ b/include/stm32_loader.h @@ -0,0 +1,46 @@ +#ifndef STM32_LOADER_H_INCLUDED +#define STM32_LOADER_H_INCLUDED + +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2018 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include + +#define PINDEF_ADDRESS 0x0801F400 +#define NUM_PIN_COMMANDS 10 +#define PIN_IN 0 +#define PIN_OUT 1 + +struct pindef +{ + uint32_t port; + uint16_t pin; + uint8_t inout; + uint8_t level; +}; + +struct pincommands +{ + struct pindef pindef[NUM_PIN_COMMANDS]; + uint32_t crc; +}; + +#define PINDEF_NUMWORDS (sizeof(struct pindef) * NUM_PIN_COMMANDS / 4) + + +#endif // STM32_LOADER_H_INCLUDED From a3240daeac24399f8ad4f40ba10d56b01d674b8a Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 12 Nov 2020 20:46:27 +0100 Subject: [PATCH 17/83] Kindof fixed rounding errors --- src/my_fp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/my_fp.c b/src/my_fp.c index ae637b9..b96a28f 100644 --- a/src/my_fp.c +++ b/src/my_fp.c @@ -69,7 +69,7 @@ s32fp fp_atoi(const char *str) { for (str++; *str >= '0' && *str <= '9'; str++) { - frac += FP_FROMINT(*str - '0') / div; + frac += (div / 2 + FP_FROMINT(*str - '0')) / div; div *= 10; } } From 1973ef68ef5731750e1aa36997c9870adad0a29d Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 19 Nov 2020 11:26:39 +0100 Subject: [PATCH 18/83] Added integrator preloading --- include/picontroller.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/picontroller.h b/include/picontroller.h index 69ed5e7..22663c5 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -68,6 +68,11 @@ class PiController /** Reset integrator to 0 */ void ResetIntegrator() { esum = 0; } + /** Preload Integrator to yield a certain output + * @pre SetCallingFrequency() and SetGains() must be called first + */ + void PreloadIntegrator(int32_t yieldedOutput) { esum = (yieldedOutput * frequency) / (ki + 1); } + protected: private: From e0d4412f45ad2a26fd5bf9f5baa28951e339bd0c Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 1 Jan 2021 19:24:16 +0100 Subject: [PATCH 19/83] Allow specifying length in Can::Send() --- include/stm32_can.h | 4 +++- src/stm32_can.cpp | 8 +++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index 90e901e..b76eaa6 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -41,7 +41,8 @@ class Can Can(uint32_t baseAddr, enum baudrates baudrate); void Clear(void); void SetBaudrate(enum baudrates baudrate); - void Send(uint32_t canId, uint32_t data[2]); + void Send(uint32_t canId, uint32_t data[2]) { Send(canId, data, 8); } + void Send(uint32_t canId, uint32_t data[2], uint8_t len); void SendAll(); void Save(); void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)); @@ -79,6 +80,7 @@ class Can struct SENDBUFFER { uint16_t id; + uint32_t len; uint32_t data[2]; }; diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index e159240..5689ead 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -366,17 +366,19 @@ uint32_t Can::GetLastRxTimestamp() * * \param canId uint32_t * \param data[2] uint32_t + * \param len message length * \return void * */ -void Can::Send(uint32_t canId, uint32_t data[2]) +void Can::Send(uint32_t canId, uint32_t data[2], uint8_t len) { can_disable_irq(canDev, CAN_IER_TMEIE); - if (can_transmit(canDev, canId, false, false, 8, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN) + if (can_transmit(canDev, canId, false, false, len, (uint8_t*)data) < 0 && sendCnt < SENDBUFFER_LEN) { /* enqueue in send buffer if all TX mailboxes are full */ sendBuffer[sendCnt].id = canId; + sendBuffer[sendCnt].len = len; sendBuffer[sendCnt].data[0] = data[0]; sendBuffer[sendCnt].data[1] = data[1]; sendCnt++; @@ -466,7 +468,7 @@ void Can::HandleRx(int fifo) void Can::HandleTx() { - while (sendCnt > 0 && can_transmit(canDev, sendBuffer[sendCnt - 1].id, false, false, 8, (uint8_t*)sendBuffer[sendCnt - 1].data) >= 0) + while (sendCnt > 0 && can_transmit(canDev, sendBuffer[sendCnt - 1].id, false, false, sendBuffer[sendCnt - 1].len, (uint8_t*)sendBuffer[sendCnt - 1].data) >= 0) sendCnt--; if (sendCnt == 0) From 9a88ce6bbca315bebdca16326a182f2702b295fd Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 7 Jan 2021 17:52:44 +0100 Subject: [PATCH 20/83] made fp_atoi more generic Added division support to CAN tx --- include/my_fp.h | 2 +- src/my_fp.c | 6 +++--- src/stm32_can.cpp | 7 ++++++- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/include/my_fp.h b/include/my_fp.h index 27ae27b..71fac62 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -35,7 +35,7 @@ extern "C" #endif char* fp_itoa(char * buf, s32fp a); -s32fp fp_atoi(const char *str); +s32fp fp_atoi(const char *str, int fracDigits); u32fp fp_sqrt(u32fp rad); s32fp fp_ln(unsigned int x); diff --git a/src/my_fp.c b/src/my_fp.c index b96a28f..13199dc 100644 --- a/src/my_fp.c +++ b/src/my_fp.c @@ -49,7 +49,7 @@ char* fp_itoa(char * buf, s32fp a) return buf; } -s32fp fp_atoi(const char *str) +s32fp fp_atoi(const char *str, int fracDigits) { int nat = 0; int frac = 0; @@ -69,12 +69,12 @@ s32fp fp_atoi(const char *str) { for (str++; *str >= '0' && *str <= '9'; str++) { - frac += (div / 2 + FP_FROMINT(*str - '0')) / div; + frac += (div / 2 + ((*str - '0') << fracDigits)) / div; div *= 10; } } - return sign * (FP_FROMINT(nat) + frac); + return sign * ((nat << fracDigits) + frac); } u32fp fp_sqrt(u32fp rad) diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index e159240..5847cce 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -220,7 +220,12 @@ void Can::SendAll() forEachPosMap(curPos, curMap) { - s32fp val = FP_MUL(Param::Get((Param::PARAM_NUM)curPos->mapParam), curPos->gain); + s32fp val = Param::Get((Param::PARAM_NUM)curPos->mapParam); + + if (curPos->gain <= 32 && curPos->gain >= -32) + val = FP_MUL(val, curPos->gain); + else + val /= curPos->gain; val &= ((1 << curPos->numBits) - 1); From 216da9a441efcce59803fde583621002727383d3 Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 22 Jan 2021 12:14:44 +0100 Subject: [PATCH 21/83] Added class with standard terminal commands --- include/terminalcommands.h | 43 ++++ src/{terminal.c => terminal.cpp} | 3 +- src/terminalcommands.cpp | 429 +++++++++++++++++++++++++++++++ 3 files changed, 473 insertions(+), 2 deletions(-) create mode 100644 include/terminalcommands.h rename src/{terminal.c => terminal.cpp} (99%) create mode 100644 src/terminalcommands.cpp diff --git a/include/terminalcommands.h b/include/terminalcommands.h new file mode 100644 index 0000000..1556609 --- /dev/null +++ b/include/terminalcommands.h @@ -0,0 +1,43 @@ +/* + * This file is part of the tumanako_vc project. + * + * Copyright (C) 2021 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef TERMINALCOMMANDS_H +#define TERMINALCOMMANDS_H + + +class TerminalCommands +{ + public: + static void ParamSet(char* arg); + static void ParamGet(char *arg); + static void ParamFlag(char *arg); + static void ParamStream(char *arg); + static void PrintParamsJson(char *arg); + static void MapCan(char *arg); + static void SaveParameters(char *arg); + static void LoadParameters(char *arg); + static void Reset(char *arg); + static void FastUart(char *arg); + + protected: + + private: + static void PrintCanMap(Param::PARAM_NUM param, int canid, int offset, int length, s32fp gain, bool rx); +}; + +#endif // TERMINALCOMMANDS_H diff --git a/src/terminal.c b/src/terminal.cpp similarity index 99% rename from src/terminal.c rename to src/terminal.cpp index 430c5b5..8ffd480 100644 --- a/src/terminal.c +++ b/src/terminal.cpp @@ -24,7 +24,6 @@ #include #include "terminal.h" #include "hwdefs.h" -#include static const TERM_CMD *CmdLookup(char *buf); static void term_send(uint32_t usart, const char *str); @@ -109,7 +108,7 @@ void term_Run() * buffering, so while one buffer is sent by DMA we can prepare the other * buffer to go next. */ -int putchar(int c) +extern "C" int putchar(int c) { static uint32_t curIdx = 0, curBuf = 0, first = 1; diff --git a/src/terminalcommands.cpp b/src/terminalcommands.cpp new file mode 100644 index 0000000..c6e6a0f --- /dev/null +++ b/src/terminalcommands.cpp @@ -0,0 +1,429 @@ +/* + * This file is part of the stm32-... project. + * + * Copyright (C) 2021 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include +#include "hwdefs.h" +#include "terminal.h" +#include "params.h" +#include "my_string.h" +#include "my_fp.h" +#include "printf.h" +#include "param_save.h" +#include "stm32_can.h" +#include "terminalcommands.h" + +void TerminalCommands::ParamSet(char* arg) +{ + char *pParamVal; + s32fp val; + Param::PARAM_NUM idx; + + arg = my_trim(arg); + pParamVal = (char *)my_strchr(arg, ' '); + + if (*pParamVal == 0) + { + printf("No parameter value given\r\n"); + return; + } + + *pParamVal = 0; + pParamVal++; + + val = fp_atoi(pParamVal, FRAC_DIGITS); + idx = Param::NumFromString(arg); + + if (Param::PARAM_INVALID != idx) + { + if (0 == Param::Set(idx, val)) + { + printf("Set OK\r\n"); + } + else + { + printf("Value out of range\r\n"); + } + } + else + { + printf("Unknown parameter %s\r\n", arg); + } +} + +void TerminalCommands::ParamGet(char* arg) +{ + Param::PARAM_NUM idx; + s32fp val; + char* comma; + char orig; + + arg = my_trim(arg); + + do + { + comma = (char*)my_strchr(arg, ','); + orig = *comma; + *comma = 0; + + idx = Param::NumFromString(arg); + + if (Param::PARAM_INVALID != idx) + { + val = Param::Get(idx); + printf("%f\r\n", val); + } + else + { + printf("Unknown parameter: '%s'\r\n", arg); + } + + *comma = orig; + arg = comma + 1; + } while (',' == *comma); +} + +void TerminalCommands::ParamFlag(char *arg) +{ + char *pFlagVal; + Param::PARAM_NUM idx; + + arg = my_trim(arg); + pFlagVal = (char *)my_strchr(arg, ' '); + + if (*pFlagVal == 0) + { + printf("No flag given\r\n"); + return; + } + + *pFlagVal = 0; + pFlagVal++; + + idx = Param::NumFromString(arg); + + if (Param::PARAM_INVALID != idx) + { + bool clearFlag = false; + Param::PARAM_FLAG flag = Param::FLAG_NONE; + + if (pFlagVal[0] == '!' || pFlagVal[0] == '~' || pFlagVal[0] == '/') + { + clearFlag = true; + pFlagVal++; + } + + if (my_strcmp("hidden", pFlagVal) == 0) + { + flag = Param::FLAG_HIDDEN; + } + + if (flag != Param::FLAG_NONE) + { + if (clearFlag) + { + Param::ClearFlag(idx, flag); + } + else + { + Param::SetFlag(idx, flag); + } + printf("Flag change OK\r\n"); + } + else + { + printf("Unknown flag\r\n"); + } + } + else + { + printf("Unknown parameter %s\r\n", arg); + } +} + +void TerminalCommands::ParamStream(char *arg) +{ + Param::PARAM_NUM indexes[10]; + int maxIndex = sizeof(indexes) / sizeof(Param::PARAM_NUM); + int curIndex = 0; + int repetitions = -1; + char* comma; + char orig; + + arg = my_trim(arg); + repetitions = my_atoi(arg); + arg = (char*)my_strchr(arg, ' '); + + if (0 == *arg) + { + printf("Usage: stream n val1,val2...\r\n"); + return; + } + arg++; //move behind space + + do + { + comma = (char*)my_strchr(arg, ','); + orig = *comma; + *comma = 0; + + Param::PARAM_NUM idx = Param::NumFromString(arg); + + *comma = orig; + arg = comma + 1; + + if (Param::PARAM_INVALID != idx) + { + indexes[curIndex] = idx; + curIndex++; + } + else + { + printf("Unknown parameter\r\n"); + } + } while (',' == *comma && curIndex < maxIndex); + + maxIndex = curIndex; + usart_recv(TERM_USART); + + while (!usart_get_flag(TERM_USART, USART_SR_RXNE) && (repetitions > 0 || repetitions == -1)) + { + comma = (char*)""; + for (curIndex = 0; curIndex < maxIndex; curIndex++) + { + s32fp val = Param::Get(indexes[curIndex]); + printf("%s%f", comma, val); + comma = (char*)","; + } + printf("\r\n"); + if (repetitions != -1) + repetitions--; + } +} + +void TerminalCommands::PrintParamsJson(char *arg) +{ + arg = my_trim(arg); + + const Param::Attributes *pAtr; + char comma = ' '; + bool printHidden = arg[0] == 'h'; + + printf("{"); + for (uint32_t idx = 0; idx < Param::PARAM_LAST; idx++) + { + int canId, canOffset, canLength; + bool isRx; + s32fp canGain; + pAtr = Param::GetAttrib((Param::PARAM_NUM)idx); + + if ((Param::GetFlag((Param::PARAM_NUM)idx) & Param::FLAG_HIDDEN) == 0 || printHidden) + { + printf("%c\r\n \"%s\": {\"unit\":\"%s\",\"value\":%f,",comma, pAtr->name, pAtr->unit, Param::Get((Param::PARAM_NUM)idx)); + + if (Can::GetInterface(0)->FindMap((Param::PARAM_NUM)idx, canId, canOffset, canLength, canGain, isRx)) + { + printf("\"canid\":%d,\"canoffset\":%d,\"canlength\":%d,\"cangain\":%d,\"isrx\":%s,", + canId, canOffset, canLength, canGain, isRx ? "true" : "false"); + } + + if (Param::IsParam((Param::PARAM_NUM)idx)) + { + printf("\"isparam\":true,\"minimum\":%f,\"maximum\":%f,\"default\":%f,\"category\":\"%s\",\"i\":%d}", + pAtr->min, pAtr->max, pAtr->def, pAtr->category, idx); + } + else + { + printf("\"isparam\":false}"); + } + comma = ','; + } + } + printf("\r\n}\r\n"); +} + +//cantx param id offset len gain +void TerminalCommands::MapCan(char *arg) +{ + Param::PARAM_NUM paramIdx = Param::PARAM_INVALID; + int values[4]; + int result; + char op; + char *ending; + const int numArgs = 4; + + arg = my_trim(arg); + + if (arg[0] == 'p') + { + Can::GetInterface(0)->IterateCanMap(PrintCanMap); + return; + } + + if (arg[0] == 'c') + { + Can::GetInterface(0)->Clear(); + printf("All message definitions cleared\r\n"); + return; + } + + op = arg[0]; + arg = (char *)my_strchr(arg, ' '); + + if (0 == *arg) + { + printf("Missing argument\r\n"); + return; + } + + arg = my_trim(arg); + ending = (char *)my_strchr(arg, ' '); + + if (*ending == 0 && op != 'd') + { + printf("Missing argument\r\n"); + return; + } + + *ending = 0; + paramIdx = Param::NumFromString(arg); + arg = my_trim(ending + 1); + + if (Param::PARAM_INVALID == paramIdx) + { + printf("Unknown parameter\r\n"); + return; + } + + if (op == 'd') + { + result = Can::GetInterface(0)->Remove(paramIdx); + printf("%d entries removed\r\n", result); + return; + } + + for (int i = 0; i < numArgs; i++) + { + ending = (char *)my_strchr(arg, ' '); + + if (0 == *ending && i < (numArgs - 1)) + { + printf("Missing argument\r\n"); + return; + } + + *ending = 0; + int iVal = my_atoi(arg); + + //allow gain values < 1 and re-interpret them + if (i == (numArgs - 1) && iVal == 0) + { + values[i] = fp_atoi(arg, 16); + //The can values interprets abs(values) < 32 as gain and > 32 as divider + //e.g. 0.25 means integer division by 4 so we need to calculate div = 1/value + //0.25 with 16 decimals is 16384, 65536/16384 = 4 + values[i] = (32 << 16) / values[i]; + } + else + { + values[i] = iVal; + } + + arg = my_trim(ending + 1); + } + + if (op == 't') + { + result = Can::GetInterface(0)->AddSend(paramIdx, values[0], values[1], values[2], values[3]); + } + else + { + result = Can::GetInterface(0)->AddRecv(paramIdx, values[0], values[1], values[2], values[3]); + } + + switch (result) + { + case CAN_ERR_INVALID_ID: + printf("Invalid CAN Id %x\r\n", values[0]); + break; + case CAN_ERR_INVALID_OFS: + printf("Invalid Offset %d\r\n", values[1]); + break; + case CAN_ERR_INVALID_LEN: + printf("Invalid length %d\r\n", values[2]); + break; + case CAN_ERR_MAXITEMS: + printf("Cannot map anymore items to CAN id %d\r\n", values[0]); + break; + case CAN_ERR_MAXMESSAGES: + printf("Max message count reached\r\n"); + break; + default: + printf("CAN map successful, %d message%s active\r\n", result, result > 1 ? "s" : ""); + } +} + +void TerminalCommands::SaveParameters(char *arg) +{ + arg = arg; + uint32_t crc = parm_save(); + printf("Parameters stored, CRC=%x\r\n", crc); + Can::GetInterface(0)->Save(); + printf("CANMAP stored\r\n"); +} + +void TerminalCommands::LoadParameters(char *arg) +{ + arg = arg; + if (0 == parm_load()) + { + parm_Change((Param::PARAM_NUM)0); + printf("Parameters loaded\r\n"); + } + else + { + printf("Parameter CRC error\r\n"); + } +} + +void TerminalCommands::Reset(char *arg) +{ + arg = arg; + scb_reset_system(); +} + +void TerminalCommands::FastUart(char *arg) +{ + arg = my_trim(arg); + int baud = arg[0] == '0' ? USART_BAUDRATE : 921600; + printf("OK\r\n"); + printf("Baud rate now %d\r\n", baud); + usart_set_baudrate(TERM_USART, baud); +} + +void TerminalCommands::PrintCanMap(Param::PARAM_NUM param, int canid, int offset, int length, s32fp gain, bool rx) +{ + const char* name = Param::GetAttrib(param)->name; + printf("can "); + + if (rx) + printf("rx "); + else + printf("tx "); + printf("%s %d %d %d %d\r\n", name, canid, offset, length, gain); +} From 1787ba725bdfa5f79c618504bc7364b5f77143cc Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 8 Feb 2021 09:29:14 +0100 Subject: [PATCH 22/83] Refactored terminal, fixed headers --- include/anain.h | 18 +++ include/digio.h | 18 +++ include/errormessage.h | 2 +- include/foc.h | 18 +++ include/fu.h | 20 ++- include/my_fp.h | 18 +++ include/my_math.h | 18 +++ include/my_string.h | 5 +- include/param_save.h | 18 +++ include/params.h | 3 +- include/picontroller.h | 2 +- include/printf.h | 19 ++- include/sine_core.h | 7 +- include/stm32_can.h | 5 +- include/stm32_loader.h | 7 +- include/stm32scheduler.h | 4 +- include/terminal.h | 63 ++++++-- include/terminalcommands.h | 21 ++- src/anain.cpp | 2 +- src/digio.cpp | 2 +- src/errormessage.cpp | 2 +- src/foc.cpp | 2 +- src/fu.cpp | 27 +++- src/my_fp.c | 2 +- src/my_string.c | 2 +- src/param_save.cpp | 2 +- src/params.cpp | 2 +- src/picontroller.cpp | 2 +- src/{printf.c => printf.cpp} | 160 ++++++++------------- src/sine_core.cpp | 12 +- src/stm32_can.cpp | 36 ++++- src/stm32scheduler.cpp | 2 +- src/terminal.cpp | 269 ++++++++++++++++++++++++----------- src/terminalcommands.cpp | 119 ++++++++-------- 34 files changed, 580 insertions(+), 329 deletions(-) rename src/{printf.c => printf.cpp} (51%) diff --git a/include/anain.h b/include/anain.h index c6872d4..3c71ca3 100644 --- a/include/anain.h +++ b/include/anain.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef ANAIO_H_INCLUDED #define ANAIO_H_INCLUDED diff --git a/include/digio.h b/include/digio.h index eccdf5b..2b56ab8 100644 --- a/include/digio.h +++ b/include/digio.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef DIGIO_H_INCLUDED #define DIGIO_H_INCLUDED diff --git a/include/errormessage.h b/include/errormessage.h index 45ced99..b9f0560 100644 --- a/include/errormessage.h +++ b/include/errormessage.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/include/foc.h b/include/foc.h index 36033c0..5605911 100644 --- a/include/foc.h +++ b/include/foc.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef FOC_H #define FOC_H diff --git a/include/fu.h b/include/fu.h index 08aa31e..be93d4e 100644 --- a/include/fu.h +++ b/include/fu.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef FU_H_INCLUDED #define FU_H_INCLUDED @@ -10,7 +28,6 @@ class MotorVoltage static void SetBoost(uint32_t boost); static void SetWeakeningFrq(u32fp frq); static void SetMaxAmp(uint32_t maxAmp); - static void SetMinFrq(u32fp frq); static uint32_t GetAmp(u32fp frq); static uint32_t GetAmpPerc(u32fp frq, u32fp perc); @@ -20,7 +37,6 @@ class MotorVoltage static u32fp fac; static uint32_t maxAmp; static u32fp endFrq; - static u32fp minFrq; static u32fp maxFrq; }; diff --git a/include/my_fp.h b/include/my_fp.h index 71fac62..3d63e55 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef MY_FP_H_INCLUDED #define MY_FP_H_INCLUDED diff --git a/include/my_math.h b/include/my_math.h index c753b90..b419917 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef MY_MATH_H_INCLUDED #define MY_MATH_H_INCLUDED diff --git a/include/my_string.h b/include/my_string.h index 3d4c8e8..3de5189 100644 --- a/include/my_string.h +++ b/include/my_string.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * @@ -16,6 +16,8 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ +#ifndef MY_STRING_H +#define MY_STRING_H #ifndef NULL #define NULL 0L @@ -43,3 +45,4 @@ void my_strcpy(char *str1, const char *str2); #ifdef __cplusplus } #endif +#endif // MY_STRING_H diff --git a/include/param_save.h b/include/param_save.h index 733ce71..3f89e35 100644 --- a/include/param_save.h +++ b/include/param_save.h @@ -1,3 +1,21 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2011 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #ifndef PARAM_SAVE_H_INCLUDED #define PARAM_SAVE_H_INCLUDED diff --git a/include/params.h b/include/params.h index 03f55be..073fd57 100644 --- a/include/params.h +++ b/include/params.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * @@ -16,6 +16,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ + #ifndef PARAM_H_INCLUDED #define PARAM_H_INCLUDED diff --git a/include/picontroller.h b/include/picontroller.h index 22663c5..01afadc 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2018 Johannes Huebner * diff --git a/include/printf.h b/include/printf.h index ff5e5cd..f4d1e1f 100644 --- a/include/printf.h +++ b/include/printf.h @@ -1,9 +1,7 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * - * Copyright (C) 2010 Johannes Huebner - * Copyright (C) 2010 Edward Cheeseman - * Copyright (C) 2009 Uwe Hermann + * Copyright (C) 2011 Johannes Huebner * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -27,17 +25,16 @@ #define debugf(...) #endif -#ifdef __cplusplus -extern "C" +class IPutChar { -#endif +public: + virtual void PutChar(char c) = 0; +}; + int printf(const char *format, ...); int sprintf(char *out, const char *format, ...); - -#ifdef __cplusplus -} -#endif +int fprintf(IPutChar* put, const char *format, ...); #endif // PRINTF_H_INCLUDED diff --git a/include/sine_core.h b/include/sine_core.h index a71bd30..55f9c20 100644 --- a/include/sine_core.h +++ b/include/sine_core.h @@ -1,7 +1,7 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * - * Copyright (C) 2012 Johannes Huebner + * Copyright (C) 2011 Johannes Huebner * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,7 +30,6 @@ class SineCore static uint16_t Atan2(int32_t cos, int32_t sin); static void SetAmp(uint32_t amp); static uint32_t GetAmp(); - static void SetMinPulseWidth(uint32_t minWidth); static int32_t CalcSVPWMOffset(int32_t a, int32_t b, int32_t c); static uint32_t DutyCycles[3]; static uint32_t Offset; @@ -44,7 +43,7 @@ class SineCore static int32_t max(int32_t a, int32_t b); /** Minimum pulse width in normalized digits */ - static uint32_t minPulse; + static const uint32_t minPulse; static uint32_t ampl; static const int16_t SinTab[]; /* sine LUT */ static const uint16_t ZERO_OFFSET; diff --git a/include/stm32_can.h b/include/stm32_can.h index b76eaa6..44adac4 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2016 Nail Güzel * Johannes Huebner @@ -44,6 +44,7 @@ class Can void Send(uint32_t canId, uint32_t data[2]) { Send(canId, data, 8); } void Send(uint32_t canId, uint32_t data[2], uint8_t len); void SendAll(); + void SDOWrite(uint8_t remoteNodeId, uint16_t index, uint8_t subIndex, uint32_t data); void Save(); void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*)); bool RegisterUserMessage(int canId); @@ -55,6 +56,7 @@ class Can void IterateCanMap(void (*callback)(Param::PARAM_NUM, int, int, int, s32fp, bool)); void HandleRx(int fifo); void HandleTx(); + void SetNodeId(uint8_t id) { nodeId = id; } static Can* GetInterface(int index); private: @@ -93,6 +95,7 @@ class Can uint16_t userIds[MAX_USER_MESSAGES]; int nextUserMessageIndex; uint32_t canDev; + uint8_t nodeId; void ProcessSDO(uint32_t data[2]); void ClearMap(CANIDMAP *canMap); diff --git a/include/stm32_loader.h b/include/stm32_loader.h index 4e75121..93ee479 100644 --- a/include/stm32_loader.h +++ b/include/stm32_loader.h @@ -1,8 +1,5 @@ -#ifndef STM32_LOADER_H_INCLUDED -#define STM32_LOADER_H_INCLUDED - /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2018 Johannes Huebner * @@ -19,6 +16,8 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ +#ifndef STM32_LOADER_H_INCLUDED +#define STM32_LOADER_H_INCLUDED #include #define PINDEF_ADDRESS 0x0801F400 diff --git a/include/stm32scheduler.h b/include/stm32scheduler.h index aa255e4..fcc727d 100644 --- a/include/stm32scheduler.h +++ b/include/stm32scheduler.h @@ -1,7 +1,7 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * - * Copyright (C) 2017 Johannes Huebner + * Copyright (C) 2017 Johannes Huebner * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by diff --git a/include/terminal.h b/include/terminal.h index 5fb8176..a2fbb7f 100644 --- a/include/terminal.h +++ b/include/terminal.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * @@ -16,23 +16,64 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ +#ifndef TERMINAL_H +#define TERMINAL_H #include +#include "printf.h" + +class Terminal; typedef struct { char const *cmd; - void (*CmdFunc)(char*); + void (*CmdFunc)(Terminal*, char*); } TERM_CMD; -#ifdef __cplusplus -extern "C" +class Terminal: public IPutChar { -#endif +public: + Terminal(uint32_t usart, const TERM_CMD* commands, bool remap = false); + void SetNodeId(uint8_t id) { nodeId = id; } + void Run(); + void PutChar(char c); + bool KeyPressed(); + void FlushInput(); + static Terminal* defaultTerminal; + +private: + struct HwInfo + { + uint32_t usart; + uint8_t dmatx; + uint8_t dmarx; + uint32_t port; + uint16_t pin; + uint32_t port_re; + uint16_t pin_re; + }; + + void ResetDMA(); + const TERM_CMD *CmdLookup(char *buf); + void EnableUart(char* arg); + void FastUart(char* arg); + void Send(const char *str); -void term_Init(); -void term_Run(); -void term_Send(char *str); + static const int bufSize = 128; + static const HwInfo hwInfo[]; + const HwInfo* hw; + uint32_t usart; + bool remap; + const TERM_CMD* termCmds; + uint8_t nodeId; + bool enabled; + const TERM_CMD *pCurCmd; + int lastIdx; + uint8_t curBuf; + uint32_t curIdx; + bool firstSend; + char inBuf[bufSize]; + char outBuf[2][bufSize]; //double buffering + char args[bufSize]; +}; -#ifdef __cplusplus -} -#endif +#endif // TERMINAL_H diff --git a/include/terminalcommands.h b/include/terminalcommands.h index 1556609..aa544eb 100644 --- a/include/terminalcommands.h +++ b/include/terminalcommands.h @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2021 Johannes Huebner * @@ -23,16 +23,15 @@ class TerminalCommands { public: - static void ParamSet(char* arg); - static void ParamGet(char *arg); - static void ParamFlag(char *arg); - static void ParamStream(char *arg); - static void PrintParamsJson(char *arg); - static void MapCan(char *arg); - static void SaveParameters(char *arg); - static void LoadParameters(char *arg); - static void Reset(char *arg); - static void FastUart(char *arg); + static void ParamSet(Terminal* term, char* arg); + static void ParamGet(Terminal* term, char *arg); + static void ParamFlag(Terminal* term, char *arg); + static void ParamStream(Terminal* term, char *arg); + static void PrintParamsJson(Terminal* term, char *arg); + static void MapCan(Terminal* term, char *arg); + static void SaveParameters(Terminal* term, char *arg); + static void LoadParameters(Terminal* term, char *arg); + static void Reset(Terminal* term, char *arg); protected: diff --git a/src/anain.cpp b/src/anain.cpp index 3557d64..7e45bd0 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2010 Johannes Huebner * Copyright (C) 2010 Edward Cheeseman diff --git a/src/digio.cpp b/src/digio.cpp index 714f59b..5d01fd1 100644 --- a/src/digio.cpp +++ b/src/digio.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2010 Johannes Huebner * Copyright (C) 2010 Edward Cheeseman diff --git a/src/errormessage.cpp b/src/errormessage.cpp index 4f845c2..313fd72 100644 --- a/src/errormessage.cpp +++ b/src/errormessage.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/foc.cpp b/src/foc.cpp index cb74d75..115c49c 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/fu.cpp b/src/fu.cpp index 463645f..525aab6 100644 --- a/src/fu.cpp +++ b/src/fu.cpp @@ -1,10 +1,28 @@ +/* + * This file is part of the libopeninv project. + * + * Copyright (C) 2016 Nail Güzel + * Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ #include "fu.h" uint32_t MotorVoltage::boost = 0; u32fp MotorVoltage::fac; uint32_t MotorVoltage::maxAmp; u32fp MotorVoltage::endFrq = 1; //avoid division by 0 when not set -u32fp MotorVoltage::minFrq; /** Set 0 Hz boost to overcome winding resistance */ void MotorVoltage::SetBoost(uint32_t boost /**< amplitude in digit */) @@ -30,7 +48,7 @@ uint32_t MotorVoltage::GetAmp(u32fp frq) uint32_t MotorVoltage::GetAmpPerc(u32fp frq, u32fp perc) { uint32_t amp = FP_MUL(perc, (FP_TOINT(FP_MUL(fac, frq)) + boost)) / 100; - if (frq < minFrq) + if (frq < FP_FROMFLT(0.2)) { amp = 0; } @@ -48,11 +66,6 @@ void MotorVoltage::SetMaxAmp(uint32_t maxAmp) CalcFac(); } -void MotorVoltage::SetMinFrq(u32fp frq) -{ - minFrq = frq; -} - /** Calculate slope of u/f */ void MotorVoltage::CalcFac() { diff --git a/src/my_fp.c b/src/my_fp.c index 13199dc..5a6ba0d 100644 --- a/src/my_fp.c +++ b/src/my_fp.c @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/my_string.c b/src/my_string.c index 015664e..465b741 100644 --- a/src/my_string.c +++ b/src/my_string.c @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the slibopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/param_save.cpp b/src/param_save.cpp index 7fc94b3..aa34489 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/params.cpp b/src/params.cpp index afaf3ba..7d3e015 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * diff --git a/src/picontroller.cpp b/src/picontroller.cpp index 394badf..112297e 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2018 Johannes Huebner * diff --git a/src/printf.c b/src/printf.cpp similarity index 51% rename from src/printf.c rename to src/printf.cpp index 9316a57..6667990 100644 --- a/src/printf.c +++ b/src/printf.cpp @@ -27,23 +27,34 @@ */ #include +#include "printf.h" #include "my_fp.h" -static void printchar(char **str, int c) -{ - extern int putchar(int c); - - if (str) { - **str = c; - ++(*str); - } - else (void)putchar(c); -} - #define PAD_RIGHT 1 #define PAD_ZERO 2 -static int prints(char **out, const char *string, int width, int pad) +extern "C" void putchar(char c); + +class ExternPutChar: public IPutChar +{ +public: + void PutChar(char c) + { + putchar(c); + } +}; + +class StringPutChar: public IPutChar +{ +public: + StringPutChar(char *s) : s(s) {} + void PutChar(char c) { *(s++) = c; } + +private: + char *s; +}; + +static int prints(IPutChar* put, const char *string, int width, int pad) { register int pc = 0, padchar = ' '; @@ -57,16 +68,16 @@ static int prints(char **out, const char *string, int width, int pad) } if (!(pad & PAD_RIGHT)) { for ( ; width > 0; --width) { - printchar (out, padchar); + put->PutChar(padchar); ++pc; } } for ( ; *string ; ++string) { - printchar (out, *string); + put->PutChar(*string); ++pc; } for ( ; width > 0; --width) { - printchar (out, padchar); + put->PutChar(padchar); ++pc; } @@ -76,7 +87,7 @@ static int prints(char **out, const char *string, int width, int pad) /* the following should be enough for 32 bit int */ #define PRINT_BUF_LEN 12 -static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +static int printi(IPutChar* put, int i, int b, int sg, int width, int pad, int letbase) { char print_buf[PRINT_BUF_LEN]; register char *s; @@ -86,7 +97,7 @@ static int printi(char **out, int i, int b, int sg, int width, int pad, int letb if (i == 0) { print_buf[0] = '0'; print_buf[1] = '\0'; - return prints (out, print_buf, width, pad); + return prints (put, print_buf, width, pad); } if (sg && b == 10 && i < 0) { @@ -107,7 +118,7 @@ static int printi(char **out, int i, int b, int sg, int width, int pad, int letb if (neg) { if( width && (pad & PAD_ZERO) ) { - printchar (out, '-'); + put->PutChar('-'); ++pc; --width; } @@ -116,19 +127,19 @@ static int printi(char **out, int i, int b, int sg, int width, int pad, int letb } } - return pc + prints (out, s, width, pad); + return pc + prints (put, s, width, pad); } -static int printfp(char **out, int i, int width, int pad) +static int printfp(IPutChar* put, int i, int width, int pad) { char print_buf[PRINT_BUF_LEN]; fp_itoa(print_buf, i); - return prints (out, print_buf, width, pad); + return prints (put, print_buf, width, pad); } -static int print(char **out, const char *format, va_list args ) +static int print(IPutChar* put, const char *format, va_list args ) { register int width, pad; register int pc = 0; @@ -154,128 +165,75 @@ static int print(char **out, const char *format, va_list args ) } if( *format == 's' ) { register char *s = (char *)va_arg( args, int ); - pc += prints (out, s?s:"(null)", width, pad); + pc += prints (put, s?s:"(null)", width, pad); continue; } if( *format == 'd' ) { - pc += printi (out, va_arg( args, int ), 10, 1, width, pad, 'a'); + pc += printi (put, va_arg( args, int ), 10, 1, width, pad, 'a'); continue; } if( *format == 'x' ) { - pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'a'); + pc += printi (put, va_arg( args, int ), 16, 0, width, pad, 'a'); continue; } if( *format == 'X' ) { - pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'A'); + pc += printi (put, va_arg( args, int ), 16, 0, width, pad, 'A'); continue; } if( *format == 'u' ) { - pc += printi (out, va_arg( args, int ), 10, 0, width, pad, 'a'); + pc += printi (put, va_arg( args, int ), 10, 0, width, pad, 'a'); continue; } if ( *format == 'f' ) { - pc += printfp (out, va_arg( args, int ), width, pad); + pc += printfp (put, va_arg( args, int ), width, pad); continue; } if( *format == 'c' ) { /* char are converted to int then pushed on the stack */ scr[0] = (char)va_arg( args, int ); scr[1] = '\0'; - pc += prints (out, scr, width, pad); + pc += prints (put, scr, width, pad); continue; } } else { out: - printchar (out, *format); + put->PutChar(*format); ++pc; } } - if (out) **out = '\0'; va_end( args ); return pc; } int printf(const char *format, ...) { - va_list args; + ExternPutChar pc; + va_list args; - va_start( args, format ); - return print( 0, format, args ); + va_start( args, format ); + return print( &pc, format, args ); } int sprintf(char *out, const char *format, ...) { - va_list args; + StringPutChar pc(out); + va_list args; + + va_start( args, format ); + + int ret = print( &pc, format, args ); - va_start( args, format ); - return print( &out, format, args ); + pc.PutChar(0); + + return ret; } -#ifdef TEST_PRINTF -int main(void) +int fprintf(IPutChar* put, const char *format, ...) { - char *ptr = "Hello world!"; - char *np = 0; - int i = 5; - unsigned int bs = sizeof(int)*8; - int mi; - char buf[80]; - - mi = (1 << (bs-1)) + 1; - printf("%s\n", ptr); - printf("printf test\n"); - printf("%s is null pointer\n", np); - printf("%d = 5\n", i); - printf("%d = - max int\n", mi); - printf("char %c = 'a'\n", 'a'); - printf("hex %x = ff\n", 0xff); - printf("hex %02x = 00\n", 0); - printf("signed %d = unsigned %u = hex %x\n", -3, -3, -3); - printf("%d %s(s)%", 0, "message"); - printf("\n"); - printf("%d %s(s) with %%\n", 0, "message"); - sprintf(buf, "justif: \"%-10s\"\n", "left"); printf("%s", buf); - sprintf(buf, "justif: \"%10s\"\n", "right"); printf("%s", buf); - sprintf(buf, " 3: %04d zero padded\n", 3); printf("%s", buf); - sprintf(buf, " 3: %-4d left justif.\n", 3); printf("%s", buf); - sprintf(buf, " 3: %4d right justif.\n", 3); printf("%s", buf); - sprintf(buf, "-3: %04d zero padded\n", -3); printf("%s", buf); - sprintf(buf, "-3: %-4d left justif.\n", -3); printf("%s", buf); - sprintf(buf, "-3: %4d right justif.\n", -3); printf("%s", buf); - - return 0; -} + va_list args; -/* - * if you compile this file with - * gcc -Wall $(YOUR_C_OPTIONS) -DTEST_PRINTF -c printf.c - * you will get a normal warning: - * printf.c:214: warning: spurious trailing `%' in format - * this line is testing an invalid % at the end of the format string. - * - * this should display (on 32bit int machine) : - * - * Hello world! - * printf test - * (null) is null pointer - * 5 = 5 - * -2147483647 = - max int - * char a = 'a' - * hex ff = ff - * hex 00 = 00 - * signed -3 = unsigned 4294967293 = hex fffffffd - * 0 message(s) - * 0 message(s) with % - * justif: "left " - * justif: " right" - * 3: 0003 zero padded - * 3: 3 left justif. - * 3: 3 right justif. - * -3: -003 zero padded - * -3: -3 left justif. - * -3: -3 right justif. - */ - -#endif + va_start( args, format ); + return print( put, format, args ); +} diff --git a/src/sine_core.cpp b/src/sine_core.cpp index 2ed315d..46a4cb4 100644 --- a/src/sine_core.cpp +++ b/src/sine_core.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2012 Johannes Huebner * @@ -31,7 +31,7 @@ #define PHASE_SHIFT120 ((uint32_t)( SINLU_ONEREV / 3)) #define PHASE_SHIFT240 ((uint32_t)(2 * (SINLU_ONEREV / 3))) -uint32_t SineCore::minPulse = 0; +const uint32_t SineCore::minPulse = 1000; uint32_t SineCore::ampl = 0; const int16_t SineCore::SinTab[] = { SINTAB };/* sine LUT */ const uint16_t SineCore::ZERO_OFFSET = SINTAB_MAX / 2; @@ -147,14 +147,6 @@ uint32_t SineCore::GetAmp() return ampl; } -/** Sets the minimum pulse width in normalized digits. - * @post duty cylcles shorter than minWidth are supressed, both on the negative and the positive pulse - */ -void SineCore::SetMinPulseWidth(uint32_t minWidth) -{ - minPulse = minWidth; -} - /* Performs a lookup in the sine table */ /* 0 = 0, 2Pi = 65535 */ int32_t SineCore::SineLookup(uint16_t Arg) diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 6cc3a6f..291c662 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2016 Nail Güzel * Johannes Huebner @@ -318,6 +318,7 @@ Can::Can(uint32_t baseAddr, enum baudrates baudrate) break; } + nodeId = 1; // Reset CAN can_reset(canDev); @@ -432,7 +433,7 @@ void Can::HandleRx(int fifo) while (can_receive(canDev, fifo, true, &id, &ext, &rtr, &fmi, &length, (uint8_t*)data, NULL) > 0) { //printf("fifo: %d, id: %x, len: %d, data[0]: %x, data[1]: %x\r\n", fifo, id, length, data[0], data[1]); - if (id == 0x601 && length == 8) //SDO request, nodeid=1 + if (id == (0x600U + nodeId) && length == 8) //SDO request, nodeid=1 { ProcessSDO(data); } @@ -482,17 +483,36 @@ void Can::HandleTx() } } +void Can::SDOWrite(uint8_t remoteNodeId, uint16_t index, uint8_t subIndex, uint32_t data) +{ + uint32_t d[2]; + CAN_SDO *sdo = (CAN_SDO*)d; + + sdo->cmd = SDO_WRITE; + sdo->index = index; + sdo->subIndex = subIndex; + sdo->data = data; + + Send(0x600 + remoteNodeId, d); +} + /****************** Private methods and ISRs ********************/ //http://www.byteme.org.uk/canopenparent/canopen/sdo-service-data-objects-canopen/ void Can::ProcessSDO(uint32_t data[2]) { CAN_SDO *sdo = (CAN_SDO*)data; - if (sdo->index == 0x2000 && sdo->subIndex < Param::PARAM_LAST) + if (sdo->index >= 0x2000 && sdo->index <= 0x2001 && sdo->subIndex < Param::PARAM_LAST) { + Param::PARAM_NUM paramIdx = (Param::PARAM_NUM)sdo->subIndex; + + //SDO index 0x2001 will lookup the parameter by its unique ID + if (sdo->index == 0x2001) + paramIdx = Param::NumFromId(sdo->subIndex); + if (sdo->cmd == SDO_WRITE) { - if (Param::Set((Param::PARAM_NUM)sdo->subIndex, sdo->data) == 0) + if (Param::Set(paramIdx, sdo->data) == 0) { sdo->cmd = SDO_WRITE_REPLY; } @@ -504,7 +524,7 @@ void Can::ProcessSDO(uint32_t data[2]) } else if (sdo->cmd == SDO_READ) { - sdo->data = Param::Get((Param::PARAM_NUM)sdo->subIndex); + sdo->data = Param::Get(paramIdx); sdo->cmd = SDO_READ_REPLY; } } @@ -542,7 +562,7 @@ void Can::ProcessSDO(uint32_t data[2]) sdo->cmd = SDO_ABORT; sdo->data = SDO_ERR_INVIDX; } - Can::Send(0x581, data); + Can::Send(0x580 + nodeId, data); } void Can::SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) @@ -562,10 +582,12 @@ void Can::SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) void Can::ConfigureFilters() { - uint16_t idList[IDS_PER_BANK] = { 0x601, 0, 0, 0 }; + uint16_t idList[IDS_PER_BANK] = { 0, 0, 0, 0 }; int idIndex = 1; int filterId = canDev == CAN1 ? 0 : ((CAN_FMR(CAN2) >> 8) & 0x3F); + idList[0] = 0x600 + nodeId; + for (int i = 0; i < nextUserMessageIndex; i++) { idList[idIndex] = userIds[i]; diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp index 5aab385..da495cc 100644 --- a/src/stm32scheduler.cpp +++ b/src/stm32scheduler.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2017 Johannes Huebner * diff --git a/src/terminal.cpp b/src/terminal.cpp index 8ffd480..07dec7e 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the tumanako_vc project. + * This file is part of the libopeninv project. * * Copyright (C) 2011 Johannes Huebner * @@ -24,82 +24,143 @@ #include #include "terminal.h" #include "hwdefs.h" +#include "printf.h" -static const TERM_CMD *CmdLookup(char *buf); -static void term_send(uint32_t usart, const char *str); -static void ResetDMA(); +#define HWINFO_ENTRIES (sizeof(hwInfo) / sizeof(struct HwInfo)) -extern const TERM_CMD TermCmds[]; -static char inBuf[TERM_BUFSIZE]; -static char outBuf[2][TERM_BUFSIZE]; //double buffering +const Terminal::HwInfo Terminal::hwInfo[] = +{ + { USART1, DMA_CHANNEL4, DMA_CHANNEL5, GPIOA, GPIO_USART1_TX, GPIOB, GPIO_USART1_RE_TX }, + { USART2, DMA_CHANNEL7, DMA_CHANNEL6, GPIOA, GPIO_USART2_TX, GPIOD, GPIO_USART2_RE_TX }, + { USART3, DMA_CHANNEL2, DMA_CHANNEL3, GPIOB, GPIO_USART3_TX, GPIOC, GPIO_USART3_PR_TX }, +}; + +Terminal* Terminal::defaultTerminal; -void term_Init() +Terminal::Terminal(uint32_t usart, const TERM_CMD* commands, bool remap) +: usart(usart), + remap(remap), + termCmds(commands), + nodeId(1), + enabled(true), + pCurCmd(NULL), + lastIdx(0), + curBuf(0), + curIdx(0), + firstSend(true) { + //Search info entry + hw = hwInfo; + for (uint32_t i = 0; i < HWINFO_ENTRIES; i++) + { + if (hw->usart == usart) break; + hw++; + } + + defaultTerminal = this; + + gpio_set_mode(remap ? hw->port_re : hw->port, GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, remap ? hw->pin_re : hw->pin); + + usart_set_baudrate(usart, USART_BAUDRATE); + usart_set_databits(usart, 8); + usart_set_stopbits(usart, USART_STOPBITS_2); + usart_set_mode(usart, USART_MODE_TX_RX); + usart_set_parity(usart, USART_PARITY_NONE); + usart_set_flow_control(usart, USART_FLOWCONTROL_NONE); + usart_enable_rx_dma(usart); + usart_enable_tx_dma(usart); + + dma_channel_reset(DMA1, hw->dmatx); + dma_set_read_from_memory(DMA1, hw->dmatx); + dma_set_peripheral_address(DMA1, hw->dmatx, (uint32_t)&USART_DR(usart)); + dma_set_peripheral_size(DMA1, hw->dmatx, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, hw->dmatx, DMA_CCR_MSIZE_8BIT); + dma_enable_memory_increment_mode(DMA1, hw->dmatx); + + dma_channel_reset(DMA1, hw->dmarx); + dma_set_peripheral_address(DMA1, hw->dmarx, (uint32_t)&USART_DR(usart)); + dma_set_peripheral_size(DMA1, hw->dmarx, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, hw->dmarx, DMA_CCR_MSIZE_8BIT); + dma_enable_memory_increment_mode(DMA1, hw->dmarx); + dma_enable_channel(DMA1, hw->dmarx); + ResetDMA(); + + usart_enable(usart); } /** Run the terminal */ -void term_Run() +void Terminal::Run() { - char args[TERM_BUFSIZE]; - const TERM_CMD *pCurCmd = NULL; - int lastIdx = 0; + int numRcvd = dma_get_number_of_data(DMA1, hw->dmarx); + int currentIdx = bufSize - numRcvd; + + if (0 == numRcvd) + ResetDMA(); - while (1) + while (lastIdx < currentIdx) //echo + usart_send_blocking(usart, inBuf[lastIdx++]); + + if (currentIdx > 0) { - int numRcvd = dma_get_number_of_data(DMA1, TERM_USART_DMARX); - int currentIdx = TERM_BUFSIZE - numRcvd; + if (inBuf[currentIdx - 1] == '\n' || inBuf[currentIdx - 1] == '\r') + { + inBuf[currentIdx] = 0; + lastIdx = 0; + char *space = (char*)my_strchr(inBuf, ' '); - if (0 == numRcvd) - ResetDMA(); + if (0 == *space) //No args after command, look for end of line + { + space = (char*)my_strchr(inBuf, '\n'); + args[0] = 0; + } + else //There are arguments, copy everything behind the space + { + my_strcpy(args, space + 1); + } - while (lastIdx < currentIdx) //echo - usart_send_blocking(TERM_USART, inBuf[lastIdx++]); + if (0 == *space) //No \n found? try \r + space = (char*)my_strchr(inBuf, '\r'); - if (currentIdx > 0) - { - if (inBuf[currentIdx - 1] == '\n' || inBuf[currentIdx - 1] == '\r') + *space = 0; + pCurCmd = NULL; + + if (my_strcmp(inBuf, "enableuart") == 0) + { + EnableUart(args); + currentIdx = 0; //Prevent unknown command message + } + else if (my_strcmp(inBuf, "fastuart") == 0) + { + FastUart(args); + currentIdx = 0; + } + else { - inBuf[currentIdx] = 0; - lastIdx = 0; - char *space = (char*)my_strchr(inBuf, ' '); - - if (0 == *space) //No args after command, look for end of line - { - space = (char*)my_strchr(inBuf, '\n'); - args[0] = 0; - } - else //There are arguments, copy everything behind the space - { - my_strcpy(args, space + 1); - } - - if (0 == *space) //No \n found? try \r - space = (char*)my_strchr(inBuf, '\r'); - - *space = 0; pCurCmd = CmdLookup(inBuf); - ResetDMA(); - - if (NULL != pCurCmd) - { - usart_wait_send_ready(TERM_USART); - pCurCmd->CmdFunc(args); - } - else if (currentIdx > 1) - { - term_send(TERM_USART, "Unknown command sequence\r\n"); - } } - else if (inBuf[0] == '!' && NULL != pCurCmd) + + ResetDMA(); + + if (NULL != pCurCmd) + { + usart_wait_send_ready(usart); + pCurCmd->CmdFunc(this, args); + } + else if (currentIdx > 1 && enabled) { - ResetDMA(); - lastIdx = 0; - pCurCmd->CmdFunc(args); + Send("Unknown command sequence\r\n"); } } - } /* while(1) */ -} /* term_Run */ + else if (inBuf[0] == '!' && NULL != pCurCmd) + { + ResetDMA(); + lastIdx = 0; + pCurCmd->CmdFunc(this, args); + } + } +} /* * Revision 1 hardware can only use synchronous sending as the DMA channel is @@ -108,31 +169,26 @@ void term_Run() * buffering, so while one buffer is sent by DMA we can prepare the other * buffer to go next. */ -extern "C" int putchar(int c) +void Terminal::PutChar(char c) { - static uint32_t curIdx = 0, curBuf = 0, first = 1; - -#ifdef UARTDMABLOCKED if (hwRev == HW_REV1) { - usart_send_blocking(TERM_USART, c); + usart_send_blocking(usart, c); } - else -#endif - if (c == '\n' || curIdx == (TERM_BUFSIZE - 1)) + else if (c == '\n' || curIdx == (bufSize - 1)) { outBuf[curBuf][curIdx] = c; - while (!dma_get_interrupt_flag(DMA1, TERM_USART_DMATX, DMA_TCIF) && !first); + while (!dma_get_interrupt_flag(DMA1, hw->dmatx, DMA_TCIF) && !firstSend); - dma_disable_channel(DMA1, TERM_USART_DMATX); - dma_set_number_of_data(DMA1, TERM_USART_DMATX, curIdx + 1); - dma_set_memory_address(DMA1, TERM_USART_DMATX, (uint32_t)outBuf[curBuf]); - dma_clear_interrupt_flags(DMA1, TERM_USART_DMATX, DMA_TCIF); - dma_enable_channel(DMA1, TERM_USART_DMATX); + dma_disable_channel(DMA1, hw->dmatx); + dma_set_number_of_data(DMA1, hw->dmatx, curIdx + 1); + dma_set_memory_address(DMA1, hw->dmatx, (uint32_t)outBuf[curBuf]); + dma_clear_interrupt_flags(DMA1, hw->dmatx, DMA_TCIF); + dma_enable_channel(DMA1, hw->dmatx); curBuf = !curBuf; //switch buffers - first = 0; //only needed once so we don't get stuck in the while loop above + firstSend = false; //only needed once so we don't get stuck in the while loop above curIdx = 0; } else @@ -140,20 +196,69 @@ extern "C" int putchar(int c) outBuf[curBuf][curIdx] = c; curIdx++; } - return 0; } -static void ResetDMA() +extern "C" void putchar(int c) +{ + Terminal::defaultTerminal->PutChar(c); +} + +bool Terminal::KeyPressed() { - dma_disable_channel(DMA1, TERM_USART_DMARX); - dma_set_memory_address(DMA1, TERM_USART_DMARX, (uint32_t)inBuf); - dma_set_number_of_data(DMA1, TERM_USART_DMARX, TERM_BUFSIZE); - dma_enable_channel(DMA1, TERM_USART_DMARX); + return usart_get_flag(usart, USART_SR_RXNE); } -static const TERM_CMD *CmdLookup(char *buf) +void Terminal::FlushInput() { - const TERM_CMD *pCmd = TermCmds; + usart_recv(usart); +} + +void Terminal::ResetDMA() +{ + dma_disable_channel(DMA1, hw->dmarx); + dma_set_memory_address(DMA1, hw->dmarx, (uint32_t)inBuf); + dma_set_number_of_data(DMA1, hw->dmarx, bufSize); + dma_enable_channel(DMA1, hw->dmarx); +} + +void Terminal::EnableUart(char* arg) +{ + arg = my_trim(arg); + int val = my_atoi(arg); + + if (val == nodeId) + { + enabled = true; + gpio_set_mode(remap ? hw->port_re : hw->port, GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, remap ? hw->pin_re : hw->pin); + Send("OK\r\n"); + } + else + { + enabled = false; + gpio_set_mode(remap ? hw->port_re : hw->port, GPIO_MODE_INPUT, + GPIO_CNF_INPUT_FLOAT, remap ? hw->pin_re : hw->pin); + } +} + +void Terminal::FastUart(char *arg) +{ + arg = my_trim(arg); + int baud = arg[0] == '0' ? USART_BAUDRATE : 921600; + if (enabled) + { + Send("OK\r\n"); + Send("Baud rate now 921600\r\n"); + } + usart_set_baudrate(usart, baud); + usart_set_stopbits(usart, USART_STOPBITS_1); +} + +const TERM_CMD* Terminal::CmdLookup(char *buf) +{ + const TERM_CMD *pCmd = termCmds; + + if (!enabled) return NULL; for (; NULL != pCmd->cmd; pCmd++) { @@ -169,10 +274,8 @@ static const TERM_CMD *CmdLookup(char *buf) return pCmd; } -static void term_send(uint32_t usart, const char *str) +void Terminal::Send(const char *str) { for (;*str > 0; str++) usart_send_blocking(usart, *str); } - - diff --git a/src/terminalcommands.cpp b/src/terminalcommands.cpp index c6e6a0f..a0f19ee 100644 --- a/src/terminalcommands.cpp +++ b/src/terminalcommands.cpp @@ -1,5 +1,5 @@ /* - * This file is part of the stm32-... project. + * This file is part of the libopeninv project. * * Copyright (C) 2021 Johannes Huebner * @@ -28,7 +28,9 @@ #include "stm32_can.h" #include "terminalcommands.h" -void TerminalCommands::ParamSet(char* arg) +static Terminal* curTerm = NULL; + +void TerminalCommands::ParamSet(Terminal* term, char* arg) { char *pParamVal; s32fp val; @@ -39,7 +41,7 @@ void TerminalCommands::ParamSet(char* arg) if (*pParamVal == 0) { - printf("No parameter value given\r\n"); + fprintf(term, "No parameter value given\r\n"); return; } @@ -53,20 +55,20 @@ void TerminalCommands::ParamSet(char* arg) { if (0 == Param::Set(idx, val)) { - printf("Set OK\r\n"); + fprintf(term, "Set OK\r\n"); } else { - printf("Value out of range\r\n"); + fprintf(term, "Value out of range\r\n"); } } else { - printf("Unknown parameter %s\r\n", arg); + fprintf(term, "Unknown parameter %s\r\n", arg); } } -void TerminalCommands::ParamGet(char* arg) +void TerminalCommands::ParamGet(Terminal* term, char* arg) { Param::PARAM_NUM idx; s32fp val; @@ -86,11 +88,11 @@ void TerminalCommands::ParamGet(char* arg) if (Param::PARAM_INVALID != idx) { val = Param::Get(idx); - printf("%f\r\n", val); + fprintf(term, "%f\r\n", val); } else { - printf("Unknown parameter: '%s'\r\n", arg); + fprintf(term, "Unknown parameter: '%s'\r\n", arg); } *comma = orig; @@ -98,7 +100,7 @@ void TerminalCommands::ParamGet(char* arg) } while (',' == *comma); } -void TerminalCommands::ParamFlag(char *arg) +void TerminalCommands::ParamFlag(Terminal* term, char *arg) { char *pFlagVal; Param::PARAM_NUM idx; @@ -108,7 +110,7 @@ void TerminalCommands::ParamFlag(char *arg) if (*pFlagVal == 0) { - printf("No flag given\r\n"); + fprintf(term, "No flag given\r\n"); return; } @@ -143,20 +145,20 @@ void TerminalCommands::ParamFlag(char *arg) { Param::SetFlag(idx, flag); } - printf("Flag change OK\r\n"); + fprintf(term, "Flag change OK\r\n"); } else { - printf("Unknown flag\r\n"); + fprintf(term, "Unknown flag\r\n"); } } else { - printf("Unknown parameter %s\r\n", arg); + fprintf(term, "Unknown parameter %s\r\n", arg); } } -void TerminalCommands::ParamStream(char *arg) +void TerminalCommands::ParamStream(Terminal* term, char *arg) { Param::PARAM_NUM indexes[10]; int maxIndex = sizeof(indexes) / sizeof(Param::PARAM_NUM); @@ -171,7 +173,7 @@ void TerminalCommands::ParamStream(char *arg) if (0 == *arg) { - printf("Usage: stream n val1,val2...\r\n"); + fprintf(term, "Usage: stream n val1,val2...\r\n"); return; } arg++; //move behind space @@ -194,29 +196,29 @@ void TerminalCommands::ParamStream(char *arg) } else { - printf("Unknown parameter\r\n"); + fprintf(term, "Unknown parameter\r\n"); } } while (',' == *comma && curIndex < maxIndex); maxIndex = curIndex; - usart_recv(TERM_USART); + term->FlushInput(); - while (!usart_get_flag(TERM_USART, USART_SR_RXNE) && (repetitions > 0 || repetitions == -1)) + while (!term->KeyPressed() && (repetitions > 0 || repetitions == -1)) { comma = (char*)""; for (curIndex = 0; curIndex < maxIndex; curIndex++) { s32fp val = Param::Get(indexes[curIndex]); - printf("%s%f", comma, val); + fprintf(term, "%s%f", comma, val); comma = (char*)","; } - printf("\r\n"); + fprintf(term, "\r\n"); if (repetitions != -1) repetitions--; } } -void TerminalCommands::PrintParamsJson(char *arg) +void TerminalCommands::PrintParamsJson(Terminal* term, char *arg) { arg = my_trim(arg); @@ -224,7 +226,7 @@ void TerminalCommands::PrintParamsJson(char *arg) char comma = ' '; bool printHidden = arg[0] == 'h'; - printf("{"); + fprintf(term, "{"); for (uint32_t idx = 0; idx < Param::PARAM_LAST; idx++) { int canId, canOffset, canLength; @@ -234,31 +236,31 @@ void TerminalCommands::PrintParamsJson(char *arg) if ((Param::GetFlag((Param::PARAM_NUM)idx) & Param::FLAG_HIDDEN) == 0 || printHidden) { - printf("%c\r\n \"%s\": {\"unit\":\"%s\",\"value\":%f,",comma, pAtr->name, pAtr->unit, Param::Get((Param::PARAM_NUM)idx)); + fprintf(term, "%c\r\n \"%s\": {\"unit\":\"%s\",\"value\":%f,",comma, pAtr->name, pAtr->unit, Param::Get((Param::PARAM_NUM)idx)); if (Can::GetInterface(0)->FindMap((Param::PARAM_NUM)idx, canId, canOffset, canLength, canGain, isRx)) { - printf("\"canid\":%d,\"canoffset\":%d,\"canlength\":%d,\"cangain\":%d,\"isrx\":%s,", + fprintf(term, "\"canid\":%d,\"canoffset\":%d,\"canlength\":%d,\"cangain\":%d,\"isrx\":%s,", canId, canOffset, canLength, canGain, isRx ? "true" : "false"); } if (Param::IsParam((Param::PARAM_NUM)idx)) { - printf("\"isparam\":true,\"minimum\":%f,\"maximum\":%f,\"default\":%f,\"category\":\"%s\",\"i\":%d}", + fprintf(term, "\"isparam\":true,\"minimum\":%f,\"maximum\":%f,\"default\":%f,\"category\":\"%s\",\"i\":%d}", pAtr->min, pAtr->max, pAtr->def, pAtr->category, idx); } else { - printf("\"isparam\":false}"); + fprintf(term, "\"isparam\":false}"); } comma = ','; } } - printf("\r\n}\r\n"); + fprintf(term, "\r\n}\r\n"); } //cantx param id offset len gain -void TerminalCommands::MapCan(char *arg) +void TerminalCommands::MapCan(Terminal* term, char *arg) { Param::PARAM_NUM paramIdx = Param::PARAM_INVALID; int values[4]; @@ -271,14 +273,17 @@ void TerminalCommands::MapCan(char *arg) if (arg[0] == 'p') { + while (curTerm != NULL); //lock + curTerm = term; Can::GetInterface(0)->IterateCanMap(PrintCanMap); + curTerm = NULL; return; } if (arg[0] == 'c') { Can::GetInterface(0)->Clear(); - printf("All message definitions cleared\r\n"); + fprintf(term, "All message definitions cleared\r\n"); return; } @@ -287,7 +292,7 @@ void TerminalCommands::MapCan(char *arg) if (0 == *arg) { - printf("Missing argument\r\n"); + fprintf(term, "Missing argument\r\n"); return; } @@ -296,7 +301,7 @@ void TerminalCommands::MapCan(char *arg) if (*ending == 0 && op != 'd') { - printf("Missing argument\r\n"); + fprintf(term, "Missing argument\r\n"); return; } @@ -306,14 +311,14 @@ void TerminalCommands::MapCan(char *arg) if (Param::PARAM_INVALID == paramIdx) { - printf("Unknown parameter\r\n"); + fprintf(term, "Unknown parameter\r\n"); return; } if (op == 'd') { result = Can::GetInterface(0)->Remove(paramIdx); - printf("%d entries removed\r\n", result); + fprintf(term, "%d entries removed\r\n", result); return; } @@ -323,7 +328,7 @@ void TerminalCommands::MapCan(char *arg) if (0 == *ending && i < (numArgs - 1)) { - printf("Missing argument\r\n"); + fprintf(term, "Missing argument\r\n"); return; } @@ -359,71 +364,63 @@ void TerminalCommands::MapCan(char *arg) switch (result) { case CAN_ERR_INVALID_ID: - printf("Invalid CAN Id %x\r\n", values[0]); + fprintf(term, "Invalid CAN Id %x\r\n", values[0]); break; case CAN_ERR_INVALID_OFS: - printf("Invalid Offset %d\r\n", values[1]); + fprintf(term, "Invalid Offset %d\r\n", values[1]); break; case CAN_ERR_INVALID_LEN: - printf("Invalid length %d\r\n", values[2]); + fprintf(term, "Invalid length %d\r\n", values[2]); break; case CAN_ERR_MAXITEMS: - printf("Cannot map anymore items to CAN id %d\r\n", values[0]); + fprintf(term, "Cannot map anymore items to CAN id %d\r\n", values[0]); break; case CAN_ERR_MAXMESSAGES: - printf("Max message count reached\r\n"); + fprintf(term, "Max message count reached\r\n"); break; default: - printf("CAN map successful, %d message%s active\r\n", result, result > 1 ? "s" : ""); + fprintf(term, "CAN map successful, %d message%s active\r\n", result, result > 1 ? "s" : ""); } } -void TerminalCommands::SaveParameters(char *arg) +void TerminalCommands::SaveParameters(Terminal* term, char *arg) { arg = arg; uint32_t crc = parm_save(); - printf("Parameters stored, CRC=%x\r\n", crc); + fprintf(term, "Parameters stored, CRC=%x\r\n", crc); Can::GetInterface(0)->Save(); - printf("CANMAP stored\r\n"); + fprintf(term, "CANMAP stored\r\n"); } -void TerminalCommands::LoadParameters(char *arg) +void TerminalCommands::LoadParameters(Terminal* term, char *arg) { arg = arg; if (0 == parm_load()) { parm_Change((Param::PARAM_NUM)0); - printf("Parameters loaded\r\n"); + fprintf(term, "Parameters loaded\r\n"); } else { - printf("Parameter CRC error\r\n"); + fprintf(term, "Parameter CRC error\r\n"); } } -void TerminalCommands::Reset(char *arg) +void TerminalCommands::Reset(Terminal* term, char *arg) { + term = term; arg = arg; scb_reset_system(); } -void TerminalCommands::FastUart(char *arg) -{ - arg = my_trim(arg); - int baud = arg[0] == '0' ? USART_BAUDRATE : 921600; - printf("OK\r\n"); - printf("Baud rate now %d\r\n", baud); - usart_set_baudrate(TERM_USART, baud); -} - void TerminalCommands::PrintCanMap(Param::PARAM_NUM param, int canid, int offset, int length, s32fp gain, bool rx) { const char* name = Param::GetAttrib(param)->name; - printf("can "); + fprintf(curTerm, "can "); if (rx) - printf("rx "); + fprintf(curTerm, "rx "); else - printf("tx "); - printf("%s %d %d %d %d\r\n", name, canid, offset, length, gain); + fprintf(curTerm, "tx "); + fprintf(curTerm, "%s %d %d %d %d\r\n", name, canid, offset, length, gain); } From a564cc7a646e70b1eff1331127d7c30ac42991c3 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 8 Feb 2021 10:29:00 +0100 Subject: [PATCH 23/83] Allow disabling TX DMA --- include/terminal.h | 2 ++ src/terminal.cpp | 26 +++++++++++++++++++------- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/include/terminal.h b/include/terminal.h index a2fbb7f..26ad7b7 100644 --- a/include/terminal.h +++ b/include/terminal.h @@ -38,6 +38,7 @@ class Terminal: public IPutChar void PutChar(char c); bool KeyPressed(); void FlushInput(); + void DisableTxDMA(); static Terminal* defaultTerminal; private: @@ -66,6 +67,7 @@ class Terminal: public IPutChar const TERM_CMD* termCmds; uint8_t nodeId; bool enabled; + bool txDmaEnabled; const TERM_CMD *pCurCmd; int lastIdx; uint8_t curBuf; diff --git a/src/terminal.cpp b/src/terminal.cpp index 07dec7e..5ba6f73 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -23,11 +23,14 @@ #include #include #include "terminal.h" -#include "hwdefs.h" #include "printf.h" #define HWINFO_ENTRIES (sizeof(hwInfo) / sizeof(struct HwInfo)) +#ifndef USART_BAUDRATE +#define USART_BAUDRATE 115200 +#endif // USART_BAUDRATE + const Terminal::HwInfo Terminal::hwInfo[] = { { USART1, DMA_CHANNEL4, DMA_CHANNEL5, GPIOA, GPIO_USART1_TX, GPIOB, GPIO_USART1_RE_TX }, @@ -43,6 +46,7 @@ Terminal::Terminal(uint32_t usart, const TERM_CMD* commands, bool remap) termCmds(commands), nodeId(1), enabled(true), + txDmaEnabled(true), pCurCmd(NULL), lastIdx(0), curBuf(0), @@ -171,7 +175,7 @@ void Terminal::Run() */ void Terminal::PutChar(char c) { - if (hwRev == HW_REV1) + if (!txDmaEnabled) { usart_send_blocking(usart, c); } @@ -198,11 +202,6 @@ void Terminal::PutChar(char c) } } -extern "C" void putchar(int c) -{ - Terminal::defaultTerminal->PutChar(c); -} - bool Terminal::KeyPressed() { return usart_get_flag(usart, USART_SR_RXNE); @@ -213,6 +212,13 @@ void Terminal::FlushInput() usart_recv(usart); } +void Terminal::DisableTxDMA() +{ + txDmaEnabled = false; + dma_disable_channel(DMA1, hw->dmatx); + usart_disable_tx_dma(usart); +} + void Terminal::ResetDMA() { dma_disable_channel(DMA1, hw->dmarx); @@ -279,3 +285,9 @@ void Terminal::Send(const char *str) for (;*str > 0; str++) usart_send_blocking(usart, *str); } + +//Backward compatibility for printf +extern "C" void putchar(int c) +{ + Terminal::defaultTerminal->PutChar(c); +} From 8a79c453177a657532936a5671ff42ffcfe38781 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 8 Feb 2021 13:19:18 +0100 Subject: [PATCH 24/83] Removed unneeded include --- src/terminalcommands.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/terminalcommands.cpp b/src/terminalcommands.cpp index a0f19ee..05919cb 100644 --- a/src/terminalcommands.cpp +++ b/src/terminalcommands.cpp @@ -17,7 +17,6 @@ * along with this program. If not, see . */ #include -#include #include "hwdefs.h" #include "terminal.h" #include "params.h" From d2eeb22c564dffe40aebddbadb15a22927ef01f8 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 17 Feb 2021 13:15:22 +0100 Subject: [PATCH 25/83] Always enable Terminal 1 --- include/terminal.h | 2 +- src/terminal.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/include/terminal.h b/include/terminal.h index 26ad7b7..4a29635 100644 --- a/include/terminal.h +++ b/include/terminal.h @@ -33,7 +33,7 @@ class Terminal: public IPutChar { public: Terminal(uint32_t usart, const TERM_CMD* commands, bool remap = false); - void SetNodeId(uint8_t id) { nodeId = id; } + void SetNodeId(uint8_t id); void Run(); void PutChar(char c); bool KeyPressed(); diff --git a/src/terminal.cpp b/src/terminal.cpp index 5ba6f73..e1b331d 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -166,6 +166,19 @@ void Terminal::Run() } } +void Terminal::SetNodeId(uint8_t id) +{ + char one[] = { '1', 0 }; + nodeId = id; + + if (nodeId != 1) + { + Send("Disabling terminal, type 'enableuart ' to re-enable\r\n"); + } + + EnableUart(one); +} + /* * Revision 1 hardware can only use synchronous sending as the DMA channel is * occupied by the encoder timer (TIM3, channel 3). From aba4a4e9fe91d68d817a4a662aa3ed292d3e45b1 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 17 Feb 2021 13:15:33 +0100 Subject: [PATCH 26/83] Small optimization --- src/foc.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index 115c49c..45e034e 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -36,12 +36,11 @@ static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-b static const s32fp lqminusld = FP_FROMFLT(0.0058); static const u32fp sqrt3 = SQRT3; static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) -static const s32fp sqrt3inv2 = 2*sqrt3inv1; //2/sqrt(2) static const s32fp zeroOffset = FP_FROMINT(1); static const int32_t modMax = FP_DIV(FP_FROMINT(2U), sqrt3); static const int32_t modMaxPow2 = modMax * modMax; -static int32_t minPulse = 1000; -static int32_t maxPulse = FP_FROMINT(2) - 1000; +static const int32_t minPulse = 1000; +static const int32_t maxPulse = FP_FROMINT(2) - 1000; s32fp FOC::id; s32fp FOC::iq; @@ -57,7 +56,7 @@ void FOC::ParkClarke(s32fp il1, s32fp il2, uint16_t angle) s32fp cos = SineCore::Cosine(angle); //Clarke transformation s32fp ia = il1; - s32fp ib = FP_MUL(sqrt3inv1, il1) + FP_MUL(sqrt3inv2, il2); + s32fp ib = FP_MUL(sqrt3inv1, il1 + 2 * il2); //Park transformation id = FP_MUL(cos, ia) + FP_MUL(sin, ib); iq = FP_MUL(cos, ib) - FP_MUL(sin, ia); From 59f77f788e7717e0448ff9eaf28cf7d1c9e23362 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Wed, 17 Mar 2021 19:04:42 +0000 Subject: [PATCH 27/83] Add microsecond blocking delay function This adds a simple blocking delay function to assist with timing sensitive SPI drivers. The delay is fixed for STM32F103 processors running at 72MHz and no attempt is made to compensate for different speeds. This should be OK for all openinverter boards. Tests: - Host in a standalone libopencm3 application and toggle a GPIO pin with a uDelay(1000) call. - Test at short 10 usec and long 0.5 sec delays and ensure that at least the minimum time is achieved. --- include/delay.h | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 include/delay.h diff --git a/include/delay.h b/include/delay.h new file mode 100644 index 0000000..e7ef51f --- /dev/null +++ b/include/delay.h @@ -0,0 +1,41 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2021 David J. Fiddes + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef DELAY_H +#define DELAY_H + +/** + * \brief Blocking delay for a period + * + * \param[in] period Length of the delay in micro-seconds + */ +inline void uDelay(int period) +{ + // Empirically determined constant by measurement of GPIO toggle + // of 1000 uS delay on a 72MHz STM32F103 processor + static const int CyclesPerMicroSecond = 12; + + int iterations = period * CyclesPerMicroSecond; + + for (int i = 0; i < iterations; i++) + { + __asm__("nop"); + } +} + +#endif // DELAY_H From bbb527e84762f42e8973a9b7007951e411908c00 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Wed, 17 Mar 2021 19:16:30 +0000 Subject: [PATCH 28/83] Add a simple C CRC-8-CCITT implementation This adds an 8-bit CRC calculation with the CCITT polynomial (x^8+x^2+X+1). It uses a 256 byte lookup table which is pre-calculated using the C pre-preprocessor. This could have been implemented with constexpr but that would require c++17 to enable use of std::array in constexpr functions. Using the pre-processor is more portable for now. Code originally comes from a stack overflow answer: https://stackoverflow.com/a/27843120/6353 Tests: - Check against incoming and outgoing logic analyser captures of STGAP1AS isolated gate drivers which use this algorithm. --- include/crc8.h | 70 +++++++++++++++++++++++++++++++++++++++++++++++ src/crc8.cpp | 74 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 144 insertions(+) create mode 100644 include/crc8.h create mode 100644 src/crc8.cpp diff --git a/include/crc8.h b/include/crc8.h new file mode 100644 index 0000000..a475cb8 --- /dev/null +++ b/include/crc8.h @@ -0,0 +1,70 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2021 David J. Fiddes + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef __CRC8_H_ +#define __CRC8_H_ + +#include + +/* + * Precalculated look up table for CRC calculation + */ +extern const uint8_t crc_table[256]; + +/** + * \brief Calculate 8-bit CRC + * + * + * Calculate an 8-bit CRC of a block of data. The algorithm used depends on + * library configuration. + * + * \param[in] p pointer to the data to take we wish to compute the CRC of + * \param[in] len length of the data + * \param[in] crc Initial value for the CRC algorithm (or CRC of previous data) + * + * \return Calculated CRC value + */ +inline uint8_t crc8(uint8_t* p, uint8_t len, uint8_t crc) +{ + while (len--) + { + crc = crc_table[crc ^ *p++]; + } + + return crc; +} + +/** + * \brief Calculate 8-bit CRC of a byte + * + * Calculate an 8-bit CRC of a single byte. The algorithm used depends on + * library configuration. + * + * \param[in] input data byte to compute the CRC of + * \param[in] crc Initial value for the CRC algorithm (or CRC of previous data) + * + * \return Calculated CRC value + */ +inline uint8_t crc8(uint8_t input, uint8_t crc) +{ + crc = crc_table[crc ^ input]; + + return crc; +} + +#endif /* __CRC8_H_ */ diff --git a/src/crc8.cpp b/src/crc8.cpp new file mode 100644 index 0000000..aaf9ee0 --- /dev/null +++ b/src/crc8.cpp @@ -0,0 +1,74 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2021 David J. Fiddes + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +/* + * Generated table driven CRC-8-CCITT code from: + * https://stackoverflow.com/a/27843120/6353 + */ + +#include "crc8.h" +#include + +/* + * Just change this define to whatever polynomial is in use + * A polynomial of 0x07 corresponds to x^8 + x^2 + x + 1 + */ +#define CRC1B(b) ((uint8_t)((b) << 1) ^ ((b)&0x80 ? 0x07 : 0)) // MS first + +/* + * 8+1 entry enum lookup table define + */ +#define CRC(b) CRC_##b // or CRC8B(b) + +enum +{ + CRC(0x01) = CRC1B(0x80), + CRC(0x02) = CRC1B(CRC(0x01)), + CRC(0x04) = CRC1B(CRC(0x02)), + CRC(0x08) = CRC1B(CRC(0x04)), + CRC(0x10) = CRC1B(CRC(0x08)), + CRC(0x20) = CRC1B(CRC(0x10)), + CRC(0x40) = CRC1B(CRC(0x20)), + CRC(0x80) = CRC1B(CRC(0x40)), + // Add 0x03 to optimise in CRCTAB1 + CRC(0x03) = CRC(0x02) ^ CRC(0x01) +}; + +/* + * Build a 256 byte CRC constant lookup table, built from from a reduced + * constant lookup table, namely CRC of each bit, 0x00 to 0x80. These will be + * defined as enumerations to take it easy on the compiler. This depends on the + * relation: CRC(a^b) = CRC(a)^CRC(b) In other words, we can build up each byte + * CRC as the xor of the CRC of each bit. So CRC(0x05) = CRC(0x04)^CRC(0x01). We + * include the CRC of 0x03 for a little more optimisation, since CRCTAB1 can use + * it instead of CRC(0x01)^CRC(0x02), again a little easier on the compiler. + */ + +#define CRCTAB1(ex) CRC(0x01) ex, CRC(0x02) ex, CRC(0x03) ex, +#define CRCTAB2(ex) CRCTAB1(ex) CRC(0x04) ex, CRCTAB1(^CRC(0x04) ex) +#define CRCTAB3(ex) CRCTAB2(ex) CRC(0x08) ex, CRCTAB2(^CRC(0x08) ex) +#define CRCTAB4(ex) CRCTAB3(ex) CRC(0x10) ex, CRCTAB3(^CRC(0x10) ex) +#define CRCTAB5(ex) CRCTAB4(ex) CRC(0x20) ex, CRCTAB4(^CRC(0x20) ex) +#define CRCTAB6(ex) CRCTAB5(ex) CRC(0x40) ex, CRCTAB5(^CRC(0x40) ex) + +/* + * This is the final lookup table. It is rough on the compiler, but generates + * the required lookup table automagically at compile time. + */ +const uint8_t crc_table[256] = { 0, CRCTAB6() CRC(0x80), CRCTAB6(^CRC(0x80)) }; From 93e1f99b2fcf88cbf2bfcd851b9d265ee67951a2 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sat, 10 Apr 2021 16:21:29 +0100 Subject: [PATCH 29/83] Allow enum parameters Allow strongly typed enum parameters to be stored. We just cast to an int with no validation. --- include/params.h | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/include/params.h b/include/params.h index 073fd57..8ce70d8 100644 --- a/include/params.h +++ b/include/params.h @@ -60,13 +60,27 @@ namespace Param uint32_t id; } Attributes; - int Set(PARAM_NUM ParamNum, s32fp ParamVal); s32fp Get(PARAM_NUM ParamNum); int GetInt(PARAM_NUM ParamNum); s32fp GetScl(PARAM_NUM ParamNum); bool GetBool(PARAM_NUM ParamNum); + + template + T GetEnum(PARAM_NUM ParamNum) + { + return static_cast(GetInt(ParamNum)); + } + + int Set(PARAM_NUM ParamNum, s32fp ParamVal); void SetInt(PARAM_NUM ParamNum, int ParamVal); void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal); + + template + void SetEnum(PARAM_NUM ParamNum, T ParamVal) + { + SetInt(ParamNum, static_cast(ParamVal)); + } + PARAM_NUM NumFromString(const char *name); PARAM_NUM NumFromId(uint32_t id); const Attributes *GetAttrib(PARAM_NUM ParamNum); From 9e906713af796cbfbadf256ee8bc5f6ba166196d Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 10 Apr 2021 23:02:29 +0200 Subject: [PATCH 30/83] Optimization of FOC code Added simple averaging to ADC module Added proportional only function to PI controller --- include/foc.h | 7 +++++-- include/picontroller.h | 8 +++++++- src/anain.cpp | 13 ++++++++++++- src/foc.cpp | 28 +++++++++++++++++----------- src/picontroller.cpp | 11 +++++++++++ 5 files changed, 52 insertions(+), 15 deletions(-) diff --git a/include/foc.h b/include/foc.h index 5605911..1a171c3 100644 --- a/include/foc.h +++ b/include/foc.h @@ -25,10 +25,11 @@ class FOC { public: - static void ParkClarke(s32fp il1, s32fp il2, uint16_t angle); + static void SetAngle(uint16_t angle); + static void ParkClarke(s32fp il1, s32fp il2); static int32_t GetQLimit(int32_t maxVd); static int32_t GetTotalVoltage(int32_t ud, int32_t uq); - static void InvParkClarke(int32_t ud, int32_t uq, uint16_t angle); + static void InvParkClarke(int32_t ud, int32_t uq); static void Mtpa(int32_t is, int32_t& idref, int32_t& iqref); static int32_t GetMaximumModulationIndex(); static s32fp id; @@ -39,6 +40,8 @@ class FOC private: static uint32_t sqrt(uint32_t rad); static u32fp fpsqrt(u32fp rad); + static s32fp sin; + static s32fp cos; }; #endif // FOC_H diff --git a/include/picontroller.h b/include/picontroller.h index 01afadc..1fff0c0 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -59,12 +59,18 @@ class PiController */ void SetCallingFrequency(int val) { frequency = val; } - /** Run regulator to obtain a new actuator value + /** Run controller to obtain a new actuator value * \param curVal currently measured value * \return new actuator value */ int32_t Run(s32fp curVal); + /** Run controller to obtain a new actuator value, run only proportional part + * \param curVal currently measured value + * \return new actuator value + */ + int32_t RunProportionalOnly(s32fp curVal); + /** Reset integrator to 0 */ void ResetIntegrator() { esum = 0; } diff --git a/src/anain.cpp b/src/anain.cpp index 7e45bd0..2bcd183 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -82,6 +82,7 @@ void AnaIn::Configure(uint32_t port, uint8_t pin) * - NUM_SAMPLES = 3: Median of last 3 values is returned * - NUM_SAMPLES = 9: Median of last 3 medians is returned * - NUM_SAMPLES = 12: Average of last 4 medians is returned +* - NUM_SAMPLES <= 16: Average is returned * * @return Filtered value */ @@ -111,8 +112,18 @@ uint16_t AnaIn::Get() } return (med[0] + med[1] + med[2] + med[3]) >> 2; + #elif NUM_SAMPLES <= 16 + uint16_t *curVal = firstValue; + uint16_t avg = 0; + + for (int i = 0; i < NUM_SAMPLES; i++, curVal += ANA_IN_COUNT) + { + avg += *curVal; + } + + return avg / NUM_SAMPLES; #else - #error NUM_SAMPLES must be 1, 3, 9 or 12 + #error NUM_SAMPLES must be <= 16 #endif } diff --git a/src/foc.cpp b/src/foc.cpp index 45e034e..46a8eaf 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -45,15 +45,25 @@ static const int32_t maxPulse = FP_FROMINT(2) - 1000; s32fp FOC::id; s32fp FOC::iq; s32fp FOC::DutyCycles[3]; +s32fp FOC::sin; +s32fp FOC::cos; + +/** @brief Set angle for Park und inverse Park transformation + * @param angle uint16_t rotor angle + */ +void FOC::SetAngle(uint16_t angle) +{ + sin = SineCore::Sine(angle); + cos = SineCore::Cosine(angle); +} /** @brief Transform current to rotor system using Clarke and Park transformation + * @pre Call SetAngle to specify angle for Park transformation * @post flux producing (id) and torque producing (iq) current are written * to FOC::id and FOC::iq */ -void FOC::ParkClarke(s32fp il1, s32fp il2, uint16_t angle) +void FOC::ParkClarke(s32fp il1, s32fp il2) { - s32fp sin = SineCore::Sine(angle); - s32fp cos = SineCore::Cosine(angle); //Clarke transformation s32fp ia = il1; s32fp ib = FP_MUL(sqrt3inv1, il1 + 2 * il2); @@ -64,10 +74,9 @@ void FOC::ParkClarke(s32fp il1, s32fp il2, uint16_t angle) /** \brief distribute motor current in magnetic torque and reluctance torque with the least total current * - * \param is int32_t total motor current + * \param[in] is int32_t total motor current * \param[out] idref int32_t& resulting direct current reference * \param[out] iqref int32_t& resulting quadrature current reference - * \return void * */ void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref) @@ -97,18 +106,15 @@ int32_t FOC::GetTotalVoltage(int32_t ud, int32_t uq) } /** \brief Calculate duty cycles for generating ud and uq at given angle + * + * @pre Call SetAngle to specify angle for inverse Park transformation * * \param ud int32_t direct voltage * \param uq int32_t quadrature voltage - * \param angle uint16_t rotor angle - * \return void * */ -void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) +void FOC::InvParkClarke(int32_t ud, int32_t uq) { - s32fp sin = SineCore::Sine(angle); - s32fp cos = SineCore::Cosine(angle); - //Inverse Park transformation s32fp ua = (cos * ud - sin * uq) >> CST_DIGITS; s32fp ub = (cos * uq + sin * ud) >> CST_DIGITS; diff --git a/src/picontroller.cpp b/src/picontroller.cpp index 112297e..a63999c 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -36,3 +36,14 @@ int32_t PiController::Run(s32fp curVal) return ylim; } + +int32_t PiController::RunProportionalOnly(s32fp curVal) +{ + s32fp err = refVal - curVal; + + int32_t y = FP_TOINT(err * kp); + int32_t ylim = MAX(y, minY); + ylim = MIN(ylim, maxY); + + return ylim; +} From 410aaf0fbe0696e9578a0f320421e13a2f0da444 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sun, 11 Apr 2021 14:09:21 +0100 Subject: [PATCH 31/83] Add a value CLAMP macro following MIN/MAX --- include/my_math.h | 1 + src/picontroller.cpp | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/my_math.h b/include/my_math.h index b419917..1806b49 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -22,6 +22,7 @@ #define ABS(a) ((a) < 0?(-a) : (a)) #define MIN(a,b) ((a) < (b)?(a):(b)) #define MAX(a,b) ((a) > (b)?(a):(b)) +#define CLAMP(a,low,high) ((a) > (high)?(high):((a) < (low)? (low):(a))) #define RAMPUP(current, target, rate) ((target < current || (current + rate) > target) ? target : current + rate) #define RAMPDOWN(current, target, rate) ((target > current || (current - rate) < target) ? target : current - rate) #define IIRFILTER(l,n,c) (((n) + ((l) << (c)) - (l)) >> (c)) diff --git a/src/picontroller.cpp b/src/picontroller.cpp index 112297e..0f73252 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -30,8 +30,7 @@ int32_t PiController::Run(s32fp curVal) esum += err; int32_t y = FP_TOINT(err * kp + (esum / frequency) * ki); - int32_t ylim = MAX(y, minY); - ylim = MIN(ylim, maxY); + int32_t ylim = CLAMP(y, minY, maxY); esum += ((ylim - y) * frequency) / (ki + 1); //anti windup return ylim; From 4a4f0c26e71a51cc7d7b0eb7479e85cabcf88636 Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 16 Apr 2021 16:08:01 +0200 Subject: [PATCH 32/83] enable temperature sensor when used --- src/anain.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/anain.cpp b/src/anain.cpp index 2bcd183..0ede62c 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -165,5 +165,6 @@ uint8_t AnaIn::AdcChFromPort(uint32_t command_port, int command_bit) if (command_bit<6) return command_bit+10; break; } + adc_enable_temperature_sensor(); return 16; } From de42cb7cb1bb2b86b43493b826dfba99e54fa446 Mon Sep 17 00:00:00 2001 From: johannes Date: Tue, 20 Apr 2021 16:02:58 +0200 Subject: [PATCH 33/83] CAN and param save module auto-detect flash and page size and always write to last/second-to-last page --- include/stm32_can.h | 1 + src/param_save.cpp | 20 ++++++++++++++++---- src/stm32_can.cpp | 17 ++++++++++++++--- 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index 44adac4..af60971 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -109,6 +109,7 @@ class Can void ReplaceParamUidByEnum(CANIDMAP *canMap); void ConfigureFilters(); void SetFilterBank(int& idIndex, int& filterId, uint16_t* idList); + uint32_t GetFlashAddress(); static Can* interfaces[]; }; diff --git a/src/param_save.cpp b/src/param_save.cpp index aa34489..1b6a328 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include "params.h" #include "param_save.h" @@ -42,6 +43,15 @@ typedef struct uint32_t padding; } PARAM_PAGE; +static uint32_t GetFlashAddress() +{ + uint32_t flashSize = desig_get_flash_size(); + uint32_t pageSize = flash_get_page_size(); + + //Always save parameters to last flash page + return FLASH_BASE + flashSize * 1024 - pageSize; +} + /** * Save parameters to flash * @@ -50,7 +60,8 @@ typedef struct uint32_t parm_save() { PARAM_PAGE parmPage; - unsigned int idx; + uint32_t idx; + uint32_t paramAddress = GetFlashAddress(); crc_reset(); memset32((int*)&parmPage, 0xFFFFFFFF, PARAM_WORDS); @@ -66,12 +77,12 @@ uint32_t parm_save() parmPage.crc = crc_calculate_block(((uint32_t*)&parmPage), (2 * NUM_PARAMS)); flash_unlock(); - flash_erase_page(PARAM_ADDRESS); + flash_erase_page(paramAddress); for (idx = 0; idx < PARAM_WORDS; idx++) { uint32_t* pData = ((uint32_t*)&parmPage) + idx; - flash_program_word(PARAM_ADDRESS + idx * sizeof(uint32_t), *pData); + flash_program_word(paramAddress + idx * sizeof(uint32_t), *pData); } flash_lock(); return parmPage.crc; @@ -85,7 +96,8 @@ uint32_t parm_save() */ int parm_load() { - PARAM_PAGE *parmPage = (PARAM_PAGE *)PARAM_ADDRESS; + uint32_t paramAddress = GetFlashAddress(); + PARAM_PAGE *parmPage = (PARAM_PAGE *)paramAddress; crc_reset(); uint32_t crc = crc_calculate_block(((uint32_t*)parmPage), (2 * NUM_PARAMS)); diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 291c662..066f9cc 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -28,10 +28,12 @@ #include #include #include +#include #include #include #include "stm32_can.h" +#define CANMAP_ADDRESS Can::GetFlashAddress() #define MAX_INTERFACES 2 #define IDS_PER_BANK 4 #define SDO_WRITE 0x40 @@ -51,9 +53,9 @@ #define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->canId < CANID_UNSET; c++) #define forEachPosMap(c,m) for (CANPOS *c = m->items; (c - m->items) < MAX_ITEMS_PER_MESSAGE && c->numBits > 0; c++) -#if (2 *((MAX_ITEMS_PER_MESSAGE * 6 + 2) * MAX_MESSAGES + 2) + 4) > FLASH_PAGE_SIZE -#error CANMAP will not fit in one flash page -#endif +//#if (2 *((MAX_ITEMS_PER_MESSAGE * 6 + 2) * MAX_MESSAGES + 2) + 4) > FLASH_PAGE_SIZE +//#error CANMAP will not fit in one flash page +//#endif struct CAN_SDO { @@ -779,6 +781,15 @@ void Can::ReplaceParamUidByEnum(CANIDMAP *canMap) } } +uint32_t Can::GetFlashAddress() +{ + uint32_t flashSize = desig_get_flash_size(); + uint32_t pageSize = flash_get_page_size(); + + //Always save CAN mapping to second-to-last flash page + return FLASH_BASE + flashSize * 1024 - pageSize * 2; +} + /* Interrupt service routines */ extern "C" void usb_lp_can_rx0_isr(void) { From f089afee699612993410acb4b2e161837bdccf1c Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 21 Apr 2021 20:59:53 +0200 Subject: [PATCH 34/83] Small optimization in scheduler --- include/stm32scheduler.h | 1 - src/stm32scheduler.cpp | 17 ++++------------- 2 files changed, 4 insertions(+), 14 deletions(-) diff --git a/include/stm32scheduler.h b/include/stm32scheduler.h index fcc727d..9007070 100644 --- a/include/stm32scheduler.h +++ b/include/stm32scheduler.h @@ -51,7 +51,6 @@ class Stm32Scheduler protected: private: - static void nofunc(void); static const enum tim_oc_id ocMap[MAX_TASKS]; void (*functions[MAX_TASKS]) (void); uint16_t periods[MAX_TASKS]; diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp index da495cc..78e1371 100644 --- a/src/stm32scheduler.cpp +++ b/src/stm32scheduler.cpp @@ -34,12 +34,6 @@ Stm32Scheduler::Stm32Scheduler(uint32_t timer) /* Maximum counter value */ timer_set_period(timer, 0xFFFF); - for (int i = 0; i < MAX_TASKS; i++) - { - functions[i] = nofunc; - periods[i] = 0xFFFF; - } - nextTask = 0; } @@ -70,30 +64,27 @@ void Stm32Scheduler::AddTask(void (*function)(void), uint16_t period) void Stm32Scheduler::Run() { - for (int i = 0; i < MAX_TASKS; i++) + for (int i = 0; i < nextTask; i++) { if (timer_get_flag(timer, TIM_SR_CC1IF << i)) { uint16_t start = timer_get_counter(timer); - timer_clear_flag(timer, TIM_SR_CC1IF << i); + TIM_CCR(timer, i) += periods[i]; functions[i](); execTicks[i] = timer_get_counter(timer) - start; } } + timer_clear_flag(timer, TIM_SR_CC1IF | TIM_SR_CC2IF | TIM_SR_CC3IF | TIM_SR_CC4IF); } int Stm32Scheduler::GetCpuLoad() { int totalLoad = 0; - for (int i = 0; i < MAX_TASKS; i++) + for (int i = 0; i < nextTask; i++) { int load = (10 * execTicks[i]) / periods[i]; totalLoad += load; } return totalLoad; } - -void Stm32Scheduler::nofunc() -{ -} From ce4adfc3b3a8d2c008d432eba5fc8aa0873d3292 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Tue, 27 Apr 2021 12:55:09 +0100 Subject: [PATCH 35/83] Fix exported putchar() so it matches its prototype This fixes a compile warning when building with LTO (which fails for other reasons). The mistmatch doesn't cause problems in regular builds because the linker doesn't check types. --- src/terminal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/terminal.cpp b/src/terminal.cpp index e1b331d..a99fc01 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -300,7 +300,7 @@ void Terminal::Send(const char *str) } //Backward compatibility for printf -extern "C" void putchar(int c) +extern "C" void putchar(char c) { Terminal::defaultTerminal->PutChar(c); } From 0317452fee9d84aceed50c1ce7a2950041fe95de Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 30 Apr 2021 08:09:47 +0200 Subject: [PATCH 36/83] Added LIN class --- include/linbus.h | 54 +++++++++++++++ src/linbus.cpp | 166 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 220 insertions(+) create mode 100644 include/linbus.h create mode 100644 src/linbus.cpp diff --git a/include/linbus.h b/include/linbus.h new file mode 100644 index 0000000..914681f --- /dev/null +++ b/include/linbus.h @@ -0,0 +1,54 @@ +/* + * This file is part of the stm32-car project. + * + * Copyright (C) 2021 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef LINBUS_H +#define LINBUS_H + + +class LinBus +{ + public: + /** Default constructor */ + LinBus(uint32_t usart, int baudrate); + void Send(uint8_t id, uint8_t* data, uint8_t len); + void Receive(); + bool HasReceived(uint8_t pid, uint8_t requiredLen); + uint8_t* GetReceivedBytes() { return &recvBuffer[2]; } + + protected: + + private: + struct HwInfo + { + uint32_t usart; + uint8_t dmatx; + uint32_t port; + uint16_t pin; + }; + + static uint8_t Checksum(uint8_t pid, uint8_t* data, int len); + + static const HwInfo hwInfo[]; + uint32_t usart; + const HwInfo* hw; + uint8_t sendBuffer[11]; + uint8_t recvBuffer[11]; + uint8_t receiveIdx; +}; + +#endif // LINBUS_H diff --git a/src/linbus.cpp b/src/linbus.cpp new file mode 100644 index 0000000..35cd40d --- /dev/null +++ b/src/linbus.cpp @@ -0,0 +1,166 @@ +/* + * This file is part of the stm32-... project. + * + * Copyright (C) 2021 Johannes Huebner + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include +#include +#include +#include "linbus.h" + +#define HWINFO_ENTRIES (sizeof(hwInfo) / sizeof(struct HwInfo)) + +const LinBus::HwInfo LinBus::hwInfo[] = +{ + { USART1, DMA_CHANNEL4, GPIOA, GPIO_USART1_TX }, + { USART2, DMA_CHANNEL7, GPIOA, GPIO_USART2_TX }, + { USART3, DMA_CHANNEL2, GPIOB, GPIO_USART3_TX }, +}; + + +/** \brief Create a new LIN bus object and initialize USART, GPIO and DMA + * \pre According USART, GPIO and DMA clocks must be enabled + * \param usart USART base address + * \param baudrate 9600 or 19200 + * + */ +LinBus::LinBus(uint32_t usart, int baudrate) + : usart(usart) +{ + hw = hwInfo; + + for (uint32_t i = 0; i < HWINFO_ENTRIES; i++) + { + if (hw->usart == usart) break; + hw++; + } + + gpio_set_mode(hw->port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, hw->pin); + + //gpio_set_mode(GPIOA, GPIO_MODE_INPUT, + // GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX); + + usart_set_baudrate(usart, baudrate); + usart_set_databits(usart, 8); + usart_set_stopbits(usart, USART_STOPBITS_1); + usart_set_mode(usart, USART_MODE_TX_RX); + usart_set_parity(usart, USART_PARITY_NONE); + usart_set_flow_control(usart, USART_FLOWCONTROL_NONE); + USART_CR2(usart) |= USART_CR2_LINEN; + usart_enable_tx_dma(usart); + + dma_channel_reset(DMA1, hw->dmatx); + dma_set_read_from_memory(DMA1, hw->dmatx); + dma_set_peripheral_address(DMA1, hw->dmatx, (uint32_t)&USART_DR(usart)); + dma_set_memory_address(DMA1, DMA_CHANNEL4, (uint32_t)sendBuffer); + dma_set_peripheral_size(DMA1, hw->dmatx, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, hw->dmatx, DMA_CCR_MSIZE_8BIT); + dma_enable_memory_increment_mode(DMA1, hw->dmatx); + + usart_enable(usart); +} + +/** \brief Send data on LIN bus + * + * \param id feature ID + * \param data payload data, if any + * \param len length of payload, if any + * + */ +void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) +{ + bool p1 = !(((id & 0x2) > 0) ^ ((id & 0x8) > 0) ^ ((id & 0x10) > 0) ^ ((id & 0x20) > 0)); + bool p0 = ((id & 0x1) > 0) ^ ((id & 0x2) > 0) ^ ((id & 0x4) > 0) ^ ((id & 0x10) > 0); + int sendLen = len == 0 ? 2 : len + 3; + + if (len > 8) return; + + dma_disable_channel(DMA1, hw->dmatx); + dma_set_number_of_data(DMA1, hw->dmatx, sendLen); + + sendBuffer[0] = 0x55; //Sync + sendBuffer[1] = id | p1 << 7 | p0 << 6; //PID + + for (uint8_t i = 0; i < len; i++) + sendBuffer[i + 2] = data[i]; + + sendBuffer[len + 2] = Checksum(sendBuffer[1], data, len); + + dma_clear_interrupt_flags(DMA1, hw->dmatx, DMA_TCIF); + + USART_CR1(usart) |= USART_CR1_SBK; + dma_enable_channel(DMA1, hw->dmatx); +} + +/** \brief Check if a break or character was received and store it, if yes + */ +void LinBus::Receive() +{ + if (receiveIdx == sizeof(recvBuffer)) return; + + if (usart_get_flag(usart, USART_SR_LBD)) + { + receiveIdx = 0; + USART_SR(usart) &= ~USART_SR_LBD; + } + else if (usart_get_flag(usart, USART_SR_RXNE)) + { + recvBuffer[receiveIdx] = usart_recv(usart); + receiveIdx++; + } +} + +/** \brief Check whether we received valid data with given PID and length + * + * \param pid ID with parity (PID) + * \param requiredLen Length of data we expect + * \return true if data with given properties was received + * + */ +bool LinBus::HasReceived(uint8_t pid, uint8_t requiredLen) +{ + if (requiredLen > 8) return false; + + if (receiveIdx == (requiredLen + 2) && recvBuffer[1] == pid) + { + uint8_t checksum = Checksum(recvBuffer[1], &recvBuffer[2], requiredLen); + + return checksum == recvBuffer[requiredLen + 2]; + } + + return false; +} + +/** \brief Calculate LIN checksum + * + * \param pid ID with parity + * \param data uint8_t* + * \param len int + * \return checksum + * + */ +uint8_t LinBus::Checksum(uint8_t pid, uint8_t* data, int len) +{ + uint8_t checksum = pid; + + for (int i = 0; i < len; i++) + { + uint16_t tmp = (uint16_t)checksum + (uint16_t)data[i]; + if (tmp > 256) tmp -= 255; + checksum = tmp; + } + return checksum ^ 0xff; +} From 306cd3b3eff567725ad580551535561aba74746e Mon Sep 17 00:00:00 2001 From: johannes Date: Sun, 2 May 2021 00:00:02 +0200 Subject: [PATCH 37/83] HasReceived now uses feature ID w/o parity --- include/linbus.h | 1 + src/linbus.cpp | 18 ++++++++++++------ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/include/linbus.h b/include/linbus.h index 914681f..f24e933 100644 --- a/include/linbus.h +++ b/include/linbus.h @@ -42,6 +42,7 @@ class LinBus }; static uint8_t Checksum(uint8_t pid, uint8_t* data, int len); + static uint8_t Parity(uint8_t id); static const HwInfo hwInfo[]; uint32_t usart; diff --git a/src/linbus.cpp b/src/linbus.cpp index 35cd40d..a1a9138 100644 --- a/src/linbus.cpp +++ b/src/linbus.cpp @@ -82,8 +82,6 @@ LinBus::LinBus(uint32_t usart, int baudrate) */ void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) { - bool p1 = !(((id & 0x2) > 0) ^ ((id & 0x8) > 0) ^ ((id & 0x10) > 0) ^ ((id & 0x20) > 0)); - bool p0 = ((id & 0x1) > 0) ^ ((id & 0x2) > 0) ^ ((id & 0x4) > 0) ^ ((id & 0x10) > 0); int sendLen = len == 0 ? 2 : len + 3; if (len > 8) return; @@ -92,7 +90,7 @@ void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) dma_set_number_of_data(DMA1, hw->dmatx, sendLen); sendBuffer[0] = 0x55; //Sync - sendBuffer[1] = id | p1 << 7 | p0 << 6; //PID + sendBuffer[1] = Parity(id); for (uint8_t i = 0; i < len; i++) sendBuffer[i + 2] = data[i]; @@ -125,16 +123,16 @@ void LinBus::Receive() /** \brief Check whether we received valid data with given PID and length * - * \param pid ID with parity (PID) + * \param pid Feature ID to check for * \param requiredLen Length of data we expect * \return true if data with given properties was received * */ -bool LinBus::HasReceived(uint8_t pid, uint8_t requiredLen) +bool LinBus::HasReceived(uint8_t id, uint8_t requiredLen) { if (requiredLen > 8) return false; - if (receiveIdx == (requiredLen + 2) && recvBuffer[1] == pid) + if (receiveIdx == (requiredLen + 2) && recvBuffer[1] == Parity(id)) { uint8_t checksum = Checksum(recvBuffer[1], &recvBuffer[2], requiredLen); @@ -164,3 +162,11 @@ uint8_t LinBus::Checksum(uint8_t pid, uint8_t* data, int len) } return checksum ^ 0xff; } + +uint8_t LinBus::Parity(uint8_t id) +{ + bool p1 = !(((id & 0x2) > 0) ^ ((id & 0x8) > 0) ^ ((id & 0x10) > 0) ^ ((id & 0x20) > 0)); + bool p0 = ((id & 0x1) > 0) ^ ((id & 0x2) > 0) ^ ((id & 0x4) > 0) ^ ((id & 0x10) > 0); + + return id | p1 << 7 | p0 << 6; +} From de5b53a65d32ad74d1bfc189ce164855d687edbb Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 6 May 2021 19:22:55 +0200 Subject: [PATCH 38/83] Only erase flash page when it is not already erased. This allows sharing a flash page between param_save and can_save. The module that writes to the (potential) start of the page must be called first! PARAM_BLKNUM defines on which block, counting backwards from end of flash the parameters are saved. CAN_BLKNUM does the same for CAN. --- src/param_save.cpp | 12 ++++++++--- src/stm32_can.cpp | 43 +++++++++++++++++++++++----------------- src/terminalcommands.cpp | 4 ++-- 3 files changed, 36 insertions(+), 23 deletions(-) diff --git a/src/param_save.cpp b/src/param_save.cpp index 1b6a328..b368086 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -46,10 +46,9 @@ typedef struct static uint32_t GetFlashAddress() { uint32_t flashSize = desig_get_flash_size(); - uint32_t pageSize = flash_get_page_size(); //Always save parameters to last flash page - return FLASH_BASE + flashSize * 1024 - pageSize; + return FLASH_BASE + flashSize * 1024 - PARAM_BLKNUM * PARAM_BLKSIZE; } /** @@ -62,6 +61,11 @@ uint32_t parm_save() PARAM_PAGE parmPage; uint32_t idx; uint32_t paramAddress = GetFlashAddress(); + uint32_t check = 0xFFFFFFFF; + uint32_t* baseAddress = (uint32_t*)paramAddress; + + for (int i = 0; i < PARAM_WORDS; i++, baseAddress++) + check &= *baseAddress; crc_reset(); memset32((int*)&parmPage, 0xFFFFFFFF, PARAM_WORDS); @@ -77,7 +81,9 @@ uint32_t parm_save() parmPage.crc = crc_calculate_block(((uint32_t*)&parmPage), (2 * NUM_PARAMS)); flash_unlock(); - flash_erase_page(paramAddress); + + if (check != 0xFFFFFFFF) + flash_erase_page(paramAddress); for (idx = 0; idx < PARAM_WORDS; idx++) { diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 066f9cc..abd6eef 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -33,7 +33,6 @@ #include #include "stm32_can.h" -#define CANMAP_ADDRESS Can::GetFlashAddress() #define MAX_INTERFACES 2 #define IDS_PER_BANK 4 #define SDO_WRITE 0x40 @@ -43,9 +42,9 @@ #define SDO_READ_REPLY 0x43 #define SDO_ERR_INVIDX 0x06020000 #define SDO_ERR_RANGE 0x06090030 -#define SENDMAP_ADDRESS CANMAP_ADDRESS -#define RECVMAP_ADDRESS (CANMAP_ADDRESS + sizeof(canSendMap)) -#define CRC_ADDRESS (CANMAP_ADDRESS + sizeof(canSendMap) + sizeof(canRecvMap)) +#define SENDMAP_ADDRESS(b) b +#define RECVMAP_ADDRESS(b) (b + sizeof(canSendMap)) +#define CRC_ADDRESS(b) (b+ + sizeof(canSendMap) + sizeof(canRecvMap)) #define SENDMAP_WORDS (sizeof(canSendMap) / sizeof(uint32_t)) #define RECVMAP_WORDS (sizeof(canRecvMap) / sizeof(uint32_t)) #define CANID_UNSET 0xffff @@ -53,9 +52,9 @@ #define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->canId < CANID_UNSET; c++) #define forEachPosMap(c,m) for (CANPOS *c = m->items; (c - m->items) < MAX_ITEMS_PER_MESSAGE && c->numBits > 0; c++) -//#if (2 *((MAX_ITEMS_PER_MESSAGE * 6 + 2) * MAX_MESSAGES + 2) + 4) > FLASH_PAGE_SIZE -//#error CANMAP will not fit in one flash page -//#endif +#if (2 *((MAX_ITEMS_PER_MESSAGE * 6 + 2) * MAX_MESSAGES + 2) + 4) > CAN_BLKSIZE +#error CANMAP will not fit in one flash page +#endif struct CAN_SDO { @@ -194,18 +193,27 @@ bool Can::FindMap(Param::PARAM_NUM param, int& canId, int& offset, int& length, void Can::Save() { uint32_t crc; + uint32_t check = 0xFFFFFFFF; + uint32_t baseAddress = GetFlashAddress(); + uint32_t *checkAddress = (uint32_t*)baseAddress; + + for (int i = 0; i < CAN_BLKSIZE / 4; i++, checkAddress++) + check &= *checkAddress; + crc_reset(); flash_unlock(); flash_set_ws(2); - flash_erase_page(CANMAP_ADDRESS); + + if (check != 0xFFFFFFFF) //Only erase when needed + flash_erase_page(baseAddress); ReplaceParamEnumByUid(canSendMap); ReplaceParamEnumByUid(canRecvMap); - SaveToFlash(SENDMAP_ADDRESS, (uint32_t *)canSendMap, SENDMAP_WORDS); - crc = SaveToFlash(RECVMAP_ADDRESS, (uint32_t *)canRecvMap, RECVMAP_WORDS); - SaveToFlash(CRC_ADDRESS, &crc, 1); + SaveToFlash(baseAddress, (uint32_t *)canSendMap, SENDMAP_WORDS); + crc = SaveToFlash(RECVMAP_ADDRESS(baseAddress), (uint32_t *)canRecvMap, RECVMAP_WORDS); + SaveToFlash(CRC_ADDRESS(baseAddress), &crc, 1); flash_lock(); ReplaceParamUidByEnum(canSendMap); @@ -620,17 +628,17 @@ void Can::ConfigureFilters() int Can::LoadFromFlash() { - uint32_t* data = (uint32_t *)CANMAP_ADDRESS; - uint32_t storedCrc = *(uint32_t*)CRC_ADDRESS; + uint32_t data = GetFlashAddress(); + uint32_t storedCrc = *(uint32_t*)CRC_ADDRESS(data); uint32_t crc; crc_reset(); - crc = crc_calculate_block(data, SENDMAP_WORDS + RECVMAP_WORDS); + crc = crc_calculate_block((uint32_t*)data, SENDMAP_WORDS + RECVMAP_WORDS); if (storedCrc == crc) { - memcpy32((int*)canSendMap, (int*)SENDMAP_ADDRESS, SENDMAP_WORDS); - memcpy32((int*)canRecvMap, (int*)RECVMAP_ADDRESS, RECVMAP_WORDS); + memcpy32((int*)canSendMap, (int*)SENDMAP_ADDRESS(data), SENDMAP_WORDS); + memcpy32((int*)canRecvMap, (int*)RECVMAP_ADDRESS(data), RECVMAP_WORDS); ReplaceParamUidByEnum(canSendMap); ReplaceParamUidByEnum(canRecvMap); return 1; @@ -784,10 +792,9 @@ void Can::ReplaceParamUidByEnum(CANIDMAP *canMap) uint32_t Can::GetFlashAddress() { uint32_t flashSize = desig_get_flash_size(); - uint32_t pageSize = flash_get_page_size(); //Always save CAN mapping to second-to-last flash page - return FLASH_BASE + flashSize * 1024 - pageSize * 2; + return FLASH_BASE + flashSize * 1024 - CAN_BLKSIZE * CAN_BLKNUM; } /* Interrupt service routines */ diff --git a/src/terminalcommands.cpp b/src/terminalcommands.cpp index 05919cb..30db61c 100644 --- a/src/terminalcommands.cpp +++ b/src/terminalcommands.cpp @@ -385,10 +385,10 @@ void TerminalCommands::MapCan(Terminal* term, char *arg) void TerminalCommands::SaveParameters(Terminal* term, char *arg) { arg = arg; - uint32_t crc = parm_save(); - fprintf(term, "Parameters stored, CRC=%x\r\n", crc); Can::GetInterface(0)->Save(); fprintf(term, "CANMAP stored\r\n"); + uint32_t crc = parm_save(); + fprintf(term, "Parameters stored, CRC=%x\r\n", crc); } void TerminalCommands::LoadParameters(Terminal* term, char *arg) From 60a6eba37b79f59bd1f46fa6a03306c41ac741f7 Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 8 May 2021 07:58:25 +0200 Subject: [PATCH 39/83] Fixed anti-windup, had forgotten an FP_FROMINT --- src/picontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/picontroller.cpp b/src/picontroller.cpp index a63999c..415a997 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -32,7 +32,7 @@ int32_t PiController::Run(s32fp curVal) int32_t y = FP_TOINT(err * kp + (esum / frequency) * ki); int32_t ylim = MAX(y, minY); ylim = MIN(ylim, maxY); - esum += ((ylim - y) * frequency) / (ki + 1); //anti windup + esum += FP_FROMINT(((ylim - y) * frequency) / (ki + 1)); //anti windup return ylim; } From d73d83afbe968bf101a3f330209d6d7024fc949b Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sat, 15 May 2021 12:37:33 +0100 Subject: [PATCH 40/83] Fix type problems with standalone printf implementation Update the definition of putchar to match normal Linux glibc to allow compilation on Linux as well as STM32. Fix a type problem with character pointer format strings in the printf implementation to allow compilation on Linux x86_64. Tests: - Build on Linux x86_64 and STM32 - Run STM32 build on Bluepill and verify parameter display is working as before --- src/printf.cpp | 4 ++-- src/terminal.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/printf.cpp b/src/printf.cpp index 6667990..c9edcca 100644 --- a/src/printf.cpp +++ b/src/printf.cpp @@ -33,7 +33,7 @@ #define PAD_RIGHT 1 #define PAD_ZERO 2 -extern "C" void putchar(char c); +extern "C" int putchar(int c); class ExternPutChar: public IPutChar { @@ -164,7 +164,7 @@ static int print(IPutChar* put, const char *format, va_list args ) width += *format - '0'; } if( *format == 's' ) { - register char *s = (char *)va_arg( args, int ); + register char *s = va_arg( args, char* ); pc += prints (put, s?s:"(null)", width, pad); continue; } diff --git a/src/terminal.cpp b/src/terminal.cpp index a99fc01..3b9dab7 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -300,7 +300,8 @@ void Terminal::Send(const char *str) } //Backward compatibility for printf -extern "C" void putchar(char c) +extern "C" int putchar(int c) { Terminal::defaultTerminal->PutChar(c); + return c; } From c3071492ae4c4d1758f1b8e6ce5432dd3a9ab991 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sat, 15 May 2021 12:41:04 +0100 Subject: [PATCH 41/83] Add helper methods to ErrorMessage to assist with testing Allow the posted messages to be reset to a default state and check whether an error has been posted. These are useful when including ErrorMessage in unit tests which need to ensure the error state is consistent. Tests: - Build on both STM32 and Linux - Verify correct operation in several independent unit tests that raise errors --- include/errormessage.h | 2 ++ src/errormessage.cpp | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/include/errormessage.h b/include/errormessage.h index b9f0560..bb038ab 100644 --- a/include/errormessage.h +++ b/include/errormessage.h @@ -44,10 +44,12 @@ class ErrorMessage public: static void SetTime(uint32_t time); static void Post(ERROR_MESSAGE_NUM err); + static void ResetAll(); static void UnpostAll(); static void PrintAllErrors(); static void PrintNewErrors(); static ERROR_MESSAGE_NUM GetLastError(); + static bool HasErrorBeenPosted(ERROR_MESSAGE_NUM err); protected: private: static void PrintError(uint32_t time, ERROR_MESSAGE_NUM err); diff --git a/src/errormessage.cpp b/src/errormessage.cpp index 313fd72..f4308a0 100644 --- a/src/errormessage.cpp +++ b/src/errormessage.cpp @@ -83,6 +83,18 @@ void ErrorMessage::Post(ERROR_MESSAGE_NUM msg) } } +/** Reset the error buffer and unpost all error messages. Useful for testing */ +void ErrorMessage::ResetAll() +{ + lastError = ERROR_NONE; + for (uint32_t i = 0; i < ERROR_BUF_SIZE; i++) + { + errorBuffer[i].msg = ERROR_MESSAGE_LAST; + errorBuffer[i].time = 0; + } + UnpostAll(); +} + /** Unpost all error message, i.e. make them postable again. Does not reset the error buffer */ void ErrorMessage::UnpostAll() @@ -106,6 +118,12 @@ ERROR_MESSAGE_NUM ErrorMessage::GetLastError() return lastError; } +/** Check to see if a particular error has been posted. Useful for testing */ +bool ErrorMessage::HasErrorBeenPosted(ERROR_MESSAGE_NUM err) +{ + return posted[err]; +} + /** Print all errors currently in error memory */ void ErrorMessage::PrintAllErrors() { From fdcbd462389aaadcc07b358960e089f21b363377 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sat, 15 May 2021 12:46:23 +0100 Subject: [PATCH 42/83] Add libopeninv CMake build Basic CMake build script for the parts of libopeninv needed to build unit tests. --- CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f983f7f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,7 @@ +add_library(libopeninv src/errormessage.cpp src/params.cpp src/my_string.c src/printf.cpp src/my_fp.c src/sine_core.cpp src/picontroller.cpp src/foc.cpp) +target_compile_definitions(libopeninv PUBLIC "CONTROL=CTRL_FOC") +target_compile_definitions(libopeninv PUBLIC "CTRL_SINE=0") +target_compile_definitions(libopeninv PUBLIC "CTRL_FOC=1") + +target_include_directories(libopeninv PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") +target_include_directories(libopeninv PRIVATE "${PROJECT_SOURCE_DIR}/include") From e04d026d1e5d93424799ef02bef031278520119f Mon Sep 17 00:00:00 2001 From: johannes Date: Sun, 16 May 2021 19:00:18 +0200 Subject: [PATCH 43/83] Properly avoid division by 0 --- src/picontroller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/picontroller.cpp b/src/picontroller.cpp index 415a997..c235a07 100644 --- a/src/picontroller.cpp +++ b/src/picontroller.cpp @@ -32,7 +32,11 @@ int32_t PiController::Run(s32fp curVal) int32_t y = FP_TOINT(err * kp + (esum / frequency) * ki); int32_t ylim = MAX(y, minY); ylim = MIN(ylim, maxY); - esum += FP_FROMINT(((ylim - y) * frequency) / (ki + 1)); //anti windup + + if (ki != 0) + { + esum += FP_FROMINT(((ylim - y) * frequency) / ki); //anti windup + } return ylim; } From def2dc3bb2a2aa02ebce74be1be83151cceb62eb Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Mon, 17 May 2021 22:45:34 +0100 Subject: [PATCH 44/83] Add macro to convert fixed point values to float values The FP_TOFLT() macro is the reverse of FP_FROMFLT() and intended to be used in test code. Tests: - Build as part of expanded unit tests and verify values returned in the debugger --- include/my_fp.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/my_fp.h b/include/my_fp.h index 3d63e55..bd6c8fc 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -38,6 +38,7 @@ #define FP_FROMINT(a) ((s32fp)((a) << CST_DIGITS)) #define FP_TOINT(a) ((s32fp)((a) >> CST_DIGITS)) #define FP_FROMFLT(a) ((s32fp)((a) * FRAC_FAC)) +#define FP_TOFLT(a) ((float)(a) / FRAC_FAC) #define FP_MUL(a, b) (((a) * (b)) >> CST_DIGITS) #define FP_DIV(a, b) (((a) << CST_DIGITS) / (b)) From 36550a1f93c251692fd3fa8a99997d1765381341 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Thu, 20 May 2021 15:59:16 +0100 Subject: [PATCH 45/83] Add coverage support to libopeninv --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f983f7f..bec387d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,6 @@ -add_library(libopeninv src/errormessage.cpp src/params.cpp src/my_string.c src/printf.cpp src/my_fp.c src/sine_core.cpp src/picontroller.cpp src/foc.cpp) +include(CodeCoverage) +append_coverage_compiler_flags() +add_library(libopeninv src/errormessage.cpp src/params.cpp src/my_string.c src/printf.cpp src/my_fp.c src/sine_core.cpp src/picontroller.cpp src/foc.cpp src/fu.cpp) target_compile_definitions(libopeninv PUBLIC "CONTROL=CTRL_FOC") target_compile_definitions(libopeninv PUBLIC "CTRL_SINE=0") target_compile_definitions(libopeninv PUBLIC "CTRL_FOC=1") From b74e3dca1647e01aa85a79d0b0be61cadcdebcd1 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Mon, 14 Jun 2021 18:01:38 +0100 Subject: [PATCH 46/83] Pull all build specific options from a global_options library Remove all of the hand rolled compile options and instead pull them from a global_options library from elsewhere in the build system. Tests: - Build as part of a refactored CMake build system --- CMakeLists.txt | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bec387d..79615ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,4 @@ -include(CodeCoverage) -append_coverage_compiler_flags() add_library(libopeninv src/errormessage.cpp src/params.cpp src/my_string.c src/printf.cpp src/my_fp.c src/sine_core.cpp src/picontroller.cpp src/foc.cpp src/fu.cpp) -target_compile_definitions(libopeninv PUBLIC "CONTROL=CTRL_FOC") -target_compile_definitions(libopeninv PUBLIC "CTRL_SINE=0") -target_compile_definitions(libopeninv PUBLIC "CTRL_FOC=1") - +target_link_libraries(libopeninv global_options) target_include_directories(libopeninv PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") target_include_directories(libopeninv PRIVATE "${PROJECT_SOURCE_DIR}/include") From 89853427f5407755650021392b849c0c71d40997 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 16 Jun 2021 11:44:18 +0200 Subject: [PATCH 47/83] fixed warning in stm32_can, fixed bug in linbus --- src/linbus.cpp | 5 +---- src/stm32_can.cpp | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/linbus.cpp b/src/linbus.cpp index a1a9138..ab7b2cb 100644 --- a/src/linbus.cpp +++ b/src/linbus.cpp @@ -50,9 +50,6 @@ LinBus::LinBus(uint32_t usart, int baudrate) gpio_set_mode(hw->port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, hw->pin); - //gpio_set_mode(GPIOA, GPIO_MODE_INPUT, - // GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX); - usart_set_baudrate(usart, baudrate); usart_set_databits(usart, 8); usart_set_stopbits(usart, USART_STOPBITS_1); @@ -65,7 +62,7 @@ LinBus::LinBus(uint32_t usart, int baudrate) dma_channel_reset(DMA1, hw->dmatx); dma_set_read_from_memory(DMA1, hw->dmatx); dma_set_peripheral_address(DMA1, hw->dmatx, (uint32_t)&USART_DR(usart)); - dma_set_memory_address(DMA1, DMA_CHANNEL4, (uint32_t)sendBuffer); + dma_set_memory_address(DMA1, hw->dmatx, (uint32_t)sendBuffer); dma_set_peripheral_size(DMA1, hw->dmatx, DMA_CCR_PSIZE_8BIT); dma_set_memory_size(DMA1, hw->dmatx, DMA_CCR_MSIZE_8BIT); dma_enable_memory_increment_mode(DMA1, hw->dmatx); diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index abd6eef..2ff08b1 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -45,8 +45,8 @@ #define SENDMAP_ADDRESS(b) b #define RECVMAP_ADDRESS(b) (b + sizeof(canSendMap)) #define CRC_ADDRESS(b) (b+ + sizeof(canSendMap) + sizeof(canRecvMap)) -#define SENDMAP_WORDS (sizeof(canSendMap) / sizeof(uint32_t)) -#define RECVMAP_WORDS (sizeof(canRecvMap) / sizeof(uint32_t)) +#define SENDMAP_WORDS (sizeof(canSendMap) / (sizeof(uint32_t))) +#define RECVMAP_WORDS (sizeof(canRecvMap) / (sizeof(uint32_t))) #define CANID_UNSET 0xffff #define NUMBITS_LASTMARKER -1 #define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->canId < CANID_UNSET; c++) From 5f232dc6412d6966b0566df0dcea325e6a11b5d8 Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 17 Jun 2021 20:35:43 +0200 Subject: [PATCH 48/83] Added crude lockout flag to inhibit sending mapped messages while map is invalid --- include/stm32_can.h | 1 + src/stm32_can.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/include/stm32_can.h b/include/stm32_can.h index af60971..2ac7c21 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -64,6 +64,7 @@ class Can static const int MAX_MESSAGES = 10; static const int SENDBUFFER_LEN = 20; static const int MAX_USER_MESSAGES = 10; + static volatile bool isSaving; struct CANPOS { diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 2ff08b1..d041bf9 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -72,6 +72,7 @@ struct CANSPEED }; Can* Can::interfaces[MAX_INTERFACES]; +volatile bool Can::isSaving = false; static void DummyCallback(uint32_t i, uint32_t* d) { i=i; d=d; } static const CANSPEED canSpeed[Can::BaudLast] = @@ -197,6 +198,8 @@ void Can::Save() uint32_t baseAddress = GetFlashAddress(); uint32_t *checkAddress = (uint32_t*)baseAddress; + isSaving = true; + for (int i = 0; i < CAN_BLKSIZE / 4; i++, checkAddress++) check &= *checkAddress; @@ -218,6 +221,8 @@ void Can::Save() ReplaceParamUidByEnum(canSendMap); ReplaceParamUidByEnum(canRecvMap); + + isSaving = false; } /** \brief Send all defined messages @@ -230,6 +235,8 @@ void Can::SendAll() forEachPosMap(curPos, curMap) { + if (isSaving) return; //Only send mapped messages when not currently saving to flash + s32fp val = Param::Get((Param::PARAM_NUM)curPos->mapParam); if (curPos->gain <= 32 && curPos->gain >= -32) @@ -449,6 +456,8 @@ void Can::HandleRx(int fifo) } else { + if (isSaving) continue; //Only handle mapped messages when not currently saving to flash + CANIDMAP *recvMap = FindById(canRecvMap, id); if (0 != recvMap) From 5644523d27a52abe22a3efa41affe1580188df1e Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 17 Jun 2021 20:39:12 +0200 Subject: [PATCH 49/83] Boot loader pin commands changed to new semantics --- include/stm32_loader.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/stm32_loader.h b/include/stm32_loader.h index 93ee479..f15357d 100644 --- a/include/stm32_loader.h +++ b/include/stm32_loader.h @@ -20,7 +20,8 @@ #define STM32_LOADER_H_INCLUDED #include -#define PINDEF_ADDRESS 0x0801F400 +#define PINDEF_BLKNUM 3 //3rd to last flash page +#define PINDEF_BLKSIZE 1024 #define NUM_PIN_COMMANDS 10 #define PIN_IN 0 #define PIN_OUT 1 From 10682633707c2bedb59051269440205ff56010b1 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Fri, 18 Jun 2021 11:47:32 +0100 Subject: [PATCH 50/83] Include all CPP files in CMake build Expand the CMake build to include a new libopeninv_stm32f1 library. This pulls in all of the CPP modules that were missing from the libopeninv library. The new library is conditional on the "stm32f1" platform being specified. CMakeList.txt reformatted with cmake-format. Tests: - Build and link into working STM32 binary --- CMakeLists.txt | 38 +++++++++++++++++++++++++++++++++++--- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 79615ab..a9e90fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,36 @@ -add_library(libopeninv src/errormessage.cpp src/params.cpp src/my_string.c src/printf.cpp src/my_fp.c src/sine_core.cpp src/picontroller.cpp src/foc.cpp src/fu.cpp) +add_library( + libopeninv + src/errormessage.cpp + src/foc.cpp + src/fu.cpp + src/my_fp.c + src/my_string.c + src/params.cpp + src/picontroller.cpp + src/printf.cpp + src/sine_core.cpp) target_link_libraries(libopeninv global_options) -target_include_directories(libopeninv PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") -target_include_directories(libopeninv PRIVATE "${PROJECT_SOURCE_DIR}/include") +target_include_directories(libopeninv + PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") +# "Project" includes needed to customise the library build are pulled from the +# top level include directory +target_include_directories(libopeninv PRIVATE "${CMAKE_SOURCE_DIR}/include") + +if(PLATFORM STREQUAL "stm32f1") + add_library( + libopeninv_stm32f1 + src/anain.cpp + src/digio.cpp + src/param_save.cpp + src/stm32_can.cpp + src/stm32scheduler.cpp + src/terminal.cpp + src/terminalcommands.cpp) + target_link_libraries(libopeninv_stm32f1 global_options) + target_include_directories(libopeninv_stm32f1 + PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") + # "Project" includes needed to customise the library build are pulled from the + # top level include directory + target_include_directories(libopeninv_stm32f1 + PRIVATE "${CMAKE_SOURCE_DIR}/include") +endif() From 7e4c6c6d3f2133edba629d67850f1256bd62870f Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Thu, 24 Jun 2021 16:10:34 +0100 Subject: [PATCH 51/83] Fix fp_ln() to use type guaranteed to be 32-bit unsigned int On the C2000 platform an "int" is 16 bits not 32-bits as it is on ARM and x86_64. This changes fp_ln() to use a type guaranteed to be 32-bit. Fixes an error on the C2000 build with: error: shift count is too large --- include/my_fp.h | 2 +- src/my_fp.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/my_fp.h b/include/my_fp.h index bd6c8fc..7ad39a1 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -56,7 +56,7 @@ extern "C" char* fp_itoa(char * buf, s32fp a); s32fp fp_atoi(const char *str, int fracDigits); u32fp fp_sqrt(u32fp rad); -s32fp fp_ln(unsigned int x); +s32fp fp_ln(uint32_t x); #ifdef __cplusplus } diff --git a/src/my_fp.c b/src/my_fp.c index 5a6ba0d..0a1c5f9 100644 --- a/src/my_fp.c +++ b/src/my_fp.c @@ -91,7 +91,7 @@ u32fp fp_sqrt(u32fp rad) return sqrt; } -s32fp fp_ln(unsigned int x) +s32fp fp_ln(uint32_t x) { int n = 0; const s32fp ln2 = FP_FROMFLT(0.6931471806); From 1eaa7f77b8dfc76fe7d96a24e767ac76398cb1fa Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 30 Jun 2021 11:01:54 +0200 Subject: [PATCH 52/83] Corrected MTPA formula --- src/foc.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index 46a8eaf..54f5a6e 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -32,7 +32,7 @@ static const s32fp fluxLinkage = FP_FROMFLT(0.09); static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); -static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-bit left shift because otherwise it can't be represented +static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.03444736); //additional 10-bit left shift because otherwise it can't be represented static const s32fp lqminusld = FP_FROMFLT(0.0058); static const u32fp sqrt3 = SQRT3; static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) @@ -83,8 +83,9 @@ void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref) { int32_t isSquared = is * is; int32_t sign = is < 0 ? -1 : 1; - s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 10)); - idref = FP_TOINT(FP_DIV(fluxLinkage - term1, lqminusld)); + //factor of 8 has been incorporated into the right shift (7 instead of 10) + s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 7)); + idref = FP_TOINT(FP_DIV(fluxLinkage - term1, 4 * lqminusld)); iqref = sign * (int32_t)sqrt(isSquared - idref * idref); } From 866a94612409d3fe5a8e2603342e3315c712c8c9 Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 30 Jun 2021 13:53:22 +0200 Subject: [PATCH 53/83] Converted "slow" functions to float math --- include/fu.h | 2 +- include/my_math.h | 1 + include/params.h | 7 ++++--- src/fu.cpp | 4 ++-- src/param_save.cpp | 2 +- src/params.cpp | 30 ++++++++++++++++++++++++++---- src/stm32_can.cpp | 2 +- 7 files changed, 36 insertions(+), 12 deletions(-) diff --git a/include/fu.h b/include/fu.h index be93d4e..14fa292 100644 --- a/include/fu.h +++ b/include/fu.h @@ -26,7 +26,7 @@ class MotorVoltage { public: static void SetBoost(uint32_t boost); - static void SetWeakeningFrq(u32fp frq); + static void SetWeakeningFrq(float frq); static void SetMaxAmp(uint32_t maxAmp); static uint32_t GetAmp(u32fp frq); static uint32_t GetAmpPerc(u32fp frq, u32fp perc); diff --git a/include/my_math.h b/include/my_math.h index b419917..60f48f2 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -25,6 +25,7 @@ #define RAMPUP(current, target, rate) ((target < current || (current + rate) > target) ? target : current + rate) #define RAMPDOWN(current, target, rate) ((target > current || (current - rate) < target) ? target : current - rate) #define IIRFILTER(l,n,c) (((n) + ((l) << (c)) - (l)) >> (c)) +#define IIRFILTERF(l,n,c) (((n) + (l) * (1 << (c - 1))) / (1 << (c))) #define MEDIAN3(a,b,c) ((a) > (b) ? ((b) > (c) ? (b) : ((a) > (c) ? (c) : (a))) \ : ((a) > (c) ? (a) : ((b) > (c) ? (c) : (b)))) #define CHK_BIPOLAR_OFS(ofs) ((ofs < (2048 - 512)) || (ofs > (2048 + 512))) diff --git a/include/params.h b/include/params.h index 073fd57..bf174ee 100644 --- a/include/params.h +++ b/include/params.h @@ -63,10 +63,11 @@ namespace Param int Set(PARAM_NUM ParamNum, s32fp ParamVal); s32fp Get(PARAM_NUM ParamNum); int GetInt(PARAM_NUM ParamNum); - s32fp GetScl(PARAM_NUM ParamNum); + float GetFloat(PARAM_NUM ParamNum); bool GetBool(PARAM_NUM ParamNum); - void SetInt(PARAM_NUM ParamNum, int ParamVal); - void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal); + void SetInt(PARAM_NUM ParamNum, int ParamVal); + void SetFixed(PARAM_NUM ParamNum, s32fp ParamVal); + void SetFloat(PARAM_NUM ParamNum, float ParamVal); PARAM_NUM NumFromString(const char *name); PARAM_NUM NumFromId(uint32_t id); const Attributes *GetAttrib(PARAM_NUM ParamNum); diff --git a/src/fu.cpp b/src/fu.cpp index 525aab6..162571a 100644 --- a/src/fu.cpp +++ b/src/fu.cpp @@ -32,9 +32,9 @@ void MotorVoltage::SetBoost(uint32_t boost /**< amplitude in digit */) } /** Set frequency where the full amplitude is to be provided */ -void MotorVoltage::SetWeakeningFrq(u32fp frq) +void MotorVoltage::SetWeakeningFrq(float frq) { - endFrq = frq; + endFrq = FP_FROMFLT(frq); CalcFac(); } diff --git a/src/param_save.cpp b/src/param_save.cpp index b368086..f2b6844 100644 --- a/src/param_save.cpp +++ b/src/param_save.cpp @@ -115,7 +115,7 @@ int parm_load() Param::PARAM_NUM idx = Param::NumFromId(parmPage->data[idxPage].key); if (idx != Param::PARAM_INVALID && parmPage->data[idxPage].key > 0) { - Param::SetFlt(idx, parmPage->data[idxPage].value); + Param::SetFixed(idx, parmPage->data[idxPage].value); Param::SetFlagsRaw(idx, parmPage->data[idxPage].flags); } } diff --git a/src/params.cpp b/src/params.cpp index 7d3e015..83f3b40 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -82,7 +82,7 @@ s32fp Get(PARAM_NUM ParamNum) } /** -* Get a parameters unscaled digit value +* Get a parameters integer value * * @param[in] ParamNum Parameter index * @return Parameters value @@ -92,6 +92,17 @@ int GetInt(PARAM_NUM ParamNum) return FP_TOINT(values[ParamNum]); } +/** +* Get a parameters float value +* +* @param[in] ParamNum Parameter index +* @return Parameters value +*/ +float GetFloat(PARAM_NUM ParamNum) +{ + return ((float)values[ParamNum]) / FRAC_FAC; +} + /** * Get a parameters boolean value, 1.00=True * @@ -115,16 +126,27 @@ void SetInt(PARAM_NUM ParamNum, int ParamVal) } /** -* Set a parameters fixed point value +* Set a parameters fixed point value without range check and callback * * @param[in] ParamNum Parameter index * @param[in] ParamVal New value of parameter */ -void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal) +void SetFixed(PARAM_NUM ParamNum, s32fp ParamVal) { values[ParamNum] = ParamVal; } +/** +* Set a parameters floating point value without range check and callback +* +* @param[in] ParamNum Parameter index +* @param[in] ParamVal New value of parameter +*/ +void SetFloat(PARAM_NUM ParamNum, float ParamVal) +{ + values[ParamNum] = (s32fp)(ParamVal * FRAC_FAC); +} + /** * Get the paramater index from a parameter name * @@ -197,7 +219,7 @@ void LoadDefaults() for (int idx = 0; idx < PARAM_LAST; idx++, curAtr++) { if (curAtr->id > 0) - SetFlt((PARAM_NUM)idx, curAtr->def); + SetFixed((PARAM_NUM)idx, curAtr->def); } } diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index d041bf9..12fc710 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -479,7 +479,7 @@ void Can::HandleRx(int fifo) if (Param::IsParam((Param::PARAM_NUM)curPos->mapParam)) Param::Set((Param::PARAM_NUM)curPos->mapParam, val); else - Param::SetFlt((Param::PARAM_NUM)curPos->mapParam, val); + Param::SetFixed((Param::PARAM_NUM)curPos->mapParam, val); } lastRxTimestamp = rtc_get_counter_val(); } From a0fb57e2dba5001348464271254e076ce613fdc3 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Mon, 5 Jul 2021 12:57:06 +0100 Subject: [PATCH 54/83] C2000 compatibility changes params.h/params.cpp: C2000 doesn't have an 8-bit type so prefer an at-least 8- bit type for flags. This will be 16-bits on C2000 but 8-bits on other architectures. sine_core.h/sine_core.cpp: On C2000 an int is 16-bits and constants are ints unless otherwise specified. We specify constants as long where they need to be 32-bits. foc.cpp: FOC::fpsqrt() mixes signed and unsigned ints which upsets the C2000 compiler. Force it to only take signed fixed point values. --- include/params.h | 2 +- include/sine_core.h | 2 +- src/foc.cpp | 9 ++++++--- src/params.cpp | 9 +++++---- src/sine_core.cpp | 6 +++--- 5 files changed, 16 insertions(+), 12 deletions(-) diff --git a/include/params.h b/include/params.h index 8ce70d8..786ef6d 100644 --- a/include/params.h +++ b/include/params.h @@ -86,7 +86,7 @@ namespace Param const Attributes *GetAttrib(PARAM_NUM ParamNum); int IsParam(PARAM_NUM ParamNum); void LoadDefaults(); - void SetFlagsRaw(PARAM_NUM param, uint8_t rawFlags); + void SetFlagsRaw(PARAM_NUM param, uint_least8_t rawFlags); void SetFlag(PARAM_NUM param, PARAM_FLAG flag); void ClearFlag(PARAM_NUM param, PARAM_FLAG flag); PARAM_FLAG GetFlag(PARAM_NUM param); diff --git a/include/sine_core.h b/include/sine_core.h index 55f9c20..be50a22 100644 --- a/include/sine_core.h +++ b/include/sine_core.h @@ -51,7 +51,7 @@ class SineCore /* Domain of lookup function */ #define SINLU_ARGDIGITS 16 -#define SINLU_ONEREV (1U << SINLU_ARGDIGITS) +#define SINLU_ONEREV (1UL << SINLU_ARGDIGITS) #define SINTAB \ 0 ,\ diff --git a/src/foc.cpp b/src/foc.cpp index 45e034e..c825e46 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -17,10 +17,12 @@ * along with this program. If not, see . */ #define CST_DIGITS 15 +#define __STDC_LIMIT_MACROS // Needed for INT32_MAX #include "my_fp.h" #include "my_math.h" #include "foc.h" #include "sine_core.h" +#include #define SQRT3 FP_FROMFLT(1.732050807568877293527446315059) #define R1 FP_FROMFLT(0.03) @@ -34,7 +36,7 @@ static const s32fp fluxLinkage = FP_FROMFLT(0.09); static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-bit left shift because otherwise it can't be represented static const s32fp lqminusld = FP_FROMFLT(0.0058); -static const u32fp sqrt3 = SQRT3; +static const s32fp sqrt3 = SQRT3; static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) static const s32fp zeroOffset = FP_FROMINT(1); static const int32_t modMax = FP_DIV(FP_FROMINT(2U), sqrt3); @@ -158,8 +160,9 @@ uint32_t FOC::sqrt(uint32_t rad) u32fp FOC::fpsqrt(u32fp rad) { - u32fp sqrt = RADSTART(rad); - u32fp sqrtl; + rad = MAX(INT32_MAX,rad); + s32fp sqrt = RADSTART((s32fp)rad); + s32fp sqrtl; do { sqrtl = sqrt; diff --git a/src/params.cpp b/src/params.cpp index 7d3e015..14f525e 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -19,6 +19,7 @@ #include "params.h" #include "my_string.h" +#include namespace Param { @@ -43,7 +44,7 @@ static s32fp values[] = #define PARAM_ENTRY(category, name, unit, min, max, def, id) 0, #define VALUE_ENTRY(name, unit, id) 0, -static uint8_t flags[] = +static uint_least8_t flags[] = { PARAM_LIST }; @@ -201,19 +202,19 @@ void LoadDefaults() } } -void SetFlagsRaw(PARAM_NUM param, uint8_t rawFlags) +void SetFlagsRaw(PARAM_NUM param, uint_least8_t rawFlags) { flags[param] = rawFlags; } void SetFlag(PARAM_NUM param, PARAM_FLAG flag) { - flags[param] |= (uint8_t)flag; + flags[param] |= (uint_least8_t)flag; } void ClearFlag(PARAM_NUM param, PARAM_FLAG flag) { - flags[param] &= (uint8_t)~flag; + flags[param] &= (uint_least8_t)~flag; } PARAM_FLAG GetFlag(PARAM_NUM param) diff --git a/src/sine_core.cpp b/src/sine_core.cpp index 46a4cb4..180f5ad 100644 --- a/src/sine_core.cpp +++ b/src/sine_core.cpp @@ -22,10 +22,10 @@ #include "sine_core.h" #define SINTAB_ARGDIGITS 11 -#define SINTAB_ENTRIES (1 << SINTAB_ARGDIGITS) +#define SINTAB_ENTRIES (1UL << SINTAB_ARGDIGITS) /* Value range of sine lookup table */ -#define SINTAB_MAX (1 << BITS) -#define BRAD_PI (1 << (BITS - 1)) +#define SINTAB_MAX (1UL << BITS) +#define BRAD_PI (1UL << (BITS - 1)) #define PHASE_SHIFT90 ((uint32_t)( SINLU_ONEREV / 4)) #define PHASE_SHIFT120 ((uint32_t)( SINLU_ONEREV / 3)) From 41f90af238f42d8b1671520d8cb1bad968f5c852 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Mon, 5 Jul 2021 14:38:59 +0100 Subject: [PATCH 55/83] Fix printf to not use the legacy C register keyword Using the "register" keyword causes a warning when building on C++17 Linux host builds. Remove this redundant use as the optimiser on all our compilers make their own mind up anyway. Tests: - Build on all platforms and verify no warnings especially on x86_64 Linux. --- src/printf.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/printf.cpp b/src/printf.cpp index c9edcca..3d4e6af 100644 --- a/src/printf.cpp +++ b/src/printf.cpp @@ -56,11 +56,11 @@ class StringPutChar: public IPutChar static int prints(IPutChar* put, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; + int pc = 0, padchar = ' '; if (width > 0) { - register int len = 0; - register const char *ptr; + int len = 0; + const char *ptr; for (ptr = string; *ptr; ++ptr) ++len; if (len >= width) width = 0; else width -= len; @@ -90,9 +90,9 @@ static int prints(IPutChar* put, const char *string, int width, int pad) static int printi(IPutChar* put, int i, int b, int sg, int width, int pad, int letbase) { char print_buf[PRINT_BUF_LEN]; - register char *s; - register int t, neg = 0, pc = 0; - register unsigned int u = i; + char *s; + int t, neg = 0, pc = 0; + unsigned int u = i; if (i == 0) { print_buf[0] = '0'; @@ -141,8 +141,8 @@ static int printfp(IPutChar* put, int i, int width, int pad) static int print(IPutChar* put, const char *format, va_list args ) { - register int width, pad; - register int pc = 0; + int width, pad; + int pc = 0; char scr[2]; for (; *format != 0; ++format) { @@ -164,7 +164,7 @@ static int print(IPutChar* put, const char *format, va_list args ) width += *format - '0'; } if( *format == 's' ) { - register char *s = va_arg( args, char* ); + char *s = va_arg( args, char* ); pc += prints (put, s?s:"(null)", width, pad); continue; } From 812e14cb0d0660c3893c3e1c2e2d48679e569ec9 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sat, 31 Jul 2021 12:39:40 +0100 Subject: [PATCH 56/83] Use int32_t in params Remove all use of non-length specific int in favour of int32_t in the params class. This changes the return type of IsParam to be bool which matches its use in the rest of the code. Tests: - Build on all platforms and run unit tests on Linux --- include/params.h | 14 +++++++------- src/params.cpp | 8 +++----- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/include/params.h b/include/params.h index 786ef6d..daff4fc 100644 --- a/include/params.h +++ b/include/params.h @@ -60,10 +60,10 @@ namespace Param uint32_t id; } Attributes; - s32fp Get(PARAM_NUM ParamNum); - int GetInt(PARAM_NUM ParamNum); - s32fp GetScl(PARAM_NUM ParamNum); - bool GetBool(PARAM_NUM ParamNum); + s32fp Get(PARAM_NUM ParamNum); + int32_t GetInt(PARAM_NUM ParamNum); + s32fp GetScl(PARAM_NUM ParamNum); + bool GetBool(PARAM_NUM ParamNum); template T GetEnum(PARAM_NUM ParamNum) @@ -72,19 +72,19 @@ namespace Param } int Set(PARAM_NUM ParamNum, s32fp ParamVal); - void SetInt(PARAM_NUM ParamNum, int ParamVal); + void SetInt(PARAM_NUM ParamNum, int32_t ParamVal); void SetFlt(PARAM_NUM ParamNum, s32fp ParamVal); template void SetEnum(PARAM_NUM ParamNum, T ParamVal) { - SetInt(ParamNum, static_cast(ParamVal)); + SetInt(ParamNum, static_cast(ParamVal)); } PARAM_NUM NumFromString(const char *name); PARAM_NUM NumFromId(uint32_t id); const Attributes *GetAttrib(PARAM_NUM ParamNum); - int IsParam(PARAM_NUM ParamNum); + bool IsParam(PARAM_NUM ParamNum); void LoadDefaults(); void SetFlagsRaw(PARAM_NUM param, uint_least8_t rawFlags); void SetFlag(PARAM_NUM param, PARAM_FLAG flag); diff --git a/src/params.cpp b/src/params.cpp index 14f525e..37ea858 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -88,7 +88,7 @@ s32fp Get(PARAM_NUM ParamNum) * @param[in] ParamNum Parameter index * @return Parameters value */ -int GetInt(PARAM_NUM ParamNum) +int32_t GetInt(PARAM_NUM ParamNum) { return FP_TOINT(values[ParamNum]); } @@ -110,7 +110,7 @@ bool GetBool(PARAM_NUM ParamNum) * @param[in] ParamNum Parameter index * @param[in] ParamVal New value of parameter */ -void SetInt(PARAM_NUM ParamNum, int ParamVal) +void SetInt(PARAM_NUM ParamNum, int32_t ParamVal) { values[ParamNum] = FP_FROMINT(ParamVal); } @@ -182,10 +182,8 @@ const Attributes *GetAttrib(PARAM_NUM ParamNum) } /** Find out if ParamNum is a parameter or display value - * @retval 1 it is a parameter - * @retval 0 otherwise */ -int IsParam(PARAM_NUM ParamNum) +bool IsParam(PARAM_NUM ParamNum) { return attribs[ParamNum].min != attribs[ParamNum].max; } From 1568dd996676c4245950b3c09baf73d4591a6fde Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Thu, 5 Aug 2021 22:09:22 +0100 Subject: [PATCH 57/83] Fix integer overflow in FOC constant and sqrt calculation The modMax constant of 2/sqrt(3) in the FOC calculation has a signed integer overflow. This results in undefined behaviour and ends up with a different on C2000/x86_64 compared to ARM. The fix is to evaluate in an external calulator and store as a floating point constant similar to the other constants for FOC. The fpsqrt method was forcing all input values to be INT32_MAX rather than clamping values above this to INT32_MAX. Fixed by using MIN() rather than MAX(). Tests: - Run against new FOC test suite --- src/foc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index c825e46..d07f045 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -39,7 +39,7 @@ static const s32fp lqminusld = FP_FROMFLT(0.0058); static const s32fp sqrt3 = SQRT3; static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) static const s32fp zeroOffset = FP_FROMINT(1); -static const int32_t modMax = FP_DIV(FP_FROMINT(2U), sqrt3); +static const int32_t modMax = FP_FROMFLT(1.154700538379); // 2.0/sqrt(3.0); static const int32_t modMaxPow2 = modMax * modMax; static const int32_t minPulse = 1000; static const int32_t maxPulse = FP_FROMINT(2) - 1000; @@ -160,7 +160,7 @@ uint32_t FOC::sqrt(uint32_t rad) u32fp FOC::fpsqrt(u32fp rad) { - rad = MAX(INT32_MAX,rad); + rad = MIN(INT32_MAX,rad); s32fp sqrt = RADSTART((s32fp)rad); s32fp sqrtl; From 4e04f589147b9797d7afbfc16ceb41701c52f219 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Thu, 5 Aug 2021 22:30:51 +0100 Subject: [PATCH 58/83] Remove duplicate SQRT3 define Remove the SQRT3 #define in preference to the sqrt3 constant. This removes an unused symbol warning when building for C2000. Tests: - Build for all platforms - Verify unit tests pass apart from expected TestFocPwmGeneration.NormalModeRun and ManulModeRun tests --- src/foc.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index d07f045..7fb1853 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -24,7 +24,6 @@ #include "sine_core.h" #include -#define SQRT3 FP_FROMFLT(1.732050807568877293527446315059) #define R1 FP_FROMFLT(0.03) #define S1 FP_FROMFLT(0.15) #define R2 FP_FROMFLT(0.5) @@ -36,7 +35,7 @@ static const s32fp fluxLinkage = FP_FROMFLT(0.09); static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-bit left shift because otherwise it can't be represented static const s32fp lqminusld = FP_FROMFLT(0.0058); -static const s32fp sqrt3 = SQRT3; +static const s32fp sqrt3 = FP_FROMFLT(1.732050807568877293527446315059); static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) static const s32fp zeroOffset = FP_FROMINT(1); static const int32_t modMax = FP_FROMFLT(1.154700538379); // 2.0/sqrt(3.0); @@ -116,8 +115,8 @@ void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) s32fp ub = (cos * uq + sin * ud) >> CST_DIGITS; //Inverse Clarke transformation DutyCycles[0] = ua; - DutyCycles[1] = (-ua + FP_MUL(SQRT3, ub)) / 2; - DutyCycles[2] = (-ua - FP_MUL(SQRT3, ub)) / 2; + DutyCycles[1] = (-ua + FP_MUL(sqrt3, ub)) / 2; + DutyCycles[2] = (-ua - FP_MUL(sqrt3, ub)) / 2; int32_t offset = SineCore::CalcSVPWMOffset(DutyCycles[0], DutyCycles[1], DutyCycles[2]); From 7d1aa5b616f14b91407fed0331122cc0fc1e539a Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sun, 8 Aug 2021 14:36:56 +0100 Subject: [PATCH 59/83] Fix FOC FP constants to avoid overflow The foc module increases CST_DIGITS to 15. When using FP_FROMINT with constants this will cause the C2000 to experience an integer overflow as constants are int unless specified. Explicitly specify constants as long to avoid the overflow. This should cause any issues on 32-bit ARM and x86_64 systems. Tests: - Build on all platforms. Verify unit tests still pass on Linux. - No tests on C2000 yet. --- src/foc.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index 7fb1853..ceea516 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -37,11 +37,11 @@ static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-b static const s32fp lqminusld = FP_FROMFLT(0.0058); static const s32fp sqrt3 = FP_FROMFLT(1.732050807568877293527446315059); static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) -static const s32fp zeroOffset = FP_FROMINT(1); +static const s32fp zeroOffset = FP_FROMINT(1L); static const int32_t modMax = FP_FROMFLT(1.154700538379); // 2.0/sqrt(3.0); static const int32_t modMaxPow2 = modMax * modMax; static const int32_t minPulse = 1000; -static const int32_t maxPulse = FP_FROMINT(2) - 1000; +static const int32_t maxPulse = FP_FROMINT(2L) - 1000; s32fp FOC::id; s32fp FOC::iq; @@ -110,7 +110,9 @@ void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) s32fp sin = SineCore::Sine(angle); s32fp cos = SineCore::Cosine(angle); - //Inverse Park transformation + // Inverse Park transformation + // Note: Manually multiply and shift to do the fixed point maths to minimise + // precision loss in preference to using FP_MUL() s32fp ua = (cos * ud - sin * uq) >> CST_DIGITS; s32fp ub = (cos * uq + sin * ud) >> CST_DIGITS; //Inverse Clarke transformation @@ -133,7 +135,7 @@ void FOC::InvParkClarke(int32_t ud, int32_t uq, uint16_t angle) } else if (DutyCycles[i] > maxPulse) { - DutyCycles[i] = FP_FROMINT(2); + DutyCycles[i] = FP_FROMINT(2L); } } } From 1abdb8447c56b7ad55d38bf2ab9c9ef2b8597812 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sun, 8 Aug 2021 19:14:02 +0100 Subject: [PATCH 60/83] Convert int to int32_t in PiController Explicitly specify the size of the integer type used for calculations in the PiController. This doesn't matter for 32- bit ARM or x86_64 but does for the C2000 which will opt for it's default 16-bit int unless told otherwise. Tests: - Run unit tests and verify no failures - Run pwm simulator and verify output on C2000 is unaffected. It still differs from the Linux build signficantly though. --- include/picontroller.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/picontroller.h b/include/picontroller.h index 01afadc..035a9c3 100644 --- a/include/picontroller.h +++ b/include/picontroller.h @@ -32,14 +32,14 @@ class PiController * \param kp New value to set for proportional gain * \param ki New value for integral gain */ - void SetGains(int kp, int ki) + void SetGains(int32_t kp, int32_t ki) { this->kp = kp; this->ki = ki; } - void SetProportionalGain(int kp) { this->kp = kp; } - void SetIntegralGain(int ki) { this->ki = ki; } + void SetProportionalGain(int32_t kp) { this->kp = kp; } + void SetIntegralGain(int32_t ki) { this->ki = ki; } /** Set regulator target set point * \param val regulator target @@ -57,7 +57,7 @@ class PiController /** Set calling frequency * \param val New value to set */ - void SetCallingFrequency(int val) { frequency = val; } + void SetCallingFrequency(int32_t val) { frequency = val; } /** Run regulator to obtain a new actuator value * \param curVal currently measured value From c52b808aeedf55f5b9d18b03d337bfdcbd0c6861 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Mon, 9 Aug 2021 12:04:58 +0100 Subject: [PATCH 61/83] Prevent FP_FROMFLT() from overflowing on C2000 FOC The FP_FROMFLT() macro overflows when used with CST_DIGITS = 15 in the FOC module. This changes the FRAC_FAC to use a long constant to prevent overflow. Tests: - Verify in the C2000 debugger that FOC::GetMaximumModulationIndex() returns 37837 the same as on Linux --- include/my_fp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/my_fp.h b/include/my_fp.h index 7ad39a1..52b0303 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -27,7 +27,7 @@ #define CST_DIGITS FRAC_DIGITS #endif -#define FRAC_FAC (1 << CST_DIGITS) +#define FRAC_FAC (1L << CST_DIGITS) #define CST_CONVERT(a) ((a) << (CST_DIGITS - FRAC_DIGITS)) #define CST_ICONVERT(a) ((a) >> (CST_DIGITS - FRAC_DIGITS)) From 9dbc313824b43e3ce262b18dd1f4abd82dad5f86 Mon Sep 17 00:00:00 2001 From: johannes Date: Sat, 25 Sep 2021 00:24:13 +0200 Subject: [PATCH 62/83] Changed order of filters, prep for performance turning --- src/stm32_can.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index 12fc710..8859dd4 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -450,7 +450,7 @@ void Can::HandleRx(int fifo) while (can_receive(canDev, fifo, true, &id, &ext, &rtr, &fmi, &length, (uint8_t*)data, NULL) > 0) { //printf("fifo: %d, id: %x, len: %d, data[0]: %x, data[1]: %x\r\n", fifo, id, length, data[0], data[1]); - if (id == (0x600U + nodeId) && length == 8) //SDO request, nodeid=1 + if (id == (0x600U + nodeId) && length == 8) //SDO request { ProcessSDO(data); } @@ -602,14 +602,12 @@ void Can::SetFilterBank(int& idIndex, int& filterId, uint16_t* idList) void Can::ConfigureFilters() { uint16_t idList[IDS_PER_BANK] = { 0, 0, 0, 0 }; - int idIndex = 1; + int idIndex = 0; int filterId = canDev == CAN1 ? 0 : ((CAN_FMR(CAN2) >> 8) & 0x3F); - idList[0] = 0x600 + nodeId; - - for (int i = 0; i < nextUserMessageIndex; i++) + forEachCanMap(curMap, canRecvMap) { - idList[idIndex] = userIds[i]; + idList[idIndex] = curMap->canId; idIndex++; if (idIndex == IDS_PER_BANK) @@ -618,9 +616,9 @@ void Can::ConfigureFilters() } } - forEachCanMap(curMap, canRecvMap) + for (int i = 0; i < nextUserMessageIndex; i++) { - idList[idIndex] = curMap->canId; + idList[idIndex] = userIds[i]; idIndex++; if (idIndex == IDS_PER_BANK) @@ -628,11 +626,10 @@ void Can::ConfigureFilters() SetFilterBank(idIndex, filterId, idList); } } - //loop terminates before adding last set of filters - if (idIndex > 0) - { - SetFilterBank(idIndex, filterId, idList); - } + + idList[idIndex] = 0x600 + nodeId; + idIndex++; + SetFilterBank(idIndex, filterId, idList); } int Can::LoadFromFlash() From 952921dfab9f9bceb73b04f46879c2888b8a3ebf Mon Sep 17 00:00:00 2001 From: johannes Date: Wed, 29 Sep 2021 11:39:13 +0200 Subject: [PATCH 63/83] Undid formula correction. Might be more to the book but caused oscillation --- src/foc.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/foc.cpp b/src/foc.cpp index 54f5a6e..46a8eaf 100644 --- a/src/foc.cpp +++ b/src/foc.cpp @@ -32,7 +32,7 @@ static const s32fp fluxLinkage = FP_FROMFLT(0.09); static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage); -static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.03444736); //additional 10-bit left shift because otherwise it can't be represented +static const s32fp lqminusldSquaredBs10 = FP_FROMFLT(0.01722); //additional 10-bit left shift because otherwise it can't be represented static const s32fp lqminusld = FP_FROMFLT(0.0058); static const u32fp sqrt3 = SQRT3; static const s32fp sqrt3inv1 = FP_FROMFLT(0.57735026919); //1/sqrt(3) @@ -83,9 +83,8 @@ void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref) { int32_t isSquared = is * is; int32_t sign = is < 0 ? -1 : 1; - //factor of 8 has been incorporated into the right shift (7 instead of 10) - s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 7)); - idref = FP_TOINT(FP_DIV(fluxLinkage - term1, 4 * lqminusld)); + s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 10)); + idref = FP_TOINT(FP_DIV(fluxLinkage - term1, lqminusld)); iqref = sign * (int32_t)sqrt(isSquared - idref * idref); } From dd7d769bddcacf6fd4bc1af058938a99136e83c0 Mon Sep 17 00:00:00 2001 From: johannes Date: Tue, 5 Oct 2021 21:49:31 +0200 Subject: [PATCH 64/83] Fixed floating point IIR Filter formula --- include/my_math.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/my_math.h b/include/my_math.h index 60f48f2..64215a1 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -25,7 +25,7 @@ #define RAMPUP(current, target, rate) ((target < current || (current + rate) > target) ? target : current + rate) #define RAMPDOWN(current, target, rate) ((target > current || (current - rate) < target) ? target : current - rate) #define IIRFILTER(l,n,c) (((n) + ((l) << (c)) - (l)) >> (c)) -#define IIRFILTERF(l,n,c) (((n) + (l) * (1 << (c - 1))) / (1 << (c))) +#define IIRFILTERF(l,n,c) (((n) + (l) * ((1 << (c)) - 1)) / (1 << (c))) #define MEDIAN3(a,b,c) ((a) > (b) ? ((b) > (c) ? (b) : ((a) > (c) ? (c) : (a))) \ : ((a) > (c) ? (a) : ((b) > (c) ? (c) : (b)))) #define CHK_BIPOLAR_OFS(ofs) ((ofs < (2048 - 512)) || (ofs > (2048 + 512))) From bf44edbd0429a30ea4d461ec26b3cd098d2d3907 Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 8 Oct 2021 12:15:37 +0200 Subject: [PATCH 65/83] Removed register attribute in printf Allow redefining CAN limits in Makefile (max messages etc.) --- include/stm32_can.h | 17 +++++++++++++---- src/printf.cpp | 18 +++++++++--------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index 2ac7c21..af4716c 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -27,6 +27,19 @@ #define CAN_ERR_MAXMESSAGES -4 #define CAN_ERR_MAXITEMS -5 +#ifndef MAX_ITEMS_PER_MESSAGE +#define MAX_ITEMS_PER_MESSAGE 8 +#endif +#ifndef MAX_MESSAGES +#define MAX_MESSAGES 10 +#endif +#ifndef SENDBUFFER_LEN +#define SENDBUFFER_LEN 20 +#endif +#ifndef MAX_USER_MESSAGES +#define MAX_USER_MESSAGES 10 +#endif + class CANIDMAP; class SENDBUFFER; @@ -60,10 +73,6 @@ class Can static Can* GetInterface(int index); private: - static const int MAX_ITEMS_PER_MESSAGE = 8; - static const int MAX_MESSAGES = 10; - static const int SENDBUFFER_LEN = 20; - static const int MAX_USER_MESSAGES = 10; static volatile bool isSaving; struct CANPOS diff --git a/src/printf.cpp b/src/printf.cpp index 6667990..66050d3 100644 --- a/src/printf.cpp +++ b/src/printf.cpp @@ -56,11 +56,11 @@ class StringPutChar: public IPutChar static int prints(IPutChar* put, const char *string, int width, int pad) { - register int pc = 0, padchar = ' '; + int pc = 0, padchar = ' '; if (width > 0) { - register int len = 0; - register const char *ptr; + int len = 0; + const char *ptr; for (ptr = string; *ptr; ++ptr) ++len; if (len >= width) width = 0; else width -= len; @@ -90,9 +90,9 @@ static int prints(IPutChar* put, const char *string, int width, int pad) static int printi(IPutChar* put, int i, int b, int sg, int width, int pad, int letbase) { char print_buf[PRINT_BUF_LEN]; - register char *s; - register int t, neg = 0, pc = 0; - register unsigned int u = i; + char *s; + int t, neg = 0, pc = 0; + unsigned int u = i; if (i == 0) { print_buf[0] = '0'; @@ -141,8 +141,8 @@ static int printfp(IPutChar* put, int i, int width, int pad) static int print(IPutChar* put, const char *format, va_list args ) { - register int width, pad; - register int pc = 0; + int width, pad; + int pc = 0; char scr[2]; for (; *format != 0; ++format) { @@ -164,7 +164,7 @@ static int print(IPutChar* put, const char *format, va_list args ) width += *format - '0'; } if( *format == 's' ) { - register char *s = (char *)va_arg( args, int ); + char *s = (char *)va_arg( args, int ); pc += prints (put, s?s:"(null)", width, pad); continue; } From c045706e0be635465fb3896bdad9833905e8382a Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 8 Oct 2021 12:37:07 +0200 Subject: [PATCH 66/83] Added remap option to CAN constructor --- include/stm32_can.h | 2 +- src/stm32_can.cpp | 42 +++++++++++++++++++++++++++++++++++------- 2 files changed, 36 insertions(+), 8 deletions(-) diff --git a/include/stm32_can.h b/include/stm32_can.h index af4716c..8b92d12 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -51,7 +51,7 @@ class Can Baud250, Baud500, Baud800, Baud1000, BaudLast }; - Can(uint32_t baseAddr, enum baudrates baudrate); + Can(uint32_t baseAddr, enum baudrates baudrate, bool remap = false); void Clear(void); void SetBaudrate(enum baudrates baudrate); void Send(uint32_t canId, uint32_t data[2]) { Send(canId, data, 8); } diff --git a/src/stm32_can.cpp b/src/stm32_can.cpp index d041bf9..56a7ef3 100644 --- a/src/stm32_can.cpp +++ b/src/stm32_can.cpp @@ -294,7 +294,7 @@ int Can::Remove(Param::PARAM_NUM param) * \return void * */ -Can::Can(uint32_t baseAddr, enum baudrates baudrate) +Can::Can(uint32_t baseAddr, enum baudrates baudrate, bool remap) : lastRxTimestamp(0), sendCnt(0), recvCallback(DummyCallback), nextUserMessageIndex(0), canDev(baseAddr) { Clear(); @@ -303,11 +303,23 @@ Can::Can(uint32_t baseAddr, enum baudrates baudrate) switch (baseAddr) { case CAN1: - // Configure CAN pin: RX (input pull-up). - gpio_set_mode(GPIO_BANK_CAN1_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_RX); - gpio_set(GPIO_BANK_CAN1_RX, GPIO_CAN1_RX); - // Configure CAN pin: TX.- - gpio_set_mode(GPIO_BANK_CAN1_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_TX); + if (remap) + { + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN1_PB_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_PB_RX); + gpio_set(GPIO_BANK_CAN1_PB_RX, GPIO_CAN1_PB_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN1_PB_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_PB_TX); + } + else + { + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN1_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN1_RX); + gpio_set(GPIO_BANK_CAN1_RX, GPIO_CAN1_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN1_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN1_TX); + } + //CAN1 RX and TX IRQs nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ); //CAN RX nvic_set_priority(NVIC_USB_LP_CAN_RX0_IRQ, 0xf << 4); //lowest priority @@ -318,7 +330,23 @@ Can::Can(uint32_t baseAddr, enum baudrates baudrate) interfaces[0] = this; break; case CAN2: - // Configure CAN pin: RX (input pull-up). + if (remap) + { + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN2_RE_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN2_RE_RX); + gpio_set(GPIO_BANK_CAN2_RE_RX, GPIO_CAN2_RE_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN2_RE_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN2_RE_TX); + } + else + { + // Configure CAN pin: RX (input pull-up). + gpio_set_mode(GPIO_BANK_CAN2_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN2_RX); + gpio_set(GPIO_BANK_CAN2_RX, GPIO_CAN2_RX); + // Configure CAN pin: TX.- + gpio_set_mode(GPIO_BANK_CAN2_TX, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_CAN2_TX); + } + // Configure CAN pin: RX (input pull-up). gpio_set_mode(GPIO_BANK_CAN2_RX, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, GPIO_CAN2_RX); gpio_set(GPIO_BANK_CAN2_RX, GPIO_CAN2_RX); // Configure CAN pin: TX.- From a00f1f0cab76d8223733f6d90dd5aa9a61ac76af Mon Sep 17 00:00:00 2001 From: johannes Date: Tue, 12 Oct 2021 11:22:21 +0200 Subject: [PATCH 67/83] Added FP_TOFLOAT macro --- include/my_fp.h | 1 + src/params.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/my_fp.h b/include/my_fp.h index 3d63e55..1d88a67 100644 --- a/include/my_fp.h +++ b/include/my_fp.h @@ -35,6 +35,7 @@ #define UTOA_FRACDEC 100 #define FP_DECIMALS 2 +#define FP_TOFLOAT(a) (((float)a) / FRAC_FAC) #define FP_FROMINT(a) ((s32fp)((a) << CST_DIGITS)) #define FP_TOINT(a) ((s32fp)((a) >> CST_DIGITS)) #define FP_FROMFLT(a) ((s32fp)((a) * FRAC_FAC)) diff --git a/src/params.cpp b/src/params.cpp index 83f3b40..ec5a158 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -100,7 +100,7 @@ int GetInt(PARAM_NUM ParamNum) */ float GetFloat(PARAM_NUM ParamNum) { - return ((float)values[ParamNum]) / FRAC_FAC; + return FP_TOFLOAT(values[ParamNum]); } /** @@ -144,7 +144,7 @@ void SetFixed(PARAM_NUM ParamNum, s32fp ParamVal) */ void SetFloat(PARAM_NUM ParamNum, float ParamVal) { - values[ParamNum] = (s32fp)(ParamVal * FRAC_FAC); + values[ParamNum] = FP_FROMFLT(ParamVal); } /** From af46faa737f0f6e10821d61cbe57bc1474605760 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Sun, 31 Oct 2021 16:59:19 +0000 Subject: [PATCH 68/83] Make CRC8 work on the C2000 architecture With the C2000 doesn't support uint8_t. Crudely expand the crc8 implementation to use uint16_t. Mask of out of range inputs so only the lower 8-bits of the uint16_t ever contain data. Tests: - Add unit tests in the stm32_sine project to test --- CMakeLists.txt | 3 ++- include/crc8.h | 10 ++++------ src/crc8.cpp | 4 ++-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a9e90fb..44a2f7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,8 @@ add_library( src/params.cpp src/picontroller.cpp src/printf.cpp - src/sine_core.cpp) + src/sine_core.cpp + src/crc8.cpp) target_link_libraries(libopeninv global_options) target_include_directories(libopeninv PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") diff --git a/include/crc8.h b/include/crc8.h index a475cb8..f0a0fbc 100644 --- a/include/crc8.h +++ b/include/crc8.h @@ -24,7 +24,7 @@ /* * Precalculated look up table for CRC calculation */ -extern const uint8_t crc_table[256]; +extern const uint16_t crc_table[256]; /** * \brief Calculate 8-bit CRC @@ -39,7 +39,7 @@ extern const uint8_t crc_table[256]; * * \return Calculated CRC value */ -inline uint8_t crc8(uint8_t* p, uint8_t len, uint8_t crc) +inline uint16_t crc8(uint16_t* p, uint16_t len, uint16_t crc) { while (len--) { @@ -60,11 +60,9 @@ inline uint8_t crc8(uint8_t* p, uint8_t len, uint8_t crc) * * \return Calculated CRC value */ -inline uint8_t crc8(uint8_t input, uint8_t crc) +inline uint16_t crc8(uint16_t input, uint16_t crc) { - crc = crc_table[crc ^ input]; - - return crc; + return crc_table[(crc & 0xFF) ^ (input & 0xFF)]; } #endif /* __CRC8_H_ */ diff --git a/src/crc8.cpp b/src/crc8.cpp index aaf9ee0..348056c 100644 --- a/src/crc8.cpp +++ b/src/crc8.cpp @@ -29,7 +29,7 @@ * Just change this define to whatever polynomial is in use * A polynomial of 0x07 corresponds to x^8 + x^2 + x + 1 */ -#define CRC1B(b) ((uint8_t)((b) << 1) ^ ((b)&0x80 ? 0x07 : 0)) // MS first +#define CRC1B(b) ((uint16_t)(((b) << 1) ^ ((b)&0x80 ? 0x07 : 0)) & 0xFF) // MS first /* * 8+1 entry enum lookup table define @@ -71,4 +71,4 @@ enum * This is the final lookup table. It is rough on the compiler, but generates * the required lookup table automagically at compile time. */ -const uint8_t crc_table[256] = { 0, CRCTAB6() CRC(0x80), CRCTAB6(^CRC(0x80)) }; +const uint16_t crc_table[256] = { 0, CRCTAB6() CRC(0x80), CRCTAB6(^CRC(0x80)) }; From a3e4923bb6b1d926051f94a49e3b3668660d929f Mon Sep 17 00:00:00 2001 From: johannes Date: Sun, 31 Oct 2021 18:07:03 +0100 Subject: [PATCH 69/83] Added Send() function for byte array --- include/stm32_can.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/stm32_can.h b/include/stm32_can.h index 8b92d12..2a42c84 100644 --- a/include/stm32_can.h +++ b/include/stm32_can.h @@ -55,6 +55,7 @@ class Can void Clear(void); void SetBaudrate(enum baudrates baudrate); void Send(uint32_t canId, uint32_t data[2]) { Send(canId, data, 8); } + void Send(uint32_t canId, uint8_t data[8], uint8_t len) { Send(canId, (uint32_t*)data, len); } void Send(uint32_t canId, uint32_t data[2], uint8_t len); void SendAll(); void SDOWrite(uint8_t remoteNodeId, uint16_t index, uint8_t subIndex, uint32_t data); From 7476273983217a2a49f96694181ebba7a71f2c17 Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 19 Nov 2021 20:58:08 +0100 Subject: [PATCH 70/83] Put param change callback in Param namespace --- include/params.h | 8 ++++---- src/params.cpp | 2 +- src/terminalcommands.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/params.h b/include/params.h index bf174ee..fb70a60 100644 --- a/include/params.h +++ b/include/params.h @@ -60,7 +60,7 @@ namespace Param uint32_t id; } Attributes; - int Set(PARAM_NUM ParamNum, s32fp ParamVal); + int Set(PARAM_NUM ParamNum, s32fp ParamVal); s32fp Get(PARAM_NUM ParamNum); int GetInt(PARAM_NUM ParamNum); float GetFloat(PARAM_NUM ParamNum); @@ -77,9 +77,9 @@ namespace Param void SetFlag(PARAM_NUM param, PARAM_FLAG flag); void ClearFlag(PARAM_NUM param, PARAM_FLAG flag); PARAM_FLAG GetFlag(PARAM_NUM param); -} -//User defined callback -extern void parm_Change(Param::PARAM_NUM ParamNum); + //User defined callback + void Change(Param::PARAM_NUM ParamNum); +} #endif //PARAM_H_INCLUDED diff --git a/src/params.cpp b/src/params.cpp index ec5a158..e35e2ed 100644 --- a/src/params.cpp +++ b/src/params.cpp @@ -64,7 +64,7 @@ int Set(PARAM_NUM ParamNum, s32fp ParamVal) if (ParamVal >= attribs[ParamNum].min && ParamVal <= attribs[ParamNum].max) { values[ParamNum] = ParamVal; - parm_Change(ParamNum); + Change(ParamNum); res = 0; } return res; diff --git a/src/terminalcommands.cpp b/src/terminalcommands.cpp index 30db61c..1396b8a 100644 --- a/src/terminalcommands.cpp +++ b/src/terminalcommands.cpp @@ -396,7 +396,7 @@ void TerminalCommands::LoadParameters(Terminal* term, char *arg) arg = arg; if (0 == parm_load()) { - parm_Change((Param::PARAM_NUM)0); + Param::Change((Param::PARAM_NUM)0); fprintf(term, "Parameters loaded\r\n"); } else From 1dcd5b6ed4eb5e0396628eee3d3947ded4be9946 Mon Sep 17 00:00:00 2001 From: johannes Date: Fri, 5 Nov 2021 11:17:16 +0100 Subject: [PATCH 71/83] removed volatile --- src/anain.cpp | 2 +- src/stm32scheduler.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/anain.cpp b/src/anain.cpp index 0ede62c..8136a1c 100644 --- a/src/anain.cpp +++ b/src/anain.cpp @@ -48,7 +48,7 @@ void AnaIn::Start() adc_power_on(ADC1); /* wait for adc starting up*/ - for (volatile int i = 0; i < 80000; i++); + for (int i = 0; i < 80000; i++); adc_reset_calibration(ADC1); adc_calibrate(ADC1); diff --git a/src/stm32scheduler.cpp b/src/stm32scheduler.cpp index 78e1371..150d2c1 100644 --- a/src/stm32scheduler.cpp +++ b/src/stm32scheduler.cpp @@ -19,7 +19,7 @@ #include "stm32scheduler.h" /* return CCRc of TIMt */ -#define TIM_CCR(t,c) (*(volatile uint32_t *)(&TIM_CCR1(t) + (c))) +#define TIM_CCR(t,c) (*(uint32_t *)(&TIM_CCR1(t) + (c))) const enum tim_oc_id Stm32Scheduler::ocMap[MAX_TASKS] = { TIM_OC1, TIM_OC2, TIM_OC3, TIM_OC4 }; From fe1fa55c9b85eade3703fdaf332022d5fb48954f Mon Sep 17 00:00:00 2001 From: johannes Date: Thu, 16 Dec 2021 09:53:01 +0100 Subject: [PATCH 72/83] Output nodeid at startup if terminal is disabled --- src/terminal.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/terminal.cpp b/src/terminal.cpp index e1b331d..4b430f3 100644 --- a/src/terminal.cpp +++ b/src/terminal.cpp @@ -169,11 +169,15 @@ void Terminal::Run() void Terminal::SetNodeId(uint8_t id) { char one[] = { '1', 0 }; + char buf[4]; nodeId = id; if (nodeId != 1) { - Send("Disabling terminal, type 'enableuart ' to re-enable\r\n"); + Send("Disabling terminal, type 'enableuart "); + my_ltoa(buf, id, 10); + Send(buf); + Send("' to re-enable\r\n"); } EnableUart(one); From ca7ecf82d54b9c2e7eb1af525cc6e52e702ae447 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 27 Dec 2021 21:14:15 +0100 Subject: [PATCH 73/83] Use RX DMA for LIN reception, would loose frames otherwise --- include/linbus.h | 11 ++++++----- src/linbus.cpp | 48 +++++++++++++++++++++++------------------------- 2 files changed, 29 insertions(+), 30 deletions(-) diff --git a/include/linbus.h b/include/linbus.h index f24e933..f7a4981 100644 --- a/include/linbus.h +++ b/include/linbus.h @@ -25,10 +25,9 @@ class LinBus public: /** Default constructor */ LinBus(uint32_t usart, int baudrate); - void Send(uint8_t id, uint8_t* data, uint8_t len); - void Receive(); + void Request(uint8_t id, uint8_t* data, uint8_t len); bool HasReceived(uint8_t pid, uint8_t requiredLen); - uint8_t* GetReceivedBytes() { return &recvBuffer[2]; } + uint8_t* GetReceivedBytes() { return &recvBuffer[payloadIndex]; } protected: @@ -37,6 +36,7 @@ class LinBus { uint32_t usart; uint8_t dmatx; + uint8_t dmarx; uint32_t port; uint16_t pin; }; @@ -45,11 +45,12 @@ class LinBus static uint8_t Parity(uint8_t id); static const HwInfo hwInfo[]; + static const int payloadIndex = 3; + static const int pidIndex = 2; uint32_t usart; const HwInfo* hw; uint8_t sendBuffer[11]; - uint8_t recvBuffer[11]; - uint8_t receiveIdx; + uint8_t recvBuffer[12]; }; #endif // LINBUS_H diff --git a/src/linbus.cpp b/src/linbus.cpp index ab7b2cb..adcd978 100644 --- a/src/linbus.cpp +++ b/src/linbus.cpp @@ -25,9 +25,9 @@ const LinBus::HwInfo LinBus::hwInfo[] = { - { USART1, DMA_CHANNEL4, GPIOA, GPIO_USART1_TX }, - { USART2, DMA_CHANNEL7, GPIOA, GPIO_USART2_TX }, - { USART3, DMA_CHANNEL2, GPIOB, GPIO_USART3_TX }, + { USART1, DMA_CHANNEL4, DMA_CHANNEL5, GPIOA, GPIO_USART1_TX }, + { USART2, DMA_CHANNEL7, DMA_CHANNEL6, GPIOA, GPIO_USART2_TX }, + { USART3, DMA_CHANNEL2, DMA_CHANNEL3, GPIOB, GPIO_USART3_TX }, }; @@ -58,6 +58,7 @@ LinBus::LinBus(uint32_t usart, int baudrate) usart_set_flow_control(usart, USART_FLOWCONTROL_NONE); USART_CR2(usart) |= USART_CR2_LINEN; usart_enable_tx_dma(usart); + usart_enable_rx_dma(usart); dma_channel_reset(DMA1, hw->dmatx); dma_set_read_from_memory(DMA1, hw->dmatx); @@ -67,6 +68,12 @@ LinBus::LinBus(uint32_t usart, int baudrate) dma_set_memory_size(DMA1, hw->dmatx, DMA_CCR_MSIZE_8BIT); dma_enable_memory_increment_mode(DMA1, hw->dmatx); + dma_channel_reset(DMA1, hw->dmarx); + dma_set_peripheral_address(DMA1, hw->dmarx, (uint32_t)&USART_DR(usart)); + dma_set_peripheral_size(DMA1, hw->dmarx, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, hw->dmarx, DMA_CCR_MSIZE_8BIT); + dma_enable_memory_increment_mode(DMA1, hw->dmarx); + usart_enable(usart); } @@ -77,7 +84,7 @@ LinBus::LinBus(uint32_t usart, int baudrate) * \param len length of payload, if any * */ -void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) +void LinBus::Request(uint8_t id, uint8_t* data, uint8_t len) { int sendLen = len == 0 ? 2 : len + 3; @@ -85,6 +92,9 @@ void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) dma_disable_channel(DMA1, hw->dmatx); dma_set_number_of_data(DMA1, hw->dmatx, sendLen); + dma_disable_channel(DMA1, hw->dmarx); + dma_set_memory_address(DMA1, hw->dmarx, (uint32_t)recvBuffer); + dma_set_number_of_data(DMA1, hw->dmarx, sizeof(recvBuffer)); sendBuffer[0] = 0x55; //Sync sendBuffer[1] = Parity(id); @@ -98,24 +108,7 @@ void LinBus::Send(uint8_t id, uint8_t* data, uint8_t len) USART_CR1(usart) |= USART_CR1_SBK; dma_enable_channel(DMA1, hw->dmatx); -} - -/** \brief Check if a break or character was received and store it, if yes - */ -void LinBus::Receive() -{ - if (receiveIdx == sizeof(recvBuffer)) return; - - if (usart_get_flag(usart, USART_SR_LBD)) - { - receiveIdx = 0; - USART_SR(usart) &= ~USART_SR_LBD; - } - else if (usart_get_flag(usart, USART_SR_RXNE)) - { - recvBuffer[receiveIdx] = usart_recv(usart); - receiveIdx++; - } + dma_enable_channel(DMA1, hw->dmarx); } /** \brief Check whether we received valid data with given PID and length @@ -127,13 +120,18 @@ void LinBus::Receive() */ bool LinBus::HasReceived(uint8_t id, uint8_t requiredLen) { + int numRcvd = dma_get_number_of_data(DMA1, hw->dmarx); + int receiveIdx = sizeof(recvBuffer) - numRcvd; + if (requiredLen > 8) return false; - if (receiveIdx == (requiredLen + 2) && recvBuffer[1] == Parity(id)) + uint8_t pid = Parity(id); + + if (receiveIdx == (requiredLen + payloadIndex + 1) && recvBuffer[pidIndex] == pid) { - uint8_t checksum = Checksum(recvBuffer[1], &recvBuffer[2], requiredLen); + uint8_t checksum = Checksum(recvBuffer[pidIndex], &recvBuffer[payloadIndex], requiredLen); - return checksum == recvBuffer[requiredLen + 2]; + return checksum == recvBuffer[requiredLen + payloadIndex]; } return false; From e4483cea2ad9392604e671abb5fa6fd07bff54d3 Mon Sep 17 00:00:00 2001 From: johannes Date: Mon, 27 Dec 2021 21:16:41 +0100 Subject: [PATCH 74/83] Corrected ABS macro --- include/my_math.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/my_math.h b/include/my_math.h index 64215a1..59db436 100644 --- a/include/my_math.h +++ b/include/my_math.h @@ -19,7 +19,7 @@ #ifndef MY_MATH_H_INCLUDED #define MY_MATH_H_INCLUDED -#define ABS(a) ((a) < 0?(-a) : (a)) +#define ABS(a) ((a) < 0?(-(a)) : (a)) #define MIN(a,b) ((a) < (b)?(a):(b)) #define MAX(a,b) ((a) > (b)?(a):(b)) #define RAMPUP(current, target, rate) ((target < current || (current + rate) > target) ? target : current + rate) From 5b943d33370186ef731e0038b25e06520aa777c0 Mon Sep 17 00:00:00 2001 From: Bernd Ocklin Date: Tue, 8 Feb 2022 10:15:56 +0100 Subject: [PATCH 75/83] Port pmic driver and add scheduler for it - move watchdog pmic driver to inverter lib - make a template off it for easier test/mock - break out specific platform dependent SPI class - add a small scheduler class used to strobe watchdog - add a small test program targeting inverter board --- platform/c2000/CMakeLists.txt | 3 + .../c2000/device_support/include/device.h | 16 + .../c2000/inverter_drivers/CMakeLists.txt | 10 +- .../inverter_drivers/c2000/pmicspidriver.h | 41 ++ .../c2000/inverter_drivers/c2000/scheduler.h | 48 ++ platform/c2000/inverter_drivers/pmicdriver.h | 433 ++++++++++++++++++ .../c2000/inverter_drivers/pmicspidriver.cpp | 91 ++++ platform/c2000/inverter_drivers/scheduler.cpp | 153 +++++++ platform/c2000/pmicdrivertest/CMakeLists.txt | 3 + .../c2000/pmicdrivertest/pmicdrivertest.cpp | 124 +++++ src/teslam3pmic.cpp | 353 -------------- 11 files changed, 921 insertions(+), 354 deletions(-) create mode 100644 platform/c2000/inverter_drivers/c2000/pmicspidriver.h create mode 100644 platform/c2000/inverter_drivers/c2000/scheduler.h create mode 100644 platform/c2000/inverter_drivers/pmicdriver.h create mode 100644 platform/c2000/inverter_drivers/pmicspidriver.cpp create mode 100644 platform/c2000/inverter_drivers/scheduler.cpp create mode 100644 platform/c2000/pmicdrivertest/CMakeLists.txt create mode 100644 platform/c2000/pmicdrivertest/pmicdrivertest.cpp delete mode 100644 src/teslam3pmic.cpp diff --git a/platform/c2000/CMakeLists.txt b/platform/c2000/CMakeLists.txt index d80dfa8..8802fb7 100644 --- a/platform/c2000/CMakeLists.txt +++ b/platform/c2000/CMakeLists.txt @@ -16,5 +16,8 @@ add_subdirectory(inverter) # Tesla M3 Gate Driver and PSU test application add_subdirectory(gatedrivertest) +# Tesla M3 PMIC Driver test +add_subdirectory(pmicdrivertest) + # Dump Tesla M3 EEPROM add_subdirectory(dumpeeprom) diff --git a/platform/c2000/device_support/include/device.h b/platform/c2000/device_support/include/device.h index 04122c7..ab10e80 100644 --- a/platform/c2000/device_support/include/device.h +++ b/platform/c2000/device_support/include/device.h @@ -139,6 +139,22 @@ a single CPU should be defined." #define DEVICE_TESLAM3_GATE_SPI SPIC_BASE + +// Tesla Model 3 Inverter Pmic Driver (TLF35584) +// Pins 8, 9, 10, 12 +// GPIOs 16, 17, 18, 19 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI 16 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO 17 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK 18 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CS 19 + +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI GPIO_16_SPISIMOA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO GPIO_17_SPISOMIA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK GPIO_18_SPICLKA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CS GPIO_19_SPISTEA + +#define DEVICE_TESLAM3_PMIC_SPI SPIA_BASE + //***************************************************************************** // // Defines related to clock configuration diff --git a/platform/c2000/inverter_drivers/CMakeLists.txt b/platform/c2000/inverter_drivers/CMakeLists.txt index 2624755..9c84b93 100644 --- a/platform/c2000/inverter_drivers/CMakeLists.txt +++ b/platform/c2000/inverter_drivers/CMakeLists.txt @@ -1,4 +1,12 @@ -add_library(inverter_drivers eeprom.cpp current.cpp encoder.cpp pwmdriver.cpp gatedriver.cpp gatedriverinterface.cpp) +add_library(inverter_drivers + eeprom.cpp + current.cpp + encoder.cpp + pmicspidriver.cpp + pwmdriver.cpp + gatedriver.cpp + gatedriverinterface.cpp + scheduler.cpp) target_link_libraries(inverter_drivers global_options libopeninv device_support) target_include_directories(inverter_drivers PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}") diff --git a/platform/c2000/inverter_drivers/c2000/pmicspidriver.h b/platform/c2000/inverter_drivers/c2000/pmicspidriver.h new file mode 100644 index 0000000..e75f642 --- /dev/null +++ b/platform/c2000/inverter_drivers/c2000/pmicspidriver.h @@ -0,0 +1,41 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2022 Bernd Ocklin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef C2000SPIDRIVER_H +#define C2000SPIDRIVER_H + +#include "stdint.h" + +class PmicSpiDriver { + +private: + static uint32_t m_base; + + static void InitGPIO(uint16_t, uint32_t); + +public: + static void InitGPIOs(); + static void InitSPIPort(); + + static void WriteData(uint16_t data); + static uint16_t ReadData(); +}; + +#endif // C2000SPIDRIVER_H + diff --git a/platform/c2000/inverter_drivers/c2000/scheduler.h b/platform/c2000/inverter_drivers/c2000/scheduler.h new file mode 100644 index 0000000..4d8d45f --- /dev/null +++ b/platform/c2000/inverter_drivers/c2000/scheduler.h @@ -0,0 +1,48 @@ +/* + * This file is part of the Tesla M3 OSS Inverter project. + * + * Copyright (C) 2022 Bernd Ocklin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef SCHEDULER_H +#define SCHEDULER_H + +#include + +class Scheduler { + + // making these public to allow extern "C" interrupt handlers to reach them +public: + + static const uint16_t maxTasks = 3; + + static uint32_t periods[maxTasks]; + + static void (*functions[maxTasks]) (void); + +public: + // init the scheduler / timer + static void Init(); + + // add a task with a specific period */ + static void AddTask(void (*function)(void), uint32_t msPeriod); + +private: + static void InitCPUTimers(); + static void ConfigureCPUTimer(uint32_t cpuTimer, uint32_t msPeriod); +}; + +#endif // SCHEDULER_H diff --git a/platform/c2000/inverter_drivers/pmicdriver.h b/platform/c2000/inverter_drivers/pmicdriver.h new file mode 100644 index 0000000..acd57b5 --- /dev/null +++ b/platform/c2000/inverter_drivers/pmicdriver.h @@ -0,0 +1,433 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2021 David J. Fiddes + * Copyright (C) 2022 Bernd Ocklin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef TESLAM3PMIC_H +#define TESLAM3PMIC_H + +#include + +#include "hw/tlf35584_safety_psu.h" + +namespace c2000 { +struct Register +{ + uint16_t reg; + uint16_t value; +}; + +/** + * \brief TLF35584 safety power supply and watchdog register set up sequence + * + */ +static const Register RegisterConfig[14] = { + { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY1 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY2 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY3 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY4 }, + { TLF35584_WDCFG1, TLF35584_WDCFG1_WDSLPEN | TLF35584_WDCFG1_FWDETHR(14) }, + { TLF35584_WDCFG0, + TLF35584_WDCFG0_WWDETHR(14) | TLF35584_WDCFG0_WWDEN | + TLF35584_WDCFG0_FWDEN | TLF35584_WDCFG0_WWDTSEL | + TLF35584_WDCFG0_WDCYC_1MS }, + { TLF35584_SYSPCFG1, + TLF35584_SYSPCFG1_SS2DEL_0MS | TLF35584_SYSPCFG1_ERRREC_1MS }, + { TLF35584_FWDCFG, TLF35584_FWDCFG_WDHBTP_CYCLES(250) }, + { TLF35584_WWDCFG0, TLF35584_WWDCFG0_CW_CYCLES(50) }, + { TLF35584_WWDCFG1, TLF35584_WWDCFG1_OW_CYCLES(100) }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY1 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY2 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY3 }, + { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY4 } +}; + +static const uint16_t RegisterConfigSize = + sizeof(RegisterConfig) / sizeof(RegisterConfig[0]); + +/** + * Functional Watchdog response sequence structure + */ +struct WatchdogResponse +{ + uint16_t resp3; + uint16_t resp2; + uint16_t resp1; + uint16_t resp0; +}; + +/** + * Canned Functional Watchdog response sequences from Table 26 in Section 15.3 + * functional Watchdog of the TLF35584 datasheet + */ +static const WatchdogResponse WatchdogResponses[0x10] = { + // clang-format off + { 0xFF, 0x0F, 0xF0, 0x00 }, + { 0xB0, 0x40, 0xBF, 0x4F }, + { 0xE9, 0x19, 0xE6, 0x16 }, + { 0xA6, 0x56, 0xA9, 0x59 }, + { 0x75, 0x85, 0x7A, 0x8A }, + { 0x3A, 0xCA, 0x35, 0xC5 }, + { 0x63, 0x93, 0x6C, 0x9C }, + { 0x2C, 0xDC, 0x23, 0xD3 }, + { 0xD2, 0x22, 0xDD, 0x2D }, + { 0x9D, 0x6D, 0x92, 0x62 }, + { 0xC4, 0x34, 0xCB, 0x3B }, + { 0x8B, 0x7B, 0x84, 0x74 }, + { 0x58, 0xA8, 0x57, 0xA7 }, + { 0x17, 0xE7, 0x18, 0xE8 }, + { 0x4E, 0xBE, 0x41, 0xB1 }, + { 0x01, 0xF1, 0x0E, 0xFE }, + // clang-format on +}; + +static const uint16_t WatchdogResponsesSize = + sizeof(WatchdogResponses) / sizeof(WatchdogResponses[0]); + +static const int StateTransitionDelay = 100; // uS + + +/** + * \brief Verify that function x didn't fail. Return immediately if it did. + * Assumes a local variable "result" of type TeslaM3PowerWatchdog::Error + */ +#define CHECK(x) \ + if ((result = x) != OK) \ + { \ + return result; \ + } + +/** + * \brief Define a scoped SPI transaction that manually asserts the ~CS line for + * the duration of a SPI transaction. + */ +struct SPITransaction +{ + SPITransaction() + { + // gpio_clear(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); + } + + ~SPITransaction() + { + // gpio_set(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); + } +}; + + + +template +class TeslaM3PowerWatchdog +{ +public: + enum Error + { + OK = 0, + WriteFail, + ReadParityFail, + StateTransitionFail + }; + +public: + /** + * \brief Set up the power management watchdog + * + * \return Error - Error code if initialisation failed otherwise Error::OK + */ + static Error Init() + { + SpiDriverT::InitGPIOs(); + SpiDriverT::InitSPIPort(); + return SetupPowerManagement(); + } + + /** + * \brief Combined window and functional strobe during normal run + * Alternating functional watchdog between fetching quest and answering it. + * + * Window Watchdog is defined with a 100ms period + * Functional Watchdog with 250ms. + * + * Thus a functional watchdog quest only needs to be answered every second WW period. + */ + static Error Strobe() + { + Error result; + + CHECK(TeslaM3PowerWatchdog::StrobeWindowWatchdog()); + + if(lastFunctionalWatchdogQuest & TLF35584_FWDSTAT0_FWDQUEST_MASK) + { + CHECK(TeslaM3PowerWatchdog::StrobeFunctionalWatchdog()); + + } else { + + CHECK(TeslaM3PowerWatchdog::FunctionalWatchdogReadQuest()); + } + + return OK; + } + +private: + + // last quest is kept track of as it is read at a different time than the response + static uint16_t lastFunctionalWatchdogQuest; + + /** + * \brief Strobe the window watchdog - this is on a 100ms cycle typically + * + * \return Error - Error code if stobe failed otherwise Error::OK + */ + static Error StrobeWindowWatchdog() + { + Error result; + + uint16_t windowStatus; + CHECK(ReadRegister(TLF35584_WWDSCMD, windowStatus)); + + // Invert the window watchdog status and write back in the lowest bit + CHECK(WriteRegister( + TLF35584_WWDSCMD, + windowStatus & TLF35584_WWDSCMD_TRIG_STATUS ? 0 : + TLF35584_WWDSCMD_TRIG)); + + return OK; + } + + + /** + * \brief Read the quest of functional watchdog + * - this is on a 200ms cycle typically + * - done together with every other strobing the window watchdog + * + * \return Error - Error code if stobe failed otherwise Error::OK + */ + static Error FunctionalWatchdogReadQuest() { + + Error result; + + uint16_t functionalStatus; + CHECK(ReadRegister(TLF35584_FWDSTAT0, functionalStatus)); + + // Determine which response the functional watchdog is expecting from us + TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest = + functionalStatus & TLF35584_FWDSTAT0_FWDQUEST_MASK; + + return OK; + } + + /** + * \brief Answer the functional watchdog quest - this is on a 200ms cycle typically + * + * \return Error - Error code if stobe failed otherwise Error::OK + */ + static Error StrobeFunctionalWatchdog() + { + + Error result; + + if(lastFunctionalWatchdogQuest & ~TLF35584_FWDSTAT0_FWDQUEST_MASK) + { + // TODO ERROR + } + + // Determine which response the functional watchdog is expecting from us + const WatchdogResponse& response = + WatchdogResponses[TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest]; + + CHECK(WriteRegister(TLF35584_FWDRSP, response.resp3)); + CHECK(WriteRegister(TLF35584_FWDRSP, response.resp2)); + CHECK(WriteRegister(TLF35584_FWDRSP, response.resp1)); + CHECK(WriteRegister(TLF35584_FWDRSPSYNC, response.resp0)); + + // reset, indicating that we need to fetch a new + lastFunctionalWatchdogQuest = ~TLF35584_FWDSTAT0_FWDQUEST_MASK; + + return OK; + } + + /** + * \brief Combined window and functional strobe during setup + */ + static Error InitStrobe() + { + Error result; + + // indicate that we still need to fetch a new one + lastFunctionalWatchdogQuest = ~TLF35584_FWDSTAT0_FWDQUEST_MASK; + + CHECK(TeslaM3PowerWatchdog::StrobeWindowWatchdog()); + CHECK(TeslaM3PowerWatchdog::FunctionalWatchdogReadQuest()); + CHECK(TeslaM3PowerWatchdog::StrobeFunctionalWatchdog()); + + return OK; + } + + + /** + * \brief Run through the set up sequence for the power supply and watchdog + */ + static Error SetupPowerManagement() + { + Error result; + + // Write the initial device configuration + Register reg; + uint16_t r = 0; + for (r = 0; r < RegisterConfigSize; r++) + { + reg = RegisterConfig[r]; + CHECK(WriteRegister(reg.reg, reg.value)); + } + + // Strobe the watchdog so that everything is happy before changing the + // initial state. This closes the Long Open Window of the Window Watchdog. + // We need to be careful not to strobe for 50ms until the Closed Window + // period finishes + CHECK(InitStrobe()); + + // Move the device into the NORMAL state + const uint16_t NewState = TLF35584_DEVCTRL_TRK2EN | TLF35584_DEVCTRL_TRK1EN | + TLF35584_DEVCTRL_COMEN | TLF35584_DEVCTRL_VREFEN | + TLF35584_DEVCTRL_STATEREQ_NORMAL; + CHECK(WriteRegister(TLF35584_DEVCTRL, NewState)); + CHECK(WriteRegister(TLF35584_DEVCTRLN, TLF35584_DEVCTRLN_STATEREQ_NORMAL)); + + // Blocking wait until the state has had time to change + DEVICE_DELAY_US(StateTransitionDelay); + + const uint16_t ExpectedState = + TLF35584_DEVSTAT_TRK2EN | TLF35584_DEVSTAT_TRK1EN | + TLF35584_DEVSTAT_COMEN | TLF35584_DEVSTAT_VREFEN | + TLF35584_DEVSTAT_NORMAL; + uint16_t state; + CHECK(ReadRegister(TLF35584_DEVSTAT, state)); + + if (state == ExpectedState) + { + return OK; + } + else + { + return StateTransitionFail; + } + } + + /** + * \brief Evaluate the parity + * + * Algorithm from + * https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel + * + * \param value Value to checked for parity + * + * \return True if there are an odd number of bits set + */ + static bool HasOddParity(uint16_t value) + { + value ^= value >> 8; + value ^= value >> 4; + value ^= value >> 2; + value ^= value >> 1; + return value & 1; + } + + /** + * \brief Write a specific register + * + * \param reg Register to write to + * \param value Value to be written + * + * \return Communication failure + */ + static Error WriteRegister(uint16_t reg, uint16_t value) + { + uint16_t out = TLF35584_SPI_REG_WRITE | TLF35584_SPI_CMD(reg) | + TLF35584_SPI_DATA(value); + + if (HasOddParity(out)) + { + out = out | TLF35584_SPI_PARITY_MASK; + } + + SPITransaction transaction; + + // A working TLF35584 will echo back any write commands so we can use this + // to verify communication somewhat + + SpiDriverT::WriteData(out); + + #if 0 + if (SpiDriverT::ReadData == out) + { + return OK; + } + else + { + return WriteFail; + } + #else + SpiDriverT::ReadData(); + return OK; + #endif + } + + /** + * \brief Read back a register + * + * \param reg Register we want to read back + * + * \return Register value + */ + + static Error ReadRegister(uint16_t reg, uint16_t& value) + { + uint16_t request = TLF35584_SPI_CMD(reg); + + if (HasOddParity(request)) + { + request = request | TLF35584_SPI_PARITY_MASK; + } + + SPITransaction transaction; + uint16_t response = 0; + + SpiDriverT::WriteData(request); + // Block until data is received and then return it + response = SpiDriverT::ReadData(); + + bool parityOk = + (response & TLF35584_SPI_PARITY_MASK) == HasOddParity(response >> 1); + + if (parityOk) + { + value = (response & TLF35584_SPI_DATA_MASK) >> TLF35584_SPI_DATA_SHIFT; + return OK; + } + else + { + return ReadParityFail; + } + } +}; + +// quests are &0x000F, set something illegal +template +uint16_t TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest = ~TLF35584_FWDSTAT0_FWDQUEST_MASK; +} + +#endif // TESLAM3PMIC_H diff --git a/platform/c2000/inverter_drivers/pmicspidriver.cpp b/platform/c2000/inverter_drivers/pmicspidriver.cpp new file mode 100644 index 0000000..3170ecc --- /dev/null +++ b/platform/c2000/inverter_drivers/pmicspidriver.cpp @@ -0,0 +1,91 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2022 Bernd Ocklin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "c2000/pmicspidriver.h" + +#include "device.h" + +uint32_t PmicSpiDriver::m_base = 0; + +void PmicSpiDriver::InitGPIO(uint16_t pin, uint32_t cfg) +{ + GPIO_setMasterCore(pin, GPIO_CORE_CPU1); + GPIO_setPinConfig(cfg); + GPIO_setPadConfig(pin, GPIO_PIN_TYPE_PULLUP); + GPIO_setQualificationMode(pin, GPIO_QUAL_ASYNC); +} + +/** + * \brief Initialise the SPI port's GPIO lines + */ +void PmicSpiDriver::InitGPIOs() { + + m_base = DEVICE_TESLAM3_PMIC_SPI; + + // SPISOMIA. + InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO, DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO); + + // SPISIMOA + InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI, DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI); + + // SPISTEA / CS + InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_CS, DEVICE_TESLAM3_GPIO_CFG_PMIC_CS); + + // SPICLK. + InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK, DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK); +} + +/** + * \brief Initialise the SPI port and associated clocks + */ +void PmicSpiDriver::InitSPIPort() +{ + // TODO + + // Assert the SPI ~CS enable line before we turn off the + // pull up to avoid glitches + + SPI_disableModule(m_base); + + // Tesla run the device at 2.4MHz so we also run slower than rated 5MHz + // to be on the safe side. + // The TLF35584 requires CPOL = 0, CPHA = 0 and 16-bit MSB transfers with software + // controlled chip-select control around each transfer. + // MASTER mode, unidirectional full duplex + + SPI_setConfig(m_base, DEVICE_LSPCLK_FREQ, SPI_PROT_POL0PHA0, + SPI_MODE_MASTER, 500000, 16); + + SPI_disableFIFO(m_base); + SPI_disableLoopback(m_base); + SPI_setEmulationMode(m_base, SPI_EMULATION_STOP_AFTER_TRANSMIT); + + + SPI_enableModule(m_base); +} + +void PmicSpiDriver::WriteData(uint16_t data) +{ + SPI_writeDataNonBlocking(m_base, data); +} + +uint16_t PmicSpiDriver::ReadData() +{ + return SPI_readDataBlockingNonFIFO(m_base); +} diff --git a/platform/c2000/inverter_drivers/scheduler.cpp b/platform/c2000/inverter_drivers/scheduler.cpp new file mode 100644 index 0000000..4220e23 --- /dev/null +++ b/platform/c2000/inverter_drivers/scheduler.cpp @@ -0,0 +1,153 @@ +/* + * This file is part of the Tesla M3 OSS Inverter project. + * + * Copyright (C) 2022 Bernd Ocklin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +extern "C" { + #include "device.h" +} +#include "c2000/scheduler.h" + +uint32_t Scheduler::periods[Scheduler::maxTasks] = {0, 0, 0}; + +void (*Scheduler::functions[Scheduler::maxTasks])(void) = {0, 0, 0}; + +const uint32_t interruptTimers[3] = {INT_TIMER0, INT_TIMER1, INT_TIMER2}; +const uint32_t cpuTimerBases[3] = {CPUTIMER0_BASE, CPUTIMER1_BASE, CPUTIMER2_BASE}; + +extern "C" { +// Irq handler for CPU Timer 0 +__interrupt void cpuTimer0ISR(void) { + + if(Scheduler::functions[0] != 0) { + Scheduler::functions[0](); + } + + // ack the interrupt to be able to see more interrupts + Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1); +} + +// Irq handler for CPU Timer 1 +__interrupt void cpuTimer1ISR(void) { + if(Scheduler::functions[1] != 0) Scheduler::functions[1](); +} + +// Irq handler for CPU Timer 2 +__interrupt void cpuTimer2ISR(void) { + if(Scheduler::functions[2] != 0) Scheduler::functions[2](); +} +} + +/* + * All timers are initialized even if there are no tasks. + */ +void Scheduler::Init() { + + // ISRs for each CPU Timer interrupt + Interrupt_register(interruptTimers[0], &cpuTimer0ISR); + Interrupt_register(interruptTimers[1], &cpuTimer1ISR); + Interrupt_register(interruptTimers[2], &cpuTimer2ISR); + + // Initializes the Device Peripheral. Initialize the CPU Timers. + Scheduler::InitCPUTimers(); +} + +void Scheduler::AddTask(void (*function)(void), uint32_t msPeriod) { + + // check if there is a free timer slot available + uint16_t freeTimer = 0xffff; + + uint16_t t = 0; + for(t = 0; t < Scheduler::maxTasks; t++) { + + if((Scheduler::periods[t] == 0) && (Scheduler::functions[t] == 0)) { + freeTimer = t; + break; + } + } + + if(freeTimer >= Scheduler::maxTasks) { + // TODO: error + return; + } + + Scheduler::functions[freeTimer] = function; + Scheduler::periods[freeTimer] = msPeriod; + + // Starts CPU-Timer + Scheduler::ConfigureCPUTimer(cpuTimerBases[freeTimer], msPeriod); + + // To ensure precise timing, use write-only instructions to write to the + // entire register. Therefore, if any of the configuration bits are changed + // in configCPUTimer and initCPUTimers, the below settings must also + // be updated. + CPUTimer_enableInterrupt(cpuTimerBases[freeTimer]); + + // Enables CPU int1, int13, and int14 which are connected to + // CPU-Timer 0, CPU-Timer 1, and CPU-Timer 2 respectively. + // Enable TINT0 in the PIE: Group 1 interrupt 7 + Interrupt_enable(interruptTimers[freeTimer]); + + CPUTimer_startTimer(cpuTimerBases[freeTimer]); +} + +/* + * Initialize all CPU timers + */ +void Scheduler::InitCPUTimers(void) +{ + uint16_t t = 0; + for(t = 0; t < Scheduler::maxTasks; t++) { + + // Initialize timer period to maximum + CPUTimer_setPeriod(cpuTimerBases[t], 0xFFFFFFFF); + + // Initialize pre-scale counter to divide by 1 (SYSCLKOUT) + CPUTimer_setPreScaler(cpuTimerBases[t], 0); + + // Make sure timer is stopped + CPUTimer_stopTimer(cpuTimerBases[t]); + + // Reload all counter register with period value + CPUTimer_reloadTimerCounter(cpuTimerBases[t]); + } +} + + +/* \brief This function initializes the selected timer + * with a period in milli seconds + */ +void Scheduler::ConfigureCPUTimer(uint32_t cpuTimer, uint32_t msPeriod) { + + // Initialize timer period: + uint32_t timerPeriod = (uint32_t)(DEVICE_SYSCLK_FREQ / 1000 * msPeriod); + + CPUTimer_setPeriod(cpuTimer, timerPeriod); + + // Set pre-scale counter to divide by 1 (SYSCLKOUT): + CPUTimer_setPreScaler(cpuTimer, 0); + + // Initializes timer control register. The timer is stopped, reloaded, + // free run disabled, and interrupt enabled. + // Additionally, the free and soft bits are set + CPUTimer_stopTimer(cpuTimer); + CPUTimer_reloadTimerCounter(cpuTimer); + CPUTimer_setEmulationMode(cpuTimer, + CPUTIMER_EMULATIONMODE_STOPAFTERNEXTDECREMENT); + + CPUTimer_enableInterrupt(cpuTimer); +} diff --git a/platform/c2000/pmicdrivertest/CMakeLists.txt b/platform/c2000/pmicdrivertest/CMakeLists.txt new file mode 100644 index 0000000..7ed1278 --- /dev/null +++ b/platform/c2000/pmicdrivertest/CMakeLists.txt @@ -0,0 +1,3 @@ +# Test application to excercise the gate driver chips and PSU +add_executable(pmicdrivertest pmicdrivertest.cpp) +target_link_libraries(pmicdrivertest global_options inverter_drivers libopeninv device_support link_last) diff --git a/platform/c2000/pmicdrivertest/pmicdrivertest.cpp b/platform/c2000/pmicdrivertest/pmicdrivertest.cpp new file mode 100644 index 0000000..1104144 --- /dev/null +++ b/platform/c2000/pmicdrivertest/pmicdrivertest.cpp @@ -0,0 +1,124 @@ +/* + * This file is part of the stm32-sine project. + * + * Copyright (C) 2021 David J. Fiddes + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "device.h" + +#include "pmicdriver.h" +#include "c2000/pmicspidriver.h" +#include "c2000/scheduler.h" + +#include "driverlib.h" +#include + +using namespace c2000; + +// task added to scheduler to strobe the powerwatchdog +static void taskStrobePowerWatchdoc() { + TeslaM3PowerWatchdog::Strobe(); +} + +void main(void) +{ + // + // Initialize device clock and peripherals + // + Device_init(); + + // + // Initialize GPIO and configure the GPIO pin as a push-pull output + // + Device_initGPIO(); + + // Use only line-by-line buffering on stdout for easier debugging + // We still need to use line buffering to avoid very poor IO performance + // with C2000 semi-hosting + setvbuf(stdout, NULL, _IOLBF, 0); + + printf("Tesla M3 pmic driver test application\n\n"); + + printf( + "Running on: %s\n", + IsTeslaM3Inverter() ? "Tesla M3 Inverter" : "TI Launchpad"); + + // + // Set up the LEDs + // + uint32_t greenLedPin; + uint32_t redLedPin; + + if (IsTeslaM3Inverter()) + { + greenLedPin = DEVICE_TESLAM3_GPIO_PIN_LED1; + redLedPin = DEVICE_TESLAM3_GPIO_PIN_LED2; + } + else + { + greenLedPin = DEVICE_LAUNCHXL_GPIO_PIN_LED1; + redLedPin = DEVICE_LAUNCHXL_GPIO_PIN_LED2; + } + + GPIO_writePin(greenLedPin, 1); + GPIO_setPadConfig(greenLedPin, GPIO_PIN_TYPE_STD); + GPIO_setDirectionMode(greenLedPin, GPIO_DIR_MODE_OUT); + + GPIO_writePin(redLedPin, 1); + GPIO_setPadConfig(redLedPin, GPIO_PIN_TYPE_STD); + GPIO_setDirectionMode(redLedPin, GPIO_DIR_MODE_OUT); + + // + // Initialize PIE and clear PIE registers. Disables CPU interrupts. + // + Interrupt_initModule(); + + // + // Initialize the PIE vector table with pointers to the shell Interrupt + // Service Routines (ISR). + // + Interrupt_initVectorTable(); + + Scheduler::Init(); + + // add a task to strobe the power watchdog every 100ms + Scheduler::AddTask(taskStrobePowerWatchdoc, 100); + + // + // Enable Global Interrupt (INTM) and realtime interrupt (DBGM) + // + EINT; + ERTM; + + printf("Pmic driver initialisation: "); + TeslaM3PowerWatchdog::Init(); + + // Wait for 1 second + DEVICE_DELAY_US(1000000); + + + for(;;) + { + + GPIO_writePin(redLedPin, 0); + + DEVICE_DELAY_US(300000); + + GPIO_writePin(redLedPin, 1); + + DEVICE_DELAY_US(300000); + } +} diff --git a/src/teslam3pmic.cpp b/src/teslam3pmic.cpp deleted file mode 100644 index 9cb1bf7..0000000 --- a/src/teslam3pmic.cpp +++ /dev/null @@ -1,353 +0,0 @@ -/* - * This file is part of the stm32-sine project. - * - * Copyright (C) 2021 David J. Fiddes - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program 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. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "teslam3pmic.h" -#include "delay.h" -#include "hwdefs.h" -#include "hw/tlf35584_safety_psu.h" -#include -#include -#include - -struct Register -{ - uint8_t reg; - uint8_t value; -}; - -/** - * \brief TLF35584 safety power supply and watchdog register set up sequence - * - */ -static const Register RegisterConfig[]{ - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY1 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY2 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY3 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY4 }, - { TLF35584_WDCFG1, TLF35584_WDCFG1_WDSLPEN | TLF35584_WDCFG1_FWDETHR(14) }, - { TLF35584_WDCFG0, - TLF35584_WDCFG0_WWDETHR(14) | TLF35584_WDCFG0_WWDEN | - TLF35584_WDCFG0_FWDEN | TLF35584_WDCFG0_WWDTSEL | - TLF35584_WDCFG0_WDCYC_1MS }, - { TLF35584_SYSPCFG1, - TLF35584_SYSPCFG1_SS2DEL_0MS | TLF35584_SYSPCFG1_ERRREC_1MS }, - { TLF35584_FWDCFG, TLF35584_FWDCFG_WDHBTP_CYCLES(250) }, - { TLF35584_WWDCFG0, TLF35584_WWDCFG0_CW_CYCLES(50) }, - { TLF35584_WWDCFG1, TLF35584_WWDCFG1_OW_CYCLES(100) }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY1 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY2 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY3 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY4 } -}; - -static const uint8_t RegisterConfigSize = - sizeof(RegisterConfig) / sizeof(RegisterConfig[0]); - -/** - * Functional Watchdog response sequence structure - */ -struct WatchdogResponse -{ - uint8_t resp3; - uint8_t resp2; - uint8_t resp1; - uint8_t resp0; -}; - -/** - * Canned Functional Watchdog response sequences from Table 26 in Section 15.3 - * functional Watchdog of the TLF35584 datasheet - */ -static const WatchdogResponse WatchdogResponses[]{ - // clang-format off - { 0xFF, 0x0F, 0xF0, 0x00 }, - { 0xB0, 0x40, 0xBF, 0x4F }, - { 0xE9, 0x19, 0xE6, 0x16 }, - { 0xA6, 0x56, 0xA9, 0x59 }, - { 0x75, 0x85, 0x7A, 0x8A }, - { 0x3A, 0xCA, 0x35, 0xC5 }, - { 0x63, 0x93, 0x6C, 0x9C }, - { 0x2C, 0xDC, 0x23, 0xD3 }, - { 0xD2, 0x22, 0xDD, 0x2D }, - { 0x9D, 0x6D, 0x92, 0x62 }, - { 0xC4, 0x34, 0xCB, 0x3B }, - { 0x8B, 0x7B, 0x84, 0x74 }, - { 0x58, 0xA8, 0x57, 0xA7 }, - { 0x17, 0xE7, 0x18, 0xE8 }, - { 0x4E, 0xBE, 0x41, 0xB1 }, - { 0x01, 0xF1, 0x0E, 0xFE }, - // clang-format on -}; - -static const uint8_t WatchdogResponsesSize = - sizeof(WatchdogResponses) / sizeof(WatchdogResponses[0]); - -static const int StateTransitionDelay = 100; // uS - -/** - * \brief Verify that function x didn't fail. Return immediately if it did. - * Assumes a local variable "result" of type TeslaM3PowerWatchdog::Error - */ -#define CHECK(x) \ - if ((result = x) != Error::OK) \ - { \ - return result; \ - } - -/** - * \brief Define a scoped SPI transaction that manually asserts the ~CS line for - * the duration of a SPI transaction. - */ -struct SPITransaction -{ - SPITransaction() - { - gpio_clear(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); - } - - ~SPITransaction() - { - gpio_set(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); - } -}; - -/** - * \brief Set up the power management watchdog - * - * \return Error - Error code if initialisation failed otherwise Error::OK - */ -TeslaM3PowerWatchdog::Error TeslaM3PowerWatchdog::Init() -{ - InitSPIPort(); - return SetupPowerManagement(); -} - -/** - * \brief Strobe the watchdog - * - * \return Error - Error code if stobe failed otherwise Error::OK - */ -TeslaM3PowerWatchdog::Error TeslaM3PowerWatchdog::Strobe() -{ - Error result; - - uint8_t windowStatus; - CHECK(ReadRegister(TLF35584_WWDSCMD, windowStatus)); - - // Invert the window watchdog status and write back in the lowest bit - CHECK(WriteRegister( - TLF35584_WWDSCMD, - windowStatus & TLF35584_WWDSCMD_TRIG_STATUS ? 0 : - TLF35584_WWDSCMD_TRIG)); - - uint8_t functionalStatus; - CHECK(ReadRegister(TLF35584_FWDSTAT0, functionalStatus)); - - // Determine which response the functional watchdog is expecting from us - const WatchdogResponse& response = - WatchdogResponses[functionalStatus & TLF35584_FWDSTAT0_FWDQUEST_MASK]; - - CHECK(WriteRegister(TLF35584_FWDRSP, response.resp3)); - CHECK(WriteRegister(TLF35584_FWDRSP, response.resp2)); - CHECK(WriteRegister(TLF35584_FWDRSP, response.resp1)); - CHECK(WriteRegister(TLF35584_FWDRSPSYNC, response.resp0)); - - return Error::OK; -} - -/** - * \brief Initialise the SPI port and associated clocks and GPIO ports - */ -void TeslaM3PowerWatchdog::InitSPIPort() -{ - rcc_periph_clock_enable(RCC_SPI2); - - // Assert the SPI ~CS enable line before we turn off the - // pull up to avoid glitches - gpio_set(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); - gpio_set_mode( - GPIO_PMIC_CS_BANK, - GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_PUSHPULL, - GPIO_PMIC_CS); - - // Configure the SPI hardware ports - gpio_primary_remap(AFIO_MAPR_SWJ_CFG_JTAG_OFF_SW_ON, AFIO_MAPR_SPI1_REMAP); - gpio_set_mode( - GPIO_PMIC_SPI_BANK, - GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, - GPIO_PMIC_SPI_SCK | GPIO_PMIC_SPI_MOSI); - gpio_set_mode( - GPIO_PMIC_SPI_BANK, GPIO_MODE_INPUT, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_PMIC_SPI_MISO); - - // SPI initialization; - // We get 2.25MHz clock with a 72MHz system frequency. Tesla run the device at 2.4MHz so we - // also run slower than rated 5MHz to be on the safe side. The TLF35584 - // requires CPOL = 0, CPHA = 0 and 16-bit MSB transfers with software - // controlled chip-select control around each transfer. - spi_reset(PMIC_SPI); - spi_init_master( - PMIC_SPI, - SPI_CR1_BAUDRATE_FPCLK_DIV_32, - SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE, - SPI_CR1_CPHA_CLK_TRANSITION_1, - SPI_CR1_DFF_16BIT, - SPI_CR1_MSBFIRST); - spi_enable_software_slave_management(PMIC_SPI); - spi_set_full_duplex_mode(PMIC_SPI); - spi_set_unidirectional_mode(PMIC_SPI); - spi_set_nss_high(PMIC_SPI); - - spi_enable(PMIC_SPI); -} - -/** - * \brief Run through the set up sequence for the power supply and watchdog - */ -TeslaM3PowerWatchdog::Error TeslaM3PowerWatchdog::SetupPowerManagement() -{ - Error result; - - // Write the initial device configuration - for (const auto reg : RegisterConfig) - { - CHECK(WriteRegister(reg.reg, reg.value)); - } - - // Strobe the watchdog so that everything is happy before changing the - // initial state. This closes the Long Open Window of the Window Watchdog. - // We need to be careful not to strobe for 50ms until the Closed Window - // period finishes - CHECK(Strobe()); - - // Move the device into the NORMAL state - const uint8_t NewState = TLF35584_DEVCTRL_TRK2EN | TLF35584_DEVCTRL_TRK1EN | - TLF35584_DEVCTRL_COMEN | TLF35584_DEVCTRL_VREFEN | - TLF35584_DEVCTRL_STATEREQ_NORMAL; - CHECK(WriteRegister(TLF35584_DEVCTRL, NewState)); - CHECK(WriteRegister(TLF35584_DEVCTRLN, TLF35584_DEVCTRLN_STATEREQ_NORMAL)); - - // Blocking wait until the state has had time to change - uDelay(StateTransitionDelay); - - const uint8_t ExpectedState = - TLF35584_DEVSTAT_TRK2EN | TLF35584_DEVSTAT_TRK1EN | - TLF35584_DEVSTAT_COMEN | TLF35584_DEVSTAT_VREFEN | - TLF35584_DEVSTAT_NORMAL; - uint8_t state; - CHECK(ReadRegister(TLF35584_DEVSTAT, state)); - - if (state == ExpectedState) - { - return OK; - } - else - { - return StateTransitionFail; - } -} - -/** - * \brief Evaluate the parity - * - * Algorithm from - * https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel - * - * \param value Value to checked for parity - * - * \return True if there are an odd number of bits set - */ -static bool HasOddParity(uint16_t value) -{ - value ^= value >> 8; - value ^= value >> 4; - value ^= value >> 2; - value ^= value >> 1; - return value & 1; -} - -/** - * \brief Write a specific register - * - * \param reg Register to write to - * \param value Value to be written - * - * \return Communication failure - */ -TeslaM3PowerWatchdog::Error TeslaM3PowerWatchdog::WriteRegister( - uint8_t reg, - uint8_t value) -{ - uint16_t out = TLF35584_SPI_REG_WRITE | TLF35584_SPI_CMD(reg) | - TLF35584_SPI_DATA(value); - - if (HasOddParity(out)) - { - out = out | TLF35584_SPI_PARITY_MASK; - } - - SPITransaction transaction; - - // A working TLF35584 will echo back any write commands so we can use this - // to verify communication somewhat - if (spi_xfer(PMIC_SPI, out) == out) - { - return OK; - } - else - { - return WriteFail; - } -} - -/** - * \brief Read back a register - * - * \param reg Register we want to read back - * - * \return Register value - */ -TeslaM3PowerWatchdog::Error TeslaM3PowerWatchdog::ReadRegister( - uint8_t reg, - uint8_t& value) -{ - uint16_t request = TLF35584_SPI_CMD(reg); - - if (HasOddParity(request)) - { - request = request | TLF35584_SPI_PARITY_MASK; - } - - SPITransaction transaction; - uint16_t response = spi_xfer(PMIC_SPI, request); - - bool parityOk = - (response & TLF35584_SPI_PARITY_MASK) == HasOddParity(response >> 1); - - if (parityOk) - { - value = (response & TLF35584_SPI_DATA_MASK) >> TLF35584_SPI_DATA_SHIFT; - return OK; - } - else - { - return ReadParityFail; - } -} From 45354d90476eeaf4ee832abd8975777febe1b06a Mon Sep 17 00:00:00 2001 From: Bernd Ocklin Date: Wed, 9 Feb 2022 11:11:44 +0100 Subject: [PATCH 76/83] add SPI ports for Launchxl to device.h --- .../c2000/device_support/include/device.h | 38 ++++++++++++++----- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/platform/c2000/device_support/include/device.h b/platform/c2000/device_support/include/device.h index ab10e80..66b6d96 100644 --- a/platform/c2000/device_support/include/device.h +++ b/platform/c2000/device_support/include/device.h @@ -139,21 +139,39 @@ a single CPU should be defined." #define DEVICE_TESLAM3_GATE_SPI SPIC_BASE +// LAUNCHXL logic analyser friendly PMIC SPI pins -// Tesla Model 3 Inverter Pmic Driver (TLF35584) +#define DEVICE_LAUNCHXL_GPIO_PIN_PMIC_MOSI 58 +#define DEVICE_LAUNCHXL_GPIO_PIN_PMIC_MISO 59 +#define DEVICE_LAUNCHXL_GPIO_PIN_PMIC_CLK 60 +#define DEVICE_LAUNCHXL_GPIO_PIN_PMIC_CS 61 + +#define DEVICE_LAUNCHXL_GPIO_CFG_PMIC_MOSI GPIO_58_SPISIMOA +#define DEVICE_LAUNCHXL_GPIO_CFG_PMIC_MISO GPIO_59_SPISOMIA +#define DEVICE_LAUNCHXL_GPIO_CFG_PMIC_CLK GPIO_60_SPICLKA +#define DEVICE_LAUNCHXL_GPIO_CFG_PMIC_CS GPIO_61_SPISTEA + +#define DEVICE_LAUNCHXL_PMIC_SPI SPIA_BASE + +// Tesla Model 3 Inverter Pmic Driver (TLF35584) + +// SPI // Pins 8, 9, 10, 12 // GPIOs 16, 17, 18, 19 -#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI 16 -#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO 17 -#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK 18 -#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CS 19 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI 16 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO 17 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK 18 +#define DEVICE_TESLAM3_GPIO_PIN_PMIC_CS 19 + +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI GPIO_16_SPISIMOA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO GPIO_17_SPISOMIA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK GPIO_18_SPICLKA +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CS GPIO_19_SPISTEA -#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI GPIO_16_SPISIMOA -#define DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO GPIO_17_SPISOMIA -#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK GPIO_18_SPICLKA -#define DEVICE_TESLAM3_GPIO_CFG_PMIC_CS GPIO_19_SPISTEA +#define DEVICE_TESLAM3_PMIC_SPI SPIA_BASE -#define DEVICE_TESLAM3_PMIC_SPI SPIA_BASE +// Interrupt pin 111, GPIO65 +#define DEVICE_TESLAM3_GPIO_CFG_PMIC_INT GPIO_65_GPIO65 //***************************************************************************** // From 07633cb8124cf1103a469bf9cf31858c42df86d2 Mon Sep 17 00:00:00 2001 From: Bernd Ocklin Date: Wed, 9 Feb 2022 11:20:34 +0100 Subject: [PATCH 77/83] add pmic testing on launchxl and bug fixing - seperate out lock/unlock sequence, add wait - add resetting registers during setup - fix DEVCTRL/N inversion - add skip result reading for testing purposes - remove SPI driver read/write, replace with transfer --- .../inverter_drivers/c2000/pmicspidriver.h | 10 +- platform/c2000/inverter_drivers/pmicdriver.h | 191 +++++++++--------- .../c2000/inverter_drivers/pmicspidriver.cpp | 97 ++++++--- .../c2000/pmicdrivertest/pmicdrivertest.cpp | 13 +- 4 files changed, 180 insertions(+), 131 deletions(-) diff --git a/platform/c2000/inverter_drivers/c2000/pmicspidriver.h b/platform/c2000/inverter_drivers/c2000/pmicspidriver.h index e75f642..e231eb9 100644 --- a/platform/c2000/inverter_drivers/c2000/pmicspidriver.h +++ b/platform/c2000/inverter_drivers/c2000/pmicspidriver.h @@ -22,10 +22,12 @@ #include "stdint.h" -class PmicSpiDriver { +class PmicSpiDriver +{ private: static uint32_t m_base; + static bool m_readAfterWrite; static void InitGPIO(uint16_t, uint32_t); @@ -33,9 +35,9 @@ class PmicSpiDriver { static void InitGPIOs(); static void InitSPIPort(); - static void WriteData(uint16_t data); - static uint16_t ReadData(); + static bool ReadDataAfterWrite(); + + static uint16_t TransferData(uint16_t data); }; #endif // C2000SPIDRIVER_H - diff --git a/platform/c2000/inverter_drivers/pmicdriver.h b/platform/c2000/inverter_drivers/pmicdriver.h index acd57b5..ecdb9d0 100644 --- a/platform/c2000/inverter_drivers/pmicdriver.h +++ b/platform/c2000/inverter_drivers/pmicdriver.h @@ -2,7 +2,7 @@ * This file is part of the stm32-sine project. * * Copyright (C) 2021 David J. Fiddes - * Copyright (C) 2022 Bernd Ocklin + * Copyright (C) 2022 Bernd Ocklin * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -35,11 +35,7 @@ struct Register * \brief TLF35584 safety power supply and watchdog register set up sequence * */ -static const Register RegisterConfig[14] = { - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY1 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY2 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY3 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY4 }, +static const Register RegisterConfig[6] = { { TLF35584_WDCFG1, TLF35584_WDCFG1_WDSLPEN | TLF35584_WDCFG1_FWDETHR(14) }, { TLF35584_WDCFG0, TLF35584_WDCFG0_WWDETHR(14) | TLF35584_WDCFG0_WWDEN | @@ -49,11 +45,7 @@ static const Register RegisterConfig[14] = { TLF35584_SYSPCFG1_SS2DEL_0MS | TLF35584_SYSPCFG1_ERRREC_1MS }, { TLF35584_FWDCFG, TLF35584_FWDCFG_WDHBTP_CYCLES(250) }, { TLF35584_WWDCFG0, TLF35584_WWDCFG0_CW_CYCLES(50) }, - { TLF35584_WWDCFG1, TLF35584_WWDCFG1_OW_CYCLES(100) }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY1 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY2 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY3 }, - { TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY4 } + { TLF35584_WWDCFG1, TLF35584_WWDCFG1_OW_CYCLES(100) } }; static const uint16_t RegisterConfigSize = @@ -100,36 +92,16 @@ static const uint16_t WatchdogResponsesSize = static const int StateTransitionDelay = 100; // uS - /** * \brief Verify that function x didn't fail. Return immediately if it did. * Assumes a local variable "result" of type TeslaM3PowerWatchdog::Error */ #define CHECK(x) \ - if ((result = x) != OK) \ + if ((result = x) != OK) \ { \ return result; \ } -/** - * \brief Define a scoped SPI transaction that manually asserts the ~CS line for - * the duration of a SPI transaction. - */ -struct SPITransaction -{ - SPITransaction() - { - // gpio_clear(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); - } - - ~SPITransaction() - { - // gpio_set(GPIO_PMIC_CS_BANK, GPIO_PMIC_CS); - } -}; - - - template class TeslaM3PowerWatchdog { @@ -157,12 +129,14 @@ class TeslaM3PowerWatchdog /** * \brief Combined window and functional strobe during normal run - * Alternating functional watchdog between fetching quest and answering it. + * Alternating functional watchdog between fetching quest and answering + * it. * * Window Watchdog is defined with a 100ms period * Functional Watchdog with 250ms. * - * Thus a functional watchdog quest only needs to be answered every second WW period. + * Thus a functional watchdog quest only needs to be answered every second + * WW period. */ static Error Strobe() { @@ -170,21 +144,21 @@ class TeslaM3PowerWatchdog CHECK(TeslaM3PowerWatchdog::StrobeWindowWatchdog()); - if(lastFunctionalWatchdogQuest & TLF35584_FWDSTAT0_FWDQUEST_MASK) + if (lastFunctionalWatchdogQuest & ~TLF35584_FWDSTAT0_FWDQUEST_MASK) { - CHECK(TeslaM3PowerWatchdog::StrobeFunctionalWatchdog()); - - } else { - CHECK(TeslaM3PowerWatchdog::FunctionalWatchdogReadQuest()); } + else + { + CHECK(TeslaM3PowerWatchdog::StrobeFunctionalWatchdog()); + } return OK; } private: - - // last quest is kept track of as it is read at a different time than the response + // last quest is kept track of as it is read at a different time than the + // response static uint16_t lastFunctionalWatchdogQuest; /** @@ -200,15 +174,12 @@ class TeslaM3PowerWatchdog CHECK(ReadRegister(TLF35584_WWDSCMD, windowStatus)); // Invert the window watchdog status and write back in the lowest bit - CHECK(WriteRegister( - TLF35584_WWDSCMD, - windowStatus & TLF35584_WWDSCMD_TRIG_STATUS ? 0 : - TLF35584_WWDSCMD_TRIG)); + windowStatus = (~(windowStatus >> 7)) & TLF35584_WWDSCMD_TRIG; + CHECK(WriteRegister(TLF35584_WWDSCMD, windowStatus)); return OK; } - /** * \brief Read the quest of functional watchdog * - this is on a 200ms cycle typically @@ -216,7 +187,8 @@ class TeslaM3PowerWatchdog * * \return Error - Error code if stobe failed otherwise Error::OK */ - static Error FunctionalWatchdogReadQuest() { + static Error FunctionalWatchdogReadQuest() + { Error result; @@ -224,14 +196,15 @@ class TeslaM3PowerWatchdog CHECK(ReadRegister(TLF35584_FWDSTAT0, functionalStatus)); // Determine which response the functional watchdog is expecting from us - TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest = - functionalStatus & TLF35584_FWDSTAT0_FWDQUEST_MASK; + lastFunctionalWatchdogQuest = + functionalStatus & TLF35584_FWDSTAT0_FWDQUEST_MASK; return OK; } /** - * \brief Answer the functional watchdog quest - this is on a 200ms cycle typically + * \brief Answer the functional watchdog quest - this is on a 200ms cycle + * typically * * \return Error - Error code if stobe failed otherwise Error::OK */ @@ -240,14 +213,14 @@ class TeslaM3PowerWatchdog Error result; - if(lastFunctionalWatchdogQuest & ~TLF35584_FWDSTAT0_FWDQUEST_MASK) + if (lastFunctionalWatchdogQuest & ~TLF35584_FWDSTAT0_FWDQUEST_MASK) { // TODO ERROR } // Determine which response the functional watchdog is expecting from us - const WatchdogResponse& response = - WatchdogResponses[TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest]; + const WatchdogResponse& response = WatchdogResponses + [TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest]; CHECK(WriteRegister(TLF35584_FWDRSP, response.resp3)); CHECK(WriteRegister(TLF35584_FWDRSP, response.resp2)); @@ -277,6 +250,29 @@ class TeslaM3PowerWatchdog return OK; } + static Error UnlockRegister() + { + Error result; + + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY1)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY2)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY3)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_UNLOCK_KEY4)); + + return OK; + } + + static Error LockRegister() + { + Error result; + + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY1)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY2)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY3)); + CHECK(WriteRegister(TLF35584_PROTCFG, TLF35584_PROTCFG_LOCK_KEY4)); + + return OK; + } /** * \brief Run through the set up sequence for the power supply and watchdog @@ -288,24 +284,43 @@ class TeslaM3PowerWatchdog // Write the initial device configuration Register reg; uint16_t r = 0; + + CHECK(UnlockRegister()) + DEVICE_DELAY_US(8); for (r = 0; r < RegisterConfigSize; r++) { reg = RegisterConfig[r]; CHECK(WriteRegister(reg.reg, reg.value)); } + DEVICE_DELAY_US(8); + CHECK(LockRegister()) // Strobe the watchdog so that everything is happy before changing the - // initial state. This closes the Long Open Window of the Window Watchdog. - // We need to be careful not to strobe for 50ms until the Closed Window - // period finishes + // initial state. This closes the Long Open Window of the Window + // Watchdog. We need to be careful not to strobe for 50ms until the + // Closed Window period finishes CHECK(InitStrobe()); + // resetting registers + CHECK(WriteRegister(TLF35584_INITERR, 0xFF)); + CHECK(WriteRegister(TLF35584_OTFAIL, 0xFF)); + CHECK(WriteRegister(TLF35584_OTWRNSF, 0xFF)); + CHECK(WriteRegister(TLF35584_MONSF0, 0xFF)); + CHECK(WriteRegister(TLF35584_MONSF1, 0xFF)); + CHECK(WriteRegister(TLF35584_MONSF2, 0xFF)); + CHECK(WriteRegister(TLF35584_MONSF3, 0xFF)); + CHECK(WriteRegister(TLF35584_SPISF, 0xFF)); + CHECK(WriteRegister(TLF35584_WKSF, 0xFF)); + // Move the device into the NORMAL state - const uint16_t NewState = TLF35584_DEVCTRL_TRK2EN | TLF35584_DEVCTRL_TRK1EN | - TLF35584_DEVCTRL_COMEN | TLF35584_DEVCTRL_VREFEN | - TLF35584_DEVCTRL_STATEREQ_NORMAL; + const uint16_t NewState = + TLF35584_DEVCTRL_TRK2EN | TLF35584_DEVCTRL_TRK1EN | + TLF35584_DEVCTRL_COMEN | TLF35584_DEVCTRL_VREFEN | + TLF35584_DEVCTRL_STATEREQ_NORMAL; + // DEVCTRLN reg write MUST come directly after DEVCTRL + // and it MUST write inverted value of DEVCTRL CHECK(WriteRegister(TLF35584_DEVCTRL, NewState)); - CHECK(WriteRegister(TLF35584_DEVCTRLN, TLF35584_DEVCTRLN_STATEREQ_NORMAL)); + CHECK(WriteRegister(TLF35584_DEVCTRLN, ~NewState)); // Blocking wait until the state has had time to change DEVICE_DELAY_US(StateTransitionDelay); @@ -364,26 +379,17 @@ class TeslaM3PowerWatchdog out = out | TLF35584_SPI_PARITY_MASK; } - SPITransaction transaction; - - // A working TLF35584 will echo back any write commands so we can use this - // to verify communication somewhat - - SpiDriverT::WriteData(out); + // A working TLF35584 will echo back any write commands so we can use + // this to verify communication somewhat + uint16_t res = SpiDriverT::TransferData(out); - #if 0 - if (SpiDriverT::ReadData == out) - { - return OK; - } - else + // only verify reading the written value echoed if + // there is a device with echo - e.g. handy for launchxl + if (SpiDriverT::ReadDataAfterWrite() && (res != out)) { return WriteFail; } - #else - SpiDriverT::ReadData(); return OK; - #endif } /** @@ -403,31 +409,32 @@ class TeslaM3PowerWatchdog request = request | TLF35584_SPI_PARITY_MASK; } - SPITransaction transaction; - uint16_t response = 0; + uint16_t response = 0; - SpiDriverT::WriteData(request); - // Block until data is received and then return it - response = SpiDriverT::ReadData(); + response = SpiDriverT::TransferData(request); - bool parityOk = - (response & TLF35584_SPI_PARITY_MASK) == HasOddParity(response >> 1); - - if (parityOk) + // only verify result if there is one - testing only + if (SpiDriverT::ReadDataAfterWrite()) { - value = (response & TLF35584_SPI_DATA_MASK) >> TLF35584_SPI_DATA_SHIFT; - return OK; - } - else - { - return ReadParityFail; + + bool parityOk = (response & TLF35584_SPI_PARITY_MASK) == + HasOddParity(response >> 1); + + if (!parityOk) + { + return ReadParityFail; + } } + value = (response & TLF35584_SPI_DATA_MASK) >> TLF35584_SPI_DATA_SHIFT; + + return OK; } }; // quests are &0x000F, set something illegal template -uint16_t TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest = ~TLF35584_FWDSTAT0_FWDQUEST_MASK; -} +uint16_t TeslaM3PowerWatchdog::lastFunctionalWatchdogQuest = + ~TLF35584_FWDSTAT0_FWDQUEST_MASK; +} // namespace c2000 #endif // TESLAM3PMIC_H diff --git a/platform/c2000/inverter_drivers/pmicspidriver.cpp b/platform/c2000/inverter_drivers/pmicspidriver.cpp index 3170ecc..19743a6 100644 --- a/platform/c2000/inverter_drivers/pmicspidriver.cpp +++ b/platform/c2000/inverter_drivers/pmicspidriver.cpp @@ -22,33 +22,70 @@ #include "device.h" uint32_t PmicSpiDriver::m_base = 0; +bool PmicSpiDriver::m_readAfterWrite = false; -void PmicSpiDriver::InitGPIO(uint16_t pin, uint32_t cfg) +void PmicSpiDriver::InitGPIO(uint16_t pin, uint32_t cfg) { GPIO_setMasterCore(pin, GPIO_CORE_CPU1); GPIO_setPinConfig(cfg); - GPIO_setPadConfig(pin, GPIO_PIN_TYPE_PULLUP); + GPIO_setPadConfig(pin, GPIO_PIN_TYPE_STD); GPIO_setQualificationMode(pin, GPIO_QUAL_ASYNC); } /** * \brief Initialise the SPI port's GPIO lines */ -void PmicSpiDriver::InitGPIOs() { - - m_base = DEVICE_TESLAM3_PMIC_SPI; - - // SPISOMIA. - InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO, DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO); - - // SPISIMOA - InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI, DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI); - - // SPISTEA / CS - InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_CS, DEVICE_TESLAM3_GPIO_CFG_PMIC_CS); +void PmicSpiDriver::InitGPIOs() +{ - // SPICLK. - InitGPIO(DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK, DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK); + if (IsTeslaM3Inverter()) + { + + m_base = DEVICE_TESLAM3_PMIC_SPI; + m_readAfterWrite = true; + + // SPISOMIA. + InitGPIO( + DEVICE_TESLAM3_GPIO_PIN_PMIC_MISO, + DEVICE_TESLAM3_GPIO_CFG_PMIC_MISO); + + // SPISIMOA + InitGPIO( + DEVICE_TESLAM3_GPIO_PIN_PMIC_MOSI, + DEVICE_TESLAM3_GPIO_CFG_PMIC_MOSI); + + // SPISTEA / CS + InitGPIO( + DEVICE_TESLAM3_GPIO_PIN_PMIC_CS, DEVICE_TESLAM3_GPIO_CFG_PMIC_CS); + + // SPICLK. + InitGPIO( + DEVICE_TESLAM3_GPIO_PIN_PMIC_CLK, DEVICE_TESLAM3_GPIO_CFG_PMIC_CLK); + } + else + { + + m_base = DEVICE_LAUNCHXL_PMIC_SPI; + + // SPISOMIA. + InitGPIO( + DEVICE_LAUNCHXL_GPIO_PIN_PMIC_MISO, + DEVICE_LAUNCHXL_GPIO_CFG_PMIC_MISO); + + // SPISIMOA + InitGPIO( + DEVICE_LAUNCHXL_GPIO_PIN_PMIC_MOSI, + DEVICE_LAUNCHXL_GPIO_CFG_PMIC_MOSI); + + // SPISTEA / CS + InitGPIO( + DEVICE_LAUNCHXL_GPIO_PIN_PMIC_CS, DEVICE_LAUNCHXL_GPIO_CFG_PMIC_CS); + + // SPICLK. + InitGPIO( + DEVICE_LAUNCHXL_GPIO_PIN_PMIC_CLK, + DEVICE_LAUNCHXL_GPIO_CFG_PMIC_CLK); + } } /** @@ -65,27 +102,33 @@ void PmicSpiDriver::InitSPIPort() // Tesla run the device at 2.4MHz so we also run slower than rated 5MHz // to be on the safe side. - // The TLF35584 requires CPOL = 0, CPHA = 0 and 16-bit MSB transfers with software - // controlled chip-select control around each transfer. - // MASTER mode, unidirectional full duplex - - SPI_setConfig(m_base, DEVICE_LSPCLK_FREQ, SPI_PROT_POL0PHA0, - SPI_MODE_MASTER, 500000, 16); + // SLI/SDO rising edge, shift falling edge of clock (CPHA 0) + // CLK low when idle (CPOL 0) + // The TLF35584 requires CPOL = 0, CPHA = 0 and 16-bit MSB transfers with + // software controlled chip-select control around each transfer. MASTER + // mode, unidirectional full duplex + + SPI_setConfig( + m_base, + DEVICE_LSPCLK_FREQ, + SPI_PROT_POL0PHA1, + SPI_MODE_MASTER, + 500000, + 16); SPI_disableFIFO(m_base); SPI_disableLoopback(m_base); SPI_setEmulationMode(m_base, SPI_EMULATION_STOP_AFTER_TRANSMIT); - SPI_enableModule(m_base); } -void PmicSpiDriver::WriteData(uint16_t data) +bool PmicSpiDriver::ReadDataAfterWrite() { - SPI_writeDataNonBlocking(m_base, data); + return m_readAfterWrite; } -uint16_t PmicSpiDriver::ReadData() +uint16_t PmicSpiDriver::TransferData(uint16_t data) { - return SPI_readDataBlockingNonFIFO(m_base); + return SPI_pollingNonFIFOTransaction(m_base, 16U, data); } diff --git a/platform/c2000/pmicdrivertest/pmicdrivertest.cpp b/platform/c2000/pmicdrivertest/pmicdrivertest.cpp index 1104144..b5685ff 100644 --- a/platform/c2000/pmicdrivertest/pmicdrivertest.cpp +++ b/platform/c2000/pmicdrivertest/pmicdrivertest.cpp @@ -29,7 +29,8 @@ using namespace c2000; // task added to scheduler to strobe the powerwatchdog -static void taskStrobePowerWatchdoc() { +static void taskStrobePowerWatchdoc() +{ TeslaM3PowerWatchdog::Strobe(); } @@ -94,9 +95,6 @@ void main(void) Scheduler::Init(); - // add a task to strobe the power watchdog every 100ms - Scheduler::AddTask(taskStrobePowerWatchdoc, 100); - // // Enable Global Interrupt (INTM) and realtime interrupt (DBGM) // @@ -106,11 +104,10 @@ void main(void) printf("Pmic driver initialisation: "); TeslaM3PowerWatchdog::Init(); - // Wait for 1 second - DEVICE_DELAY_US(1000000); - + // add a task to strobe the power watchdog every 100ms + Scheduler::AddTask(taskStrobePowerWatchdoc, 100); - for(;;) + for (;;) { GPIO_writePin(redLedPin, 0); From 231a6bef4abdcecd45ff20fcc449e040e92e5caa Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Wed, 9 Feb 2022 11:49:34 +0000 Subject: [PATCH 78/83] Integate gate driver support into main C2000 inverter Update the c2000 inverter application to use the gate driver. This involves turning on the gate drive PSU before initialising the gate drivers for each phase. The fault state of the gate drivers is reported to stdout every second. Add a simple heartbeat LED toggle to the main loop to compensate for the gate drive fault LED no longer being on (!) as a way of determinig if the inverter is powered up. Tests: - Run on the Tesla M3 inverter and verify it operates with the gate drive fault LED extinguished and not gate drive fault is reported. --- .../c2000/gatedrivertest/gatedrivertest.cpp | 2 +- platform/c2000/inverter/inverter_main.cpp | 52 +++++++++++++++++-- .../inverter_drivers/{ => c2000}/gatedriver.h | 2 +- .../{ => c2000}/gatedriverinterface.h | 0 .../c2000/inverter_drivers/gatedriver.cpp | 2 +- .../inverter_drivers/gatedriverinterface.cpp | 2 +- 6 files changed, 52 insertions(+), 8 deletions(-) rename platform/c2000/inverter_drivers/{ => c2000}/gatedriver.h (98%) rename platform/c2000/inverter_drivers/{ => c2000}/gatedriverinterface.h (100%) diff --git a/platform/c2000/gatedrivertest/gatedrivertest.cpp b/platform/c2000/gatedrivertest/gatedrivertest.cpp index bb84d7c..24c0c92 100644 --- a/platform/c2000/gatedrivertest/gatedrivertest.cpp +++ b/platform/c2000/gatedrivertest/gatedrivertest.cpp @@ -17,7 +17,7 @@ * along with this program. If not, see . */ -#include "gatedriver.h" +#include "c2000/gatedriver.h" #include "device.h" #include "driverlib.h" #include diff --git a/platform/c2000/inverter/inverter_main.cpp b/platform/c2000/inverter/inverter_main.cpp index 7fa3dc9..76b865c 100644 --- a/platform/c2000/inverter/inverter_main.cpp +++ b/platform/c2000/inverter/inverter_main.cpp @@ -17,15 +17,16 @@ * along with this program. If not, see . */ -#include "c2000/current.h" -#include "c2000/encoder.h" -#include "c2000/pwmdriver.h" -#include "c2000/pwmgeneration.h" #include "device.h" #include "driverlib.h" #include "errormessage.h" #include "focpwmgeneration.h" +#include "c2000/current.h" +#include "c2000/encoder.h" +#include "c2000/gatedriver.h" #include "c2000/performancecounter.h" +#include "c2000/pwmdriver.h" +#include "c2000/pwmgeneration.h" #include // Pull in the whole C2000 namespace as this is platform specific code obviously @@ -69,6 +70,45 @@ void main(void) GPIO_setPinConfig(GPIO_5_EPWM3B); } + // + // Set up the heartbeat LED + // + uint32_t heartbeatLedPin; + + if (IsTeslaM3Inverter()) + { + heartbeatLedPin = DEVICE_TESLAM3_GPIO_PIN_LED1; + } + else + { + heartbeatLedPin = DEVICE_LAUNCHXL_GPIO_PIN_LED1; + } + GPIO_writePin(heartbeatLedPin, 1); + GPIO_setPadConfig(heartbeatLedPin, GPIO_PIN_TYPE_STD); + GPIO_setDirectionMode(heartbeatLedPin, GPIO_DIR_MODE_OUT); + + // + // Turn on the gate drive PSU + // + GPIO_writePin(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, 0); + GPIO_setPadConfig(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, GPIO_PIN_TYPE_STD); + GPIO_setDirectionMode(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, GPIO_DIR_MODE_OUT); + printf("Gate Drive PSU OFF\n"); + + // + // Set up the gate drivers for PWM operation + // + printf("Gate Drive initialisation: "); + if (GateDriver::Init()) + { + printf("Successful\n"); + GateDriver::Enable(); + } + else + { + printf("Failed\n"); + } + // // Initialize PIE and clear PIE registers. Disables CPU interrupts. // @@ -130,8 +170,12 @@ void main(void) { DEVICE_DELAY_US(500000); + printf("Gate Drive: %s\n", GateDriver::IsFaulty() ? "FAULT" : "OK"); + int32_t currentLoad = PwmGeneration::GetCpuLoad(); printf("PWM cycles: %ld\n", currentLoad - lastLoad); lastLoad = currentLoad; + + GPIO_togglePin(heartbeatLedPin); } } diff --git a/platform/c2000/inverter_drivers/gatedriver.h b/platform/c2000/inverter_drivers/c2000/gatedriver.h similarity index 98% rename from platform/c2000/inverter_drivers/gatedriver.h rename to platform/c2000/inverter_drivers/c2000/gatedriver.h index 91f7b2a..2978ae0 100644 --- a/platform/c2000/inverter_drivers/gatedriver.h +++ b/platform/c2000/inverter_drivers/c2000/gatedriver.h @@ -19,7 +19,7 @@ #ifndef GATEDRIVER_H #define GATEDRIVER_H -#include "gatedriverinterface.h" +#include "c2000/gatedriverinterface.h" #include namespace c2000 { diff --git a/platform/c2000/inverter_drivers/gatedriverinterface.h b/platform/c2000/inverter_drivers/c2000/gatedriverinterface.h similarity index 100% rename from platform/c2000/inverter_drivers/gatedriverinterface.h rename to platform/c2000/inverter_drivers/c2000/gatedriverinterface.h diff --git a/platform/c2000/inverter_drivers/gatedriver.cpp b/platform/c2000/inverter_drivers/gatedriver.cpp index c4a5a83..2ab7b33 100644 --- a/platform/c2000/inverter_drivers/gatedriver.cpp +++ b/platform/c2000/inverter_drivers/gatedriver.cpp @@ -16,7 +16,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ -#include "gatedriver.h" +#include "c2000/gatedriver.h" #include "crc8.h" #include "device.h" #include "driverlib.h" diff --git a/platform/c2000/inverter_drivers/gatedriverinterface.cpp b/platform/c2000/inverter_drivers/gatedriverinterface.cpp index 8c0eeda..a6f329d 100644 --- a/platform/c2000/inverter_drivers/gatedriverinterface.cpp +++ b/platform/c2000/inverter_drivers/gatedriverinterface.cpp @@ -17,7 +17,7 @@ * along with this program. If not, see . */ -#include "gatedriverinterface.h" +#include "c2000/gatedriverinterface.h" #include "device.h" #include "driverlib.h" #include "hw/stgap1as_gate_driver.h" From ff48e167e4d870173bc28dff3ac780a3fafc36d1 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Wed, 9 Feb 2022 18:23:13 +0000 Subject: [PATCH 79/83] Move repo contents to sub-directory prior to merge Prior to merging this repo into the parent c2000-inverter repo we move the contents into a sub-directory with the same name. Follows the guide at https://medium.com/walkme-engineering/how-to-merge-a-git-submodule-into-its-main-repository-d83a215a319c --- CMakeLists.txt => libopeninv/CMakeLists.txt | 0 LICENSE => libopeninv/LICENSE | 0 README.md => libopeninv/README.md | 0 {include => libopeninv/include}/anain.h | 0 {include => libopeninv/include}/crc8.h | 0 {include => libopeninv/include}/delay.h | 0 {include => libopeninv/include}/digio.h | 0 {include => libopeninv/include}/errormessage.h | 0 {include => libopeninv/include}/foc.h | 0 {include => libopeninv/include}/fu.h | 0 {include => libopeninv/include}/linbus.h | 0 {include => libopeninv/include}/my_fp.h | 0 {include => libopeninv/include}/my_math.h | 0 {include => libopeninv/include}/my_string.h | 0 {include => libopeninv/include}/param_save.h | 0 {include => libopeninv/include}/params.h | 0 {include => libopeninv/include}/picontroller.h | 0 {include => libopeninv/include}/printf.h | 0 {include => libopeninv/include}/sine_core.h | 0 {include => libopeninv/include}/stm32_can.h | 0 {include => libopeninv/include}/stm32_loader.h | 0 {include => libopeninv/include}/stm32scheduler.h | 0 {include => libopeninv/include}/terminal.h | 0 {include => libopeninv/include}/terminalcommands.h | 0 {src => libopeninv/src}/anain.cpp | 0 {src => libopeninv/src}/crc8.cpp | 0 {src => libopeninv/src}/digio.cpp | 0 {src => libopeninv/src}/errormessage.cpp | 0 {src => libopeninv/src}/foc.cpp | 0 {src => libopeninv/src}/fu.cpp | 0 {src => libopeninv/src}/linbus.cpp | 0 {src => libopeninv/src}/my_fp.c | 0 {src => libopeninv/src}/my_string.c | 0 {src => libopeninv/src}/param_save.cpp | 0 {src => libopeninv/src}/params.cpp | 0 {src => libopeninv/src}/picontroller.cpp | 0 {src => libopeninv/src}/printf.cpp | 0 {src => libopeninv/src}/sine_core.cpp | 0 {src => libopeninv/src}/stm32_can.cpp | 0 {src => libopeninv/src}/stm32scheduler.cpp | 0 {src => libopeninv/src}/terminal.cpp | 0 {src => libopeninv/src}/terminalcommands.cpp | 0 42 files changed, 0 insertions(+), 0 deletions(-) rename CMakeLists.txt => libopeninv/CMakeLists.txt (100%) rename LICENSE => libopeninv/LICENSE (100%) rename README.md => libopeninv/README.md (100%) rename {include => libopeninv/include}/anain.h (100%) rename {include => libopeninv/include}/crc8.h (100%) rename {include => libopeninv/include}/delay.h (100%) rename {include => libopeninv/include}/digio.h (100%) rename {include => libopeninv/include}/errormessage.h (100%) rename {include => libopeninv/include}/foc.h (100%) rename {include => libopeninv/include}/fu.h (100%) rename {include => libopeninv/include}/linbus.h (100%) rename {include => libopeninv/include}/my_fp.h (100%) rename {include => libopeninv/include}/my_math.h (100%) rename {include => libopeninv/include}/my_string.h (100%) rename {include => libopeninv/include}/param_save.h (100%) rename {include => libopeninv/include}/params.h (100%) rename {include => libopeninv/include}/picontroller.h (100%) rename {include => libopeninv/include}/printf.h (100%) rename {include => libopeninv/include}/sine_core.h (100%) rename {include => libopeninv/include}/stm32_can.h (100%) rename {include => libopeninv/include}/stm32_loader.h (100%) rename {include => libopeninv/include}/stm32scheduler.h (100%) rename {include => libopeninv/include}/terminal.h (100%) rename {include => libopeninv/include}/terminalcommands.h (100%) rename {src => libopeninv/src}/anain.cpp (100%) rename {src => libopeninv/src}/crc8.cpp (100%) rename {src => libopeninv/src}/digio.cpp (100%) rename {src => libopeninv/src}/errormessage.cpp (100%) rename {src => libopeninv/src}/foc.cpp (100%) rename {src => libopeninv/src}/fu.cpp (100%) rename {src => libopeninv/src}/linbus.cpp (100%) rename {src => libopeninv/src}/my_fp.c (100%) rename {src => libopeninv/src}/my_string.c (100%) rename {src => libopeninv/src}/param_save.cpp (100%) rename {src => libopeninv/src}/params.cpp (100%) rename {src => libopeninv/src}/picontroller.cpp (100%) rename {src => libopeninv/src}/printf.cpp (100%) rename {src => libopeninv/src}/sine_core.cpp (100%) rename {src => libopeninv/src}/stm32_can.cpp (100%) rename {src => libopeninv/src}/stm32scheduler.cpp (100%) rename {src => libopeninv/src}/terminal.cpp (100%) rename {src => libopeninv/src}/terminalcommands.cpp (100%) diff --git a/CMakeLists.txt b/libopeninv/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to libopeninv/CMakeLists.txt diff --git a/LICENSE b/libopeninv/LICENSE similarity index 100% rename from LICENSE rename to libopeninv/LICENSE diff --git a/README.md b/libopeninv/README.md similarity index 100% rename from README.md rename to libopeninv/README.md diff --git a/include/anain.h b/libopeninv/include/anain.h similarity index 100% rename from include/anain.h rename to libopeninv/include/anain.h diff --git a/include/crc8.h b/libopeninv/include/crc8.h similarity index 100% rename from include/crc8.h rename to libopeninv/include/crc8.h diff --git a/include/delay.h b/libopeninv/include/delay.h similarity index 100% rename from include/delay.h rename to libopeninv/include/delay.h diff --git a/include/digio.h b/libopeninv/include/digio.h similarity index 100% rename from include/digio.h rename to libopeninv/include/digio.h diff --git a/include/errormessage.h b/libopeninv/include/errormessage.h similarity index 100% rename from include/errormessage.h rename to libopeninv/include/errormessage.h diff --git a/include/foc.h b/libopeninv/include/foc.h similarity index 100% rename from include/foc.h rename to libopeninv/include/foc.h diff --git a/include/fu.h b/libopeninv/include/fu.h similarity index 100% rename from include/fu.h rename to libopeninv/include/fu.h diff --git a/include/linbus.h b/libopeninv/include/linbus.h similarity index 100% rename from include/linbus.h rename to libopeninv/include/linbus.h diff --git a/include/my_fp.h b/libopeninv/include/my_fp.h similarity index 100% rename from include/my_fp.h rename to libopeninv/include/my_fp.h diff --git a/include/my_math.h b/libopeninv/include/my_math.h similarity index 100% rename from include/my_math.h rename to libopeninv/include/my_math.h diff --git a/include/my_string.h b/libopeninv/include/my_string.h similarity index 100% rename from include/my_string.h rename to libopeninv/include/my_string.h diff --git a/include/param_save.h b/libopeninv/include/param_save.h similarity index 100% rename from include/param_save.h rename to libopeninv/include/param_save.h diff --git a/include/params.h b/libopeninv/include/params.h similarity index 100% rename from include/params.h rename to libopeninv/include/params.h diff --git a/include/picontroller.h b/libopeninv/include/picontroller.h similarity index 100% rename from include/picontroller.h rename to libopeninv/include/picontroller.h diff --git a/include/printf.h b/libopeninv/include/printf.h similarity index 100% rename from include/printf.h rename to libopeninv/include/printf.h diff --git a/include/sine_core.h b/libopeninv/include/sine_core.h similarity index 100% rename from include/sine_core.h rename to libopeninv/include/sine_core.h diff --git a/include/stm32_can.h b/libopeninv/include/stm32_can.h similarity index 100% rename from include/stm32_can.h rename to libopeninv/include/stm32_can.h diff --git a/include/stm32_loader.h b/libopeninv/include/stm32_loader.h similarity index 100% rename from include/stm32_loader.h rename to libopeninv/include/stm32_loader.h diff --git a/include/stm32scheduler.h b/libopeninv/include/stm32scheduler.h similarity index 100% rename from include/stm32scheduler.h rename to libopeninv/include/stm32scheduler.h diff --git a/include/terminal.h b/libopeninv/include/terminal.h similarity index 100% rename from include/terminal.h rename to libopeninv/include/terminal.h diff --git a/include/terminalcommands.h b/libopeninv/include/terminalcommands.h similarity index 100% rename from include/terminalcommands.h rename to libopeninv/include/terminalcommands.h diff --git a/src/anain.cpp b/libopeninv/src/anain.cpp similarity index 100% rename from src/anain.cpp rename to libopeninv/src/anain.cpp diff --git a/src/crc8.cpp b/libopeninv/src/crc8.cpp similarity index 100% rename from src/crc8.cpp rename to libopeninv/src/crc8.cpp diff --git a/src/digio.cpp b/libopeninv/src/digio.cpp similarity index 100% rename from src/digio.cpp rename to libopeninv/src/digio.cpp diff --git a/src/errormessage.cpp b/libopeninv/src/errormessage.cpp similarity index 100% rename from src/errormessage.cpp rename to libopeninv/src/errormessage.cpp diff --git a/src/foc.cpp b/libopeninv/src/foc.cpp similarity index 100% rename from src/foc.cpp rename to libopeninv/src/foc.cpp diff --git a/src/fu.cpp b/libopeninv/src/fu.cpp similarity index 100% rename from src/fu.cpp rename to libopeninv/src/fu.cpp diff --git a/src/linbus.cpp b/libopeninv/src/linbus.cpp similarity index 100% rename from src/linbus.cpp rename to libopeninv/src/linbus.cpp diff --git a/src/my_fp.c b/libopeninv/src/my_fp.c similarity index 100% rename from src/my_fp.c rename to libopeninv/src/my_fp.c diff --git a/src/my_string.c b/libopeninv/src/my_string.c similarity index 100% rename from src/my_string.c rename to libopeninv/src/my_string.c diff --git a/src/param_save.cpp b/libopeninv/src/param_save.cpp similarity index 100% rename from src/param_save.cpp rename to libopeninv/src/param_save.cpp diff --git a/src/params.cpp b/libopeninv/src/params.cpp similarity index 100% rename from src/params.cpp rename to libopeninv/src/params.cpp diff --git a/src/picontroller.cpp b/libopeninv/src/picontroller.cpp similarity index 100% rename from src/picontroller.cpp rename to libopeninv/src/picontroller.cpp diff --git a/src/printf.cpp b/libopeninv/src/printf.cpp similarity index 100% rename from src/printf.cpp rename to libopeninv/src/printf.cpp diff --git a/src/sine_core.cpp b/libopeninv/src/sine_core.cpp similarity index 100% rename from src/sine_core.cpp rename to libopeninv/src/sine_core.cpp diff --git a/src/stm32_can.cpp b/libopeninv/src/stm32_can.cpp similarity index 100% rename from src/stm32_can.cpp rename to libopeninv/src/stm32_can.cpp diff --git a/src/stm32scheduler.cpp b/libopeninv/src/stm32scheduler.cpp similarity index 100% rename from src/stm32scheduler.cpp rename to libopeninv/src/stm32scheduler.cpp diff --git a/src/terminal.cpp b/libopeninv/src/terminal.cpp similarity index 100% rename from src/terminal.cpp rename to libopeninv/src/terminal.cpp diff --git a/src/terminalcommands.cpp b/libopeninv/src/terminalcommands.cpp similarity index 100% rename from src/terminalcommands.cpp rename to libopeninv/src/terminalcommands.cpp From 93ff3b300468d19a3ecae0e8c476bc7127e07abd Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Wed, 9 Feb 2022 18:26:55 +0000 Subject: [PATCH 80/83] Remove libopeninv submodule --- .gitmodules | 4 ---- libopeninv | 1 - 2 files changed, 5 deletions(-) delete mode 160000 libopeninv diff --git a/.gitmodules b/.gitmodules index cc8c79b..687b8f7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,3 @@ [submodule "libopencm3"] path = libopencm3 url = https://github.com/jsphuebner/libopencm3 -[submodule "libopeninv"] - path = libopeninv - url = https://github.com/davefiddes/libopeninv.git - branch = portable-cpp diff --git a/libopeninv b/libopeninv deleted file mode 160000 index 7b367b8..0000000 --- a/libopeninv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7b367b8eaa7c78f9c142c828ebaaafd4129e797c From d1c28bfbf3870cee9edec3d8a9868ea2efbe2513 Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Thu, 10 Feb 2022 10:41:13 +0000 Subject: [PATCH 81/83] Move STM32 inverter into dedicated platform directory This moves the main STM32F1 inverter code into a platform directory so it follows the style and layout of the C2000 port. STM32 platform specific code remains in libopeninv. Tests: - Build all variants of the CMake build process - Build FOC and SINE variants with STM32 Makefile --- Makefile | 4 ++-- platform/stm32f1/CMakeLists.txt | 2 ++ platform/stm32f1/inverter/CMakeLists.txt | 11 +++++++++++ {src => platform/stm32f1/inverter}/hwinit.cpp | 0 {src => platform/stm32f1/inverter}/inc_encoder.cpp | 0 {src => platform/stm32f1/inverter}/stm32_sine.cpp | 0 .../stm32f1/inverter}/stm32pwmdriver.cpp | 0 {src => platform/stm32f1/inverter}/temp_meas.cpp | 0 .../stm32f1/inverter}/terminal_prj.cpp | 0 .../stm32f1/inverter}/vehiclecontrol.cpp | 0 src/CMakeLists.txt | 14 -------------- 11 files changed, 15 insertions(+), 16 deletions(-) create mode 100644 platform/stm32f1/CMakeLists.txt create mode 100644 platform/stm32f1/inverter/CMakeLists.txt rename {src => platform/stm32f1/inverter}/hwinit.cpp (100%) rename {src => platform/stm32f1/inverter}/inc_encoder.cpp (100%) rename {src => platform/stm32f1/inverter}/stm32_sine.cpp (100%) rename {src => platform/stm32f1/inverter}/stm32pwmdriver.cpp (100%) rename {src => platform/stm32f1/inverter}/temp_meas.cpp (100%) rename {src => platform/stm32f1/inverter}/terminal_prj.cpp (100%) rename {src => platform/stm32f1/inverter}/vehiclecontrol.cpp (100%) diff --git a/Makefile b/Makefile index 120149d..2c51696 100644 --- a/Makefile +++ b/Makefile @@ -50,8 +50,8 @@ ifeq ($(CONTROL), FOC) endif OBJS = $(patsubst %.o,obj/%.o, $(OBJSL)) -vpath %.c src/ libopeninv/src -vpath %.cpp src/ libopeninv/src +vpath %.c src/ libopeninv/src platform/stm32f1/inverter +vpath %.cpp src/ libopeninv/src platform/stm32f1/inverter OPENOCD_BASE = /usr OPENOCD = $(OPENOCD_BASE)/bin/openocd diff --git a/platform/stm32f1/CMakeLists.txt b/platform/stm32f1/CMakeLists.txt new file mode 100644 index 0000000..59c3cb8 --- /dev/null +++ b/platform/stm32f1/CMakeLists.txt @@ -0,0 +1,2 @@ +# STM32F1 main inverter executable (stm32_sine) +add_subdirectory(inverter) diff --git a/platform/stm32f1/inverter/CMakeLists.txt b/platform/stm32f1/inverter/CMakeLists.txt new file mode 100644 index 0000000..7756c8c --- /dev/null +++ b/platform/stm32f1/inverter/CMakeLists.txt @@ -0,0 +1,11 @@ +add_executable( + stm32_sine + hwinit.cpp + inc_encoder.cpp + stm32_sine.cpp + stm32pwmdriver.cpp + temp_meas.cpp + terminal_prj.cpp + vehiclecontrol.cpp) +target_link_libraries(stm32_sine global_options) +target_link_libraries(stm32_sine libopeninv libopeninv_stm32f1 portablesine) diff --git a/src/hwinit.cpp b/platform/stm32f1/inverter/hwinit.cpp similarity index 100% rename from src/hwinit.cpp rename to platform/stm32f1/inverter/hwinit.cpp diff --git a/src/inc_encoder.cpp b/platform/stm32f1/inverter/inc_encoder.cpp similarity index 100% rename from src/inc_encoder.cpp rename to platform/stm32f1/inverter/inc_encoder.cpp diff --git a/src/stm32_sine.cpp b/platform/stm32f1/inverter/stm32_sine.cpp similarity index 100% rename from src/stm32_sine.cpp rename to platform/stm32f1/inverter/stm32_sine.cpp diff --git a/src/stm32pwmdriver.cpp b/platform/stm32f1/inverter/stm32pwmdriver.cpp similarity index 100% rename from src/stm32pwmdriver.cpp rename to platform/stm32f1/inverter/stm32pwmdriver.cpp diff --git a/src/temp_meas.cpp b/platform/stm32f1/inverter/temp_meas.cpp similarity index 100% rename from src/temp_meas.cpp rename to platform/stm32f1/inverter/temp_meas.cpp diff --git a/src/terminal_prj.cpp b/platform/stm32f1/inverter/terminal_prj.cpp similarity index 100% rename from src/terminal_prj.cpp rename to platform/stm32f1/inverter/terminal_prj.cpp diff --git a/src/vehiclecontrol.cpp b/platform/stm32f1/inverter/vehiclecontrol.cpp similarity index 100% rename from src/vehiclecontrol.cpp rename to platform/stm32f1/inverter/vehiclecontrol.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 45b4dee..d6b57d5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,17 +1,3 @@ add_library(portablesine throttle.cpp) target_link_libraries(portablesine global_options libopeninv) target_include_directories(libopeninv PUBLIC "${CMAKE_SOURCE_DIR}/include") - -if(PLATFORM STREQUAL "stm32f1") - add_executable( - stm32_sine - hwinit.cpp - inc_encoder.cpp - stm32_sine.cpp - stm32pwmdriver.cpp - temp_meas.cpp - terminal_prj.cpp - vehiclecontrol.cpp) - target_link_libraries(stm32_sine global_options) - target_link_libraries(stm32_sine libopeninv libopeninv_stm32f1 portablesine) -endif() From 884dcfde73b5b0138983c138ac6dca0879ade92d Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Fri, 11 Feb 2022 12:15:12 +0000 Subject: [PATCH 82/83] Isolate STM32 components of libopeninv in platform This change moves all of the STM32 specific parts of libopeninv into the platform/stm32f1 directory. This leaves the libopeninv directory in the top-level to contain only cross-platform code. No code changes were required only build system updates. Tests: - Build all CMake variants - Build FOC and SINE variants using the old stm32_sine Makefile --- Makefile | 8 ++++---- libopeninv/CMakeLists.txt | 19 ------------------- platform/stm32f1/CMakeLists.txt | 3 +++ .../stm32f1/libopeninv_stm32f1/CMakeLists.txt | 16 ++++++++++++++++ .../libopeninv_stm32f1}/include/anain.h | 0 .../libopeninv_stm32f1}/include/digio.h | 0 .../libopeninv_stm32f1}/include/param_save.h | 0 .../libopeninv_stm32f1}/include/stm32_can.h | 0 .../include/stm32_loader.h | 0 .../include/stm32scheduler.h | 0 .../libopeninv_stm32f1}/include/terminal.h | 0 .../include/terminalcommands.h | 0 .../stm32f1/libopeninv_stm32f1}/src/anain.cpp | 0 .../stm32f1/libopeninv_stm32f1}/src/digio.cpp | 0 .../libopeninv_stm32f1}/src/param_save.cpp | 0 .../libopeninv_stm32f1}/src/stm32_can.cpp | 0 .../src/stm32scheduler.cpp | 0 .../libopeninv_stm32f1}/src/terminal.cpp | 0 .../src/terminalcommands.cpp | 0 19 files changed, 23 insertions(+), 23 deletions(-) create mode 100644 platform/stm32f1/libopeninv_stm32f1/CMakeLists.txt rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/anain.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/digio.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/param_save.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/stm32_can.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/stm32_loader.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/stm32scheduler.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/terminal.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/include/terminalcommands.h (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/anain.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/digio.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/param_save.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/stm32_can.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/stm32scheduler.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/terminal.cpp (100%) rename {libopeninv => platform/stm32f1/libopeninv_stm32f1}/src/terminalcommands.cpp (100%) diff --git a/Makefile b/Makefile index 2c51696..bf2605e 100644 --- a/Makefile +++ b/Makefile @@ -30,11 +30,11 @@ OBJCOPY = $(PREFIX)-objcopy OBJDUMP = $(PREFIX)-objdump MKDIR_P = mkdir -p TERMINAL_DEBUG ?= 0 -CFLAGS = -Os -Wall -Wextra -Iinclude/ -Ilibopeninv/include -Ilibopencm3/include \ +CFLAGS = -Os -Wall -Wextra -Iinclude/ -Ilibopeninv/include -Iplatform/stm32f1/libopeninv_stm32f1/include -Ilibopencm3/include \ -fno-common -fno-builtin -pedantic -DSTM32F1 -DT_DEBUG=$(TERMINAL_DEBUG) \ -DCONTROL=CTRL_$(CONTROL) -DCTRL_SINE=0 -DCTRL_FOC=1 \ -mcpu=cortex-m3 -mthumb -std=gnu99 -ffunction-sections -fdata-sections -ggdb3 -CPPFLAGS = -Os -Wall -Wextra -Iinclude/ -Ilibopeninv/include -Ilibopencm3/include \ +CPPFLAGS = -Os -Wall -Wextra -Iinclude/ -Ilibopeninv/include -Iplatform/stm32f1/libopeninv_stm32f1/include -Ilibopencm3/include \ -fno-common -std=c++11 -pedantic -DSTM32F1 -DT_DEBUG=$(TERMINAL_DEBUG) \ -DCONTROL=CTRL_$(CONTROL) -DCTRL_SINE=0 -DCTRL_FOC=1 \ -ffunction-sections -fdata-sections -fno-builtin -fno-rtti -fno-exceptions -fno-unwind-tables -mcpu=cortex-m3 -mthumb -ggdb3 @@ -50,8 +50,8 @@ ifeq ($(CONTROL), FOC) endif OBJS = $(patsubst %.o,obj/%.o, $(OBJSL)) -vpath %.c src/ libopeninv/src platform/stm32f1/inverter -vpath %.cpp src/ libopeninv/src platform/stm32f1/inverter +vpath %.c src/ libopeninv/src platform/stm32f1/libopeninv_stm32f1/src platform/stm32f1/inverter +vpath %.cpp src/ libopeninv/src platform/stm32f1/libopeninv_stm32f1/src platform/stm32f1/inverter OPENOCD_BASE = /usr OPENOCD = $(OPENOCD_BASE)/bin/openocd diff --git a/libopeninv/CMakeLists.txt b/libopeninv/CMakeLists.txt index 44a2f7b..1ed7d14 100644 --- a/libopeninv/CMakeLists.txt +++ b/libopeninv/CMakeLists.txt @@ -16,22 +16,3 @@ target_include_directories(libopeninv # "Project" includes needed to customise the library build are pulled from the # top level include directory target_include_directories(libopeninv PRIVATE "${CMAKE_SOURCE_DIR}/include") - -if(PLATFORM STREQUAL "stm32f1") - add_library( - libopeninv_stm32f1 - src/anain.cpp - src/digio.cpp - src/param_save.cpp - src/stm32_can.cpp - src/stm32scheduler.cpp - src/terminal.cpp - src/terminalcommands.cpp) - target_link_libraries(libopeninv_stm32f1 global_options) - target_include_directories(libopeninv_stm32f1 - PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") - # "Project" includes needed to customise the library build are pulled from the - # top level include directory - target_include_directories(libopeninv_stm32f1 - PRIVATE "${CMAKE_SOURCE_DIR}/include") -endif() diff --git a/platform/stm32f1/CMakeLists.txt b/platform/stm32f1/CMakeLists.txt index 59c3cb8..1e2d498 100644 --- a/platform/stm32f1/CMakeLists.txt +++ b/platform/stm32f1/CMakeLists.txt @@ -1,2 +1,5 @@ # STM32F1 main inverter executable (stm32_sine) add_subdirectory(inverter) + +# STM32F1 specific parts of libopeninv +add_subdirectory(libopeninv_stm32f1) diff --git a/platform/stm32f1/libopeninv_stm32f1/CMakeLists.txt b/platform/stm32f1/libopeninv_stm32f1/CMakeLists.txt new file mode 100644 index 0000000..96f76a8 --- /dev/null +++ b/platform/stm32f1/libopeninv_stm32f1/CMakeLists.txt @@ -0,0 +1,16 @@ +add_library( + libopeninv_stm32f1 + src/anain.cpp + src/digio.cpp + src/param_save.cpp + src/stm32_can.cpp + src/stm32scheduler.cpp + src/terminal.cpp + src/terminalcommands.cpp) +target_link_libraries(libopeninv_stm32f1 global_options libopeninv) +target_include_directories(libopeninv_stm32f1 + PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") +# "Project" includes needed to customise the library build are pulled from the +# top level include directory +target_include_directories(libopeninv_stm32f1 + PRIVATE "${CMAKE_SOURCE_DIR}/include") diff --git a/libopeninv/include/anain.h b/platform/stm32f1/libopeninv_stm32f1/include/anain.h similarity index 100% rename from libopeninv/include/anain.h rename to platform/stm32f1/libopeninv_stm32f1/include/anain.h diff --git a/libopeninv/include/digio.h b/platform/stm32f1/libopeninv_stm32f1/include/digio.h similarity index 100% rename from libopeninv/include/digio.h rename to platform/stm32f1/libopeninv_stm32f1/include/digio.h diff --git a/libopeninv/include/param_save.h b/platform/stm32f1/libopeninv_stm32f1/include/param_save.h similarity index 100% rename from libopeninv/include/param_save.h rename to platform/stm32f1/libopeninv_stm32f1/include/param_save.h diff --git a/libopeninv/include/stm32_can.h b/platform/stm32f1/libopeninv_stm32f1/include/stm32_can.h similarity index 100% rename from libopeninv/include/stm32_can.h rename to platform/stm32f1/libopeninv_stm32f1/include/stm32_can.h diff --git a/libopeninv/include/stm32_loader.h b/platform/stm32f1/libopeninv_stm32f1/include/stm32_loader.h similarity index 100% rename from libopeninv/include/stm32_loader.h rename to platform/stm32f1/libopeninv_stm32f1/include/stm32_loader.h diff --git a/libopeninv/include/stm32scheduler.h b/platform/stm32f1/libopeninv_stm32f1/include/stm32scheduler.h similarity index 100% rename from libopeninv/include/stm32scheduler.h rename to platform/stm32f1/libopeninv_stm32f1/include/stm32scheduler.h diff --git a/libopeninv/include/terminal.h b/platform/stm32f1/libopeninv_stm32f1/include/terminal.h similarity index 100% rename from libopeninv/include/terminal.h rename to platform/stm32f1/libopeninv_stm32f1/include/terminal.h diff --git a/libopeninv/include/terminalcommands.h b/platform/stm32f1/libopeninv_stm32f1/include/terminalcommands.h similarity index 100% rename from libopeninv/include/terminalcommands.h rename to platform/stm32f1/libopeninv_stm32f1/include/terminalcommands.h diff --git a/libopeninv/src/anain.cpp b/platform/stm32f1/libopeninv_stm32f1/src/anain.cpp similarity index 100% rename from libopeninv/src/anain.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/anain.cpp diff --git a/libopeninv/src/digio.cpp b/platform/stm32f1/libopeninv_stm32f1/src/digio.cpp similarity index 100% rename from libopeninv/src/digio.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/digio.cpp diff --git a/libopeninv/src/param_save.cpp b/platform/stm32f1/libopeninv_stm32f1/src/param_save.cpp similarity index 100% rename from libopeninv/src/param_save.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/param_save.cpp diff --git a/libopeninv/src/stm32_can.cpp b/platform/stm32f1/libopeninv_stm32f1/src/stm32_can.cpp similarity index 100% rename from libopeninv/src/stm32_can.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/stm32_can.cpp diff --git a/libopeninv/src/stm32scheduler.cpp b/platform/stm32f1/libopeninv_stm32f1/src/stm32scheduler.cpp similarity index 100% rename from libopeninv/src/stm32scheduler.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/stm32scheduler.cpp diff --git a/libopeninv/src/terminal.cpp b/platform/stm32f1/libopeninv_stm32f1/src/terminal.cpp similarity index 100% rename from libopeninv/src/terminal.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/terminal.cpp diff --git a/libopeninv/src/terminalcommands.cpp b/platform/stm32f1/libopeninv_stm32f1/src/terminalcommands.cpp similarity index 100% rename from libopeninv/src/terminalcommands.cpp rename to platform/stm32f1/libopeninv_stm32f1/src/terminalcommands.cpp From 9e619c4812cec744e2f5e9d78df6b36d1812fccd Mon Sep 17 00:00:00 2001 From: "David J. Fiddes" Date: Fri, 11 Feb 2022 12:42:45 +0000 Subject: [PATCH 83/83] Fix gate drive PSU state message in C2000 inverter The C2000 inverter erroneously reported "Gate Drive PSU OFF" when it was in fact turned on. The PSU was in the desired state it was just the message. Tests: - Run on the Tesla M3 inverter and verify the updated message is displayed when running the inverter --- platform/c2000/inverter/inverter_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platform/c2000/inverter/inverter_main.cpp b/platform/c2000/inverter/inverter_main.cpp index 76b865c..73a54c1 100644 --- a/platform/c2000/inverter/inverter_main.cpp +++ b/platform/c2000/inverter/inverter_main.cpp @@ -93,7 +93,7 @@ void main(void) GPIO_writePin(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, 0); GPIO_setPadConfig(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, GPIO_PIN_TYPE_STD); GPIO_setDirectionMode(DEVICE_GPIO_PIN_GATE_PSU_ENABLE, GPIO_DIR_MODE_OUT); - printf("Gate Drive PSU OFF\n"); + printf("Gate Drive PSU ON\n"); // // Set up the gate drivers for PWM operation