adding dependencies
This commit is contained in:
Janosch
@@ -0,0 +1,165 @@
|
||||
GNU LESSER GENERAL PUBLIC LICENSE
|
||||
Version 3, 29 June 2007
|
||||
|
||||
Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
|
||||
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.
|
||||
@@ -0,0 +1,2 @@
|
||||
# libopeninv
|
||||
Generic modules that can be used in many projects
|
||||
@@ -0,0 +1,58 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef ANAIO_H_INCLUDED
|
||||
#define ANAIO_H_INCLUDED
|
||||
|
||||
#include <stdint.h>
|
||||
#include "anain_prj.h"
|
||||
|
||||
|
||||
class AnaIn
|
||||
{
|
||||
public:
|
||||
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
|
||||
|
||||
static void Start();
|
||||
void Configure(uint32_t port, uint8_t pin);
|
||||
uint16_t Get();
|
||||
|
||||
private:
|
||||
static uint16_t values[];
|
||||
static uint8_t channel_array[];
|
||||
|
||||
uint16_t GetIndex() { return firstValue - values; }
|
||||
static uint8_t AdcChFromPort(uint32_t command_port, int command_bit);
|
||||
static int median3(int a, int b, int c);
|
||||
|
||||
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
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* This file is part of the stm32-sine project.
|
||||
*
|
||||
* Copyright (C) 2021 David J. Fiddes <D.J@fiddes.net>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef __CRC8_H_
|
||||
#define __CRC8_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* 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_ */
|
||||
@@ -0,0 +1,41 @@
|
||||
/*
|
||||
* This file is part of the stm32-sine project.
|
||||
*
|
||||
* Copyright (C) 2021 David J. Fiddes <D.J@fiddes.net>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef DIGIO_H_INCLUDED
|
||||
#define DIGIO_H_INCLUDED
|
||||
|
||||
#include <libopencm3/stm32/gpio.h>
|
||||
#include "digio_prj.h"
|
||||
|
||||
namespace PinMode {
|
||||
enum PinMode
|
||||
{
|
||||
INPUT_PD,
|
||||
INPUT_PU,
|
||||
INPUT_FLT,
|
||||
INPUT_AIN,
|
||||
OUTPUT,
|
||||
OUTPUT_OD,
|
||||
LAST
|
||||
};
|
||||
}
|
||||
|
||||
class DigIo
|
||||
{
|
||||
public:
|
||||
#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
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef ERRORMESSAGE_H
|
||||
#define ERRORMESSAGE_H
|
||||
|
||||
#include "errormessage_prj.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef FOC_H
|
||||
#define FOC_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "my_fp.h"
|
||||
|
||||
class FOC
|
||||
{
|
||||
public:
|
||||
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);
|
||||
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);
|
||||
static s32fp sin;
|
||||
static s32fp cos;
|
||||
};
|
||||
|
||||
#endif // FOC_H
|
||||
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef FU_H_INCLUDED
|
||||
#define FU_H_INCLUDED
|
||||
|
||||
#include <stdint.h>
|
||||
#include "my_fp.h"
|
||||
|
||||
class MotorVoltage
|
||||
{
|
||||
public:
|
||||
static void SetBoost(uint32_t boost);
|
||||
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);
|
||||
|
||||
private:
|
||||
static void CalcFac();
|
||||
static uint32_t boost;
|
||||
static u32fp fac;
|
||||
static uint32_t maxAmp;
|
||||
static u32fp endFrq;
|
||||
static u32fp maxFrq;
|
||||
};
|
||||
|
||||
#endif // FU_H_INCLUDED
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* This file is part of the stm32-car project.
|
||||
*
|
||||
* Copyright (C) 2021 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef LINBUS_H
|
||||
#define LINBUS_H
|
||||
|
||||
|
||||
class LinBus
|
||||
{
|
||||
public:
|
||||
/** Default constructor */
|
||||
LinBus(uint32_t usart, int baudrate);
|
||||
void Request(uint8_t id, uint8_t* data, uint8_t len);
|
||||
bool HasReceived(uint8_t pid, uint8_t requiredLen);
|
||||
uint8_t* GetReceivedBytes() { return &recvBuffer[payloadIndex]; }
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
struct HwInfo
|
||||
{
|
||||
uint32_t usart;
|
||||
uint8_t dmatx;
|
||||
uint8_t dmarx;
|
||||
uint32_t port;
|
||||
uint16_t pin;
|
||||
};
|
||||
|
||||
static uint8_t Checksum(uint8_t pid, uint8_t* data, int len);
|
||||
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[12];
|
||||
};
|
||||
|
||||
#endif // LINBUS_H
|
||||
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef MY_FP_H_INCLUDED
|
||||
#define MY_FP_H_INCLUDED
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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_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))
|
||||
|
||||
#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, int fracDigits);
|
||||
u32fp fp_sqrt(u32fp rad);
|
||||
s32fp fp_ln(unsigned int x);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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 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)))
|
||||
|
||||
#endif // MY_MATH_H_INCLUDED
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef MY_STRING_H
|
||||
#define MY_STRING_H
|
||||
|
||||
#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 memset32(int* target, int value, int length);
|
||||
void my_strcpy(char *str1, const char *str2);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif // MY_STRING_H
|
||||
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
float GetFloat(PARAM_NUM ParamNum);
|
||||
bool GetBool(PARAM_NUM ParamNum);
|
||||
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);
|
||||
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
|
||||
void Change(Param::PARAM_NUM ParamNum);
|
||||
}
|
||||
|
||||
#endif //PARAM_H_INCLUDED
|
||||
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2018 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef PIREGULATOR_H
|
||||
#define PIREGULATOR_H
|
||||
|
||||
#include "my_fp.h"
|
||||
#include "my_math.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;
|
||||
}
|
||||
|
||||
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; }
|
||||
|
||||
s32fp GetRef() { return refVal; }
|
||||
|
||||
/** Set maximum controller output
|
||||
* \param val actuator saturation value
|
||||
*/
|
||||
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 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; }
|
||||
|
||||
/** Preload Integrator to yield a certain output
|
||||
* @pre SetCallingFrequency() and SetGains() must be called first
|
||||
*/
|
||||
void PreloadIntegrator(int32_t yieldedOutput) { esum = ki != 0 ? FP_FROMINT((yieldedOutput * frequency) / ki) : 0; }
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
int32_t kp; //!< Proportional controller gain
|
||||
int32_t ki; //!< Integral controller gain
|
||||
s32fp esum; //!< Integrator
|
||||
s32fp refVal; //!< control target
|
||||
int32_t frequency; //!< Calling frequency
|
||||
int32_t maxY; //!< upper actuator saturation value
|
||||
int32_t minY; //!< lower actuator saturation value
|
||||
};
|
||||
|
||||
#endif // PIREGULATOR_H
|
||||
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef PRINTF_H_INCLUDED
|
||||
#define PRINTF_H_INCLUDED
|
||||
|
||||
#if T_DEBUG
|
||||
#define debugf(...) printf(__VA_ARGS__)
|
||||
#else
|
||||
#define debugf(...)
|
||||
#endif
|
||||
|
||||
class IPutChar
|
||||
{
|
||||
public:
|
||||
virtual void PutChar(char c) = 0;
|
||||
};
|
||||
|
||||
|
||||
int printf(const char *format, ...);
|
||||
int sprintf(char *out, const char *format, ...);
|
||||
int fprintf(IPutChar* put, const char *format, ...);
|
||||
|
||||
|
||||
#endif // PRINTF_H_INCLUDED
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2016 Nail Güzel
|
||||
* Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
|
||||
#ifndef MAX_ITEMS
|
||||
#define MAX_ITEMS 70
|
||||
#endif // MAX_ITEMS_PER_MESSAGE
|
||||
|
||||
#ifndef MAX_MESSAGES
|
||||
#define MAX_MESSAGES 10
|
||||
#endif // MAX_MESSAGES
|
||||
|
||||
#ifndef SENDBUFFER_LEN
|
||||
#define SENDBUFFER_LEN 20
|
||||
#endif // SENDBUFFER_LEN
|
||||
|
||||
#ifndef MAX_USER_MESSAGES
|
||||
#define MAX_USER_MESSAGES 10
|
||||
#endif // MAX_USER_MESSAGES
|
||||
|
||||
|
||||
class CANIDMAP;
|
||||
class SENDBUFFER;
|
||||
|
||||
class Can
|
||||
{
|
||||
public:
|
||||
enum baudrates
|
||||
{
|
||||
Baud125, Baud250, Baud500, Baud800, Baud1000, BaudLast
|
||||
};
|
||||
|
||||
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); }
|
||||
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);
|
||||
void Save();
|
||||
void SetReceiveCallback(void (*recv)(uint32_t, uint32_t*));
|
||||
bool RegisterUserMessage(uint32_t canId);
|
||||
void ClearUserMessages();
|
||||
uint32_t GetLastRxTimestamp();
|
||||
int AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain);
|
||||
int AddRecv(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain);
|
||||
int AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset);
|
||||
int AddRecv(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset);
|
||||
int Remove(Param::PARAM_NUM param);
|
||||
bool FindMap(Param::PARAM_NUM param, uint32_t& canId, uint8_t& offset, uint8_t& length, float& gain, bool& rx);
|
||||
void IterateCanMap(void (*callback)(Param::PARAM_NUM, uint32_t, uint8_t, uint8_t, float, bool));
|
||||
void HandleRx(int fifo);
|
||||
void HandleTx();
|
||||
void SetNodeId(uint8_t id) { nodeId = id; }
|
||||
static Can* GetInterface(int index);
|
||||
|
||||
private:
|
||||
static volatile bool isSaving;
|
||||
|
||||
struct CANPOS
|
||||
{
|
||||
float gain;
|
||||
uint16_t mapParam;
|
||||
int8_t offset;
|
||||
uint8_t offsetBits;
|
||||
uint8_t numBits;
|
||||
uint8_t next;
|
||||
};
|
||||
|
||||
struct CANIDMAP
|
||||
{
|
||||
#ifdef CAN_EXT
|
||||
uint32_t canId;
|
||||
#else
|
||||
uint16_t canId;
|
||||
#endif // CAN_EXT
|
||||
uint8_t first;
|
||||
};
|
||||
|
||||
struct SENDBUFFER
|
||||
{
|
||||
uint32_t id;
|
||||
uint32_t len;
|
||||
uint32_t data[2];
|
||||
};
|
||||
|
||||
CANIDMAP canSendMap[MAX_MESSAGES];
|
||||
CANIDMAP canRecvMap[MAX_MESSAGES];
|
||||
CANPOS canPosMap[MAX_ITEMS + 1]; //Last item is a "tail"
|
||||
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;
|
||||
uint8_t nodeId;
|
||||
|
||||
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, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset);
|
||||
uint32_t SaveToFlash(uint32_t baseAddress, uint32_t* data, int len);
|
||||
int LoadFromFlash();
|
||||
CANIDMAP *FindById(CANIDMAP *canMap, uint32_t 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);
|
||||
void SetFilterBank29(int& idIndex, int& filterId, uint32_t* idList);
|
||||
uint32_t GetFlashAddress();
|
||||
|
||||
static Can* interfaces[];
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,46 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2018 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef STM32_LOADER_H_INCLUDED
|
||||
#define STM32_LOADER_H_INCLUDED
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
|
||||
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
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2017 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef STM32SCHEDULER_H
|
||||
#define STM32SCHEDULER_H
|
||||
#include <stdint.h>
|
||||
#include <libopencm3/stm32/timer.h>
|
||||
#include <libopencm3/cm3/nvic.h>
|
||||
#include <libopencm3/stm32/rcc.h>
|
||||
|
||||
#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 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
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef TERMINAL_H
|
||||
#define TERMINAL_H
|
||||
#include <stdint.h>
|
||||
#include "printf.h"
|
||||
|
||||
class Terminal;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
char const *cmd;
|
||||
void (*CmdFunc)(Terminal*, char*);
|
||||
} TERM_CMD;
|
||||
|
||||
class Terminal: public IPutChar
|
||||
{
|
||||
public:
|
||||
Terminal(uint32_t usart, const TERM_CMD* commands, bool remap = false);
|
||||
void SetNodeId(uint8_t id);
|
||||
void Run();
|
||||
void PutChar(char c);
|
||||
bool KeyPressed();
|
||||
void FlushInput();
|
||||
void DisableTxDMA();
|
||||
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);
|
||||
|
||||
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;
|
||||
bool txDmaEnabled;
|
||||
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];
|
||||
};
|
||||
|
||||
#endif // TERMINAL_H
|
||||
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2021 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef TERMINALCOMMANDS_H
|
||||
#define TERMINALCOMMANDS_H
|
||||
|
||||
|
||||
class TerminalCommands
|
||||
{
|
||||
public:
|
||||
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:
|
||||
|
||||
private:
|
||||
static void PrintCanMap(Param::PARAM_NUM param, uint32_t canid, uint8_t offset, uint8_t length, float gain, bool rx);
|
||||
};
|
||||
|
||||
#endif // TERMINALCOMMANDS_H
|
||||
Binary file not shown.
@@ -0,0 +1,171 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2010 Johannes Huebner <contact@johanneshuebner.com>
|
||||
* Copyright (C) 2010 Edward Cheeseman <cheesemanedward@gmail.com>
|
||||
* Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <libopencm3/stm32/gpio.h>
|
||||
#include <libopencm3/stm32/dma.h>
|
||||
#include <libopencm3/stm32/adc.h>
|
||||
#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))
|
||||
|
||||
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::Start()
|
||||
{
|
||||
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 (int i = 0; i < 80000; i++);
|
||||
|
||||
adc_reset_calibration(ADC1);
|
||||
adc_calibrate(ADC1);
|
||||
|
||||
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_CR2(ADC1) |= ADC_CR2_JSWSTART;
|
||||
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
|
||||
*
|
||||
* - NUM_SAMPLES = 1: Most recent sample is returned
|
||||
* - NUM_SAMPLES = 3: Median of last 3 samples is returned
|
||||
* - NUM_SAMPLES = 9: Median of last 3 medians is returned
|
||||
* - NUM_SAMPLES = 12: Average of last 4 medians is returned
|
||||
* - NUM_SAMPLES = 64: Average of last 64 samples is returned
|
||||
*
|
||||
* @return Filtered value
|
||||
*/
|
||||
uint16_t AnaIn::Get()
|
||||
{
|
||||
#if NUM_SAMPLES == 1
|
||||
return *firstValue;
|
||||
#elif NUM_SAMPLES == 3
|
||||
return MEDIAN3_FROM_ADC_ARRAY(firstValue);
|
||||
#elif NUM_SAMPLES == 9
|
||||
uint16_t *curVal = firstValue;
|
||||
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 = firstValue;
|
||||
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;
|
||||
#elif NUM_SAMPLES == 64
|
||||
uint16_t *curVal = firstValue;
|
||||
uint32_t sum = 0;
|
||||
|
||||
for (int i = 0; i < NUM_SAMPLES; i++, curVal += ANA_IN_COUNT)
|
||||
{
|
||||
sum += *curVal;
|
||||
}
|
||||
|
||||
return sum >> 6;
|
||||
#else
|
||||
#error NUM_SAMPLES must be 1, 3, 9, 12 or 64
|
||||
#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;
|
||||
}
|
||||
adc_enable_temperature_sensor();
|
||||
return 16;
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
* This file is part of the stm32-sine project.
|
||||
*
|
||||
* Copyright (C) 2021 David J. Fiddes <D.J@fiddes.net>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Generated table driven CRC-8-CCITT code from:
|
||||
* https://stackoverflow.com/a/27843120/6353
|
||||
*/
|
||||
|
||||
#include "crc8.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* 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)) };
|
||||
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2010 Johannes Huebner <contact@johanneshuebner.com>
|
||||
* Copyright (C) 2010 Edward Cheeseman <cheesemanedward@gmail.com>
|
||||
* Copyright (C) 2009 Uwe Hermann <uwe@hermann-uwe.de>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "digio.h"
|
||||
|
||||
#define DIG_IO_OFF 0
|
||||
#define DIG_IO_ON 1
|
||||
|
||||
#undef DIG_IO_ENTRY
|
||||
#define DIG_IO_ENTRY(name, port, pin, mode) DigIo DigIo::name;
|
||||
DIG_IO_LIST
|
||||
|
||||
void DigIo::Configure(uint32_t port, uint16_t pin, PinMode::PinMode pinMode)
|
||||
{
|
||||
uint8_t mode = GPIO_MODE_INPUT;
|
||||
uint8_t cnf = GPIO_CNF_INPUT_PULL_UPDOWN;
|
||||
uint16_t val = DIG_IO_OFF;
|
||||
|
||||
_port = port;
|
||||
_pin = pin;
|
||||
|
||||
switch (pinMode)
|
||||
{
|
||||
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;
|
||||
case PinMode::OUTPUT_OD:
|
||||
mode = GPIO_MODE_OUTPUT_50_MHZ;
|
||||
cnf = GPIO_CNF_OUTPUT_OPENDRAIN;
|
||||
val = DIG_IO_ON;
|
||||
break;
|
||||
}
|
||||
|
||||
if (DIG_IO_ON == val)
|
||||
{
|
||||
gpio_set(port, pin);
|
||||
}
|
||||
gpio_set_mode(port, mode, cnf, pin);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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_LAST },
|
||||
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);
|
||||
}
|
||||
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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)
|
||||
#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);
|
||||
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 zeroOffset = FP_FROMINT(1);
|
||||
static const int32_t modMax = FP_DIV(FP_FROMINT(2U), sqrt3);
|
||||
static const int32_t modMaxPow2 = modMax * modMax;
|
||||
static const int32_t minPulse = 1000;
|
||||
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)
|
||||
{
|
||||
//Clarke transformation
|
||||
s32fp ia = il1;
|
||||
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);
|
||||
}
|
||||
|
||||
/** \brief distribute motor current in magnetic torque and reluctance torque with the least total 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
|
||||
*
|
||||
*/
|
||||
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));
|
||||
}
|
||||
|
||||
/** \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
|
||||
*
|
||||
*/
|
||||
void FOC::InvParkClarke(int32_t ud, int32_t uq)
|
||||
{
|
||||
//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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2016 Nail Güzel
|
||||
* Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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
|
||||
|
||||
/** 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(float frq)
|
||||
{
|
||||
endFrq = FP_FROMFLT(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 < FP_FROMFLT(0.2))
|
||||
{
|
||||
amp = 0;
|
||||
}
|
||||
if (amp > maxAmp)
|
||||
{
|
||||
amp = maxAmp;
|
||||
}
|
||||
|
||||
return amp;
|
||||
}
|
||||
|
||||
void MotorVoltage::SetMaxAmp(uint32_t maxAmp)
|
||||
{
|
||||
MotorVoltage::maxAmp = maxAmp;
|
||||
CalcFac();
|
||||
}
|
||||
|
||||
/** Calculate slope of u/f */
|
||||
void MotorVoltage::CalcFac()
|
||||
{
|
||||
fac = FP_DIV(FP_FROMINT(maxAmp - boost), endFrq);
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
/*
|
||||
* This file is part of the stm32-... project.
|
||||
*
|
||||
* Copyright (C) 2021 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <libopencm3/stm32/dma.h>
|
||||
#include <libopencm3/stm32/usart.h>
|
||||
#include <libopencm3/stm32/gpio.h>
|
||||
#include "linbus.h"
|
||||
|
||||
#define HWINFO_ENTRIES (sizeof(hwInfo) / sizeof(struct HwInfo))
|
||||
|
||||
const LinBus::HwInfo LinBus::hwInfo[] =
|
||||
{
|
||||
{ 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 },
|
||||
};
|
||||
|
||||
|
||||
/** \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);
|
||||
|
||||
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);
|
||||
usart_enable_rx_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, 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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/** \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::Request(uint8_t id, uint8_t* data, uint8_t len)
|
||||
{
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
dma_enable_channel(DMA1, hw->dmarx);
|
||||
}
|
||||
|
||||
/** \brief Check whether we received valid data with given PID and length
|
||||
*
|
||||
* \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 id, uint8_t requiredLen)
|
||||
{
|
||||
int numRcvd = dma_get_number_of_data(DMA1, hw->dmarx);
|
||||
int receiveIdx = sizeof(recvBuffer) - numRcvd;
|
||||
|
||||
if (requiredLen > 8) return false;
|
||||
|
||||
uint8_t pid = Parity(id);
|
||||
|
||||
if (receiveIdx == (requiredLen + payloadIndex + 1) && recvBuffer[pidIndex] == pid)
|
||||
{
|
||||
uint8_t checksum = Checksum(recvBuffer[pidIndex], &recvBuffer[payloadIndex], requiredLen);
|
||||
|
||||
return checksum == recvBuffer[requiredLen + payloadIndex];
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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 fracDigits)
|
||||
{
|
||||
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 += (div / 2 + ((*str - '0') << fracDigits)) / div;
|
||||
div *= 10;
|
||||
}
|
||||
}
|
||||
|
||||
return sign * ((nat << fracDigits) + 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;
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
/*
|
||||
* This file is part of the slibopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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++;
|
||||
}
|
||||
|
||||
void memset32(int* target, int value, int length)
|
||||
{
|
||||
while (length--)
|
||||
*target++ = value;
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <libopencm3/stm32/flash.h>
|
||||
#include <libopencm3/stm32/desig.h>
|
||||
#include <libopencm3/stm32/crc.h>
|
||||
#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)
|
||||
|
||||
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;
|
||||
|
||||
static uint32_t GetFlashAddress()
|
||||
{
|
||||
uint32_t flashSize = desig_get_flash_size();
|
||||
|
||||
//Always save parameters to last flash page
|
||||
return FLASH_BASE + flashSize * 1024 - PARAM_BLKNUM * PARAM_BLKSIZE;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save parameters to flash
|
||||
*
|
||||
* @return CRC of parameter flash page
|
||||
*/
|
||||
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);
|
||||
|
||||
//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].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);
|
||||
}
|
||||
|
||||
parmPage.crc = crc_calculate_block(((uint32_t*)&parmPage), (2 * NUM_PARAMS));
|
||||
flash_unlock();
|
||||
|
||||
if (check != 0xFFFFFFFF)
|
||||
flash_erase_page(paramAddress);
|
||||
|
||||
for (idx = 0; idx < PARAM_WORDS; idx++)
|
||||
{
|
||||
uint32_t* pData = ((uint32_t*)&parmPage) + idx;
|
||||
flash_program_word(paramAddress + 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()
|
||||
{
|
||||
uint32_t paramAddress = GetFlashAddress();
|
||||
PARAM_PAGE *parmPage = (PARAM_PAGE *)paramAddress;
|
||||
|
||||
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 && parmPage->data[idxPage].key > 0)
|
||||
{
|
||||
Param::SetFixed(idx, parmPage->data[idxPage].value);
|
||||
Param::SetFlagsRaw(idx, parmPage->data[idxPage].flags);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
@@ -0,0 +1,246 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
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 integer value
|
||||
*
|
||||
* @param[in] ParamNum Parameter index
|
||||
* @return Parameters value
|
||||
*/
|
||||
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 FP_TOFLOAT(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 without range check and callback
|
||||
*
|
||||
* @param[in] ParamNum Parameter index
|
||||
* @param[in] ParamVal New value of parameter
|
||||
*/
|
||||
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] = FP_FROMFLT(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)
|
||||
SetFixed((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];
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2018 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "picontroller.h"
|
||||
#include "my_math.h"
|
||||
|
||||
PiController::PiController()
|
||||
: kp(0), ki(0), esum(0), refVal(0), frequency(1), maxY(0), minY(0)
|
||||
{
|
||||
}
|
||||
|
||||
int32_t PiController::Run(s32fp curVal)
|
||||
{
|
||||
s32fp err = refVal - curVal;
|
||||
s32fp esumTemp = esum + err;
|
||||
|
||||
int32_t y = FP_TOINT(err * kp + (esumTemp / frequency) * ki);
|
||||
int32_t ylim = MAX(y, minY);
|
||||
ylim = MIN(ylim, maxY);
|
||||
|
||||
if (ylim == y)
|
||||
{
|
||||
esum = esumTemp; //anti windup, only integrate when not saturated
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,239 @@
|
||||
/*
|
||||
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 <stdarg.h>
|
||||
#include "printf.h"
|
||||
#include "my_fp.h"
|
||||
|
||||
#define PAD_RIGHT 1
|
||||
#define PAD_ZERO 2
|
||||
|
||||
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)
|
||||
{
|
||||
int pc = 0, padchar = ' ';
|
||||
|
||||
if (width > 0) {
|
||||
int len = 0;
|
||||
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) {
|
||||
put->PutChar(padchar);
|
||||
++pc;
|
||||
}
|
||||
}
|
||||
for ( ; *string ; ++string) {
|
||||
put->PutChar(*string);
|
||||
++pc;
|
||||
}
|
||||
for ( ; width > 0; --width) {
|
||||
put->PutChar(padchar);
|
||||
++pc;
|
||||
}
|
||||
|
||||
return pc;
|
||||
}
|
||||
|
||||
/* the following should be enough for 32 bit int */
|
||||
#define PRINT_BUF_LEN 12
|
||||
|
||||
static int printi(IPutChar* put, int i, int b, int sg, int width, int pad, int letbase)
|
||||
{
|
||||
char print_buf[PRINT_BUF_LEN];
|
||||
char *s;
|
||||
int t, neg = 0, pc = 0;
|
||||
unsigned int u = i;
|
||||
|
||||
if (i == 0) {
|
||||
print_buf[0] = '0';
|
||||
print_buf[1] = '\0';
|
||||
return prints (put, 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) ) {
|
||||
put->PutChar('-');
|
||||
++pc;
|
||||
--width;
|
||||
}
|
||||
else {
|
||||
*--s = '-';
|
||||
}
|
||||
}
|
||||
|
||||
return pc + prints (put, s, width, 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 (put, print_buf, width, pad);
|
||||
}
|
||||
|
||||
static int print(IPutChar* put, const char *format, va_list args )
|
||||
{
|
||||
int width, pad;
|
||||
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' ) {
|
||||
char *s = (char *)va_arg( args, int );
|
||||
pc += prints (put, s?s:"(null)", width, pad);
|
||||
continue;
|
||||
}
|
||||
if( *format == 'd' ) {
|
||||
pc += printi (put, va_arg( args, int ), 10, 1, width, pad, 'a');
|
||||
continue;
|
||||
}
|
||||
if( *format == 'x' ) {
|
||||
pc += printi (put, va_arg( args, int ), 16, 0, width, pad, 'a');
|
||||
continue;
|
||||
}
|
||||
if( *format == 'X' ) {
|
||||
pc += printi (put, va_arg( args, int ), 16, 0, width, pad, 'A');
|
||||
continue;
|
||||
}
|
||||
if( *format == 'u' ) {
|
||||
pc += printi (put, va_arg( args, int ), 10, 0, width, pad, 'a');
|
||||
continue;
|
||||
}
|
||||
if ( *format == 'f' ) {
|
||||
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 (put, scr, width, pad);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else {
|
||||
out:
|
||||
put->PutChar(*format);
|
||||
++pc;
|
||||
}
|
||||
}
|
||||
va_end( args );
|
||||
return pc;
|
||||
}
|
||||
|
||||
int printf(const char *format, ...)
|
||||
{
|
||||
ExternPutChar pc;
|
||||
va_list args;
|
||||
|
||||
va_start( args, format );
|
||||
return print( &pc, format, args );
|
||||
}
|
||||
|
||||
int sprintf(char *out, const char *format, ...)
|
||||
{
|
||||
StringPutChar pc(out);
|
||||
va_list args;
|
||||
|
||||
va_start( args, format );
|
||||
|
||||
int ret = print( &pc, format, args );
|
||||
|
||||
pc.PutChar(0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int fprintf(IPutChar* put, const char *format, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start( args, format );
|
||||
|
||||
return print( put, format, args );
|
||||
}
|
||||
@@ -0,0 +1,191 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2012 Johannes Huebner <contact@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/** @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)))
|
||||
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/** @} */
|
||||
@@ -0,0 +1,978 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2016 Nail Güzel
|
||||
* Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <stdint.h>
|
||||
#include "hwdefs.h"
|
||||
#include "my_string.h"
|
||||
#include "my_math.h"
|
||||
#include "printf.h"
|
||||
#include <libopencm3/stm32/can.h>
|
||||
#include <libopencm3/stm32/gpio.h>
|
||||
#include <libopencm3/stm32/rcc.h>
|
||||
#include <libopencm3/stm32/flash.h>
|
||||
#include <libopencm3/stm32/crc.h>
|
||||
#include <libopencm3/stm32/rtc.h>
|
||||
#include <libopencm3/stm32/desig.h>
|
||||
#include <libopencm3/cm3/common.h>
|
||||
#include <libopencm3/cm3/nvic.h>
|
||||
#include "stm32_can.h"
|
||||
|
||||
#define MAX_INTERFACES 2
|
||||
#define IDS_PER_BANK 4
|
||||
#define EXT_IDS_PER_BANK 2
|
||||
#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(b) b
|
||||
#define RECVMAP_ADDRESS(b) (b + sizeof(canSendMap))
|
||||
#define POSMAP_ADDRESS(b) (b + sizeof(canSendMap) + sizeof(canRecvMap))
|
||||
#define CRC_ADDRESS(b) (b + sizeof(canSendMap) + sizeof(canRecvMap) + sizeof(canPosMap))
|
||||
#define SENDMAP_WORDS (sizeof(canSendMap) / (sizeof(uint32_t)))
|
||||
#define RECVMAP_WORDS (sizeof(canRecvMap) / (sizeof(uint32_t)))
|
||||
#define POSMAP_WORDS ((sizeof(CANPOS) * MAX_ITEMS) / (sizeof(uint32_t)))
|
||||
#define ITEM_UNSET 0xff
|
||||
#define forEachCanMap(c,m) for (CANIDMAP *c = m; (c - m) < MAX_MESSAGES && c->first != MAX_ITEMS; c++)
|
||||
#define forEachPosMap(c,m) for (CANPOS *c = &canPosMap[m->first]; c->next != ITEM_UNSET; c = &canPosMap[c->next])
|
||||
|
||||
#ifdef CAN_EXT
|
||||
#define IDMAPSIZE 8
|
||||
#else
|
||||
#define IDMAPSIZE 4
|
||||
#endif // CAN_EXT
|
||||
#if (MAX_ITEMS * 12 + 2 * MAX_MESSAGES * IDMAPSIZE + 4) > FLASH_PAGE_SIZE
|
||||
#error CANMAP will not fit in one flash page
|
||||
#endif
|
||||
|
||||
struct CAN_SDO
|
||||
{
|
||||
uint8_t cmd;
|
||||
uint16_t index;
|
||||
uint8_t subIndex;
|
||||
uint32_t data;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct CANSPEED
|
||||
{
|
||||
uint32_t ts1;
|
||||
uint32_t ts2;
|
||||
uint32_t prescaler;
|
||||
};
|
||||
|
||||
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] =
|
||||
{
|
||||
{ CAN_BTR_TS1_9TQ, CAN_BTR_TS2_6TQ, 18}, //125kbps
|
||||
{ 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
|
||||
};
|
||||
|
||||
/** \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 > 0x1fffffff
|
||||
* - CAN_ERR_INVALID_OFS Offset > 63
|
||||
* - CAN_ERR_INVALID_LEN Length > 32
|
||||
* - CAN_ERR_MAXMESSAGES Already 10 send messages defined
|
||||
* - CAN_ERR_MAXITEMS Already than MAX_ITEMS items total defined
|
||||
*/
|
||||
int Can::AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset)
|
||||
{
|
||||
return Add(canSendMap, param, canId, offsetBits, length, gain, offset);
|
||||
}
|
||||
|
||||
int Can::AddSend(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain)
|
||||
{
|
||||
return Add(canSendMap, param, canId, offsetBits, length, gain, 0);
|
||||
}
|
||||
|
||||
/** \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 > 0x1fffffff
|
||||
* - CAN_ERR_INVALID_OFS Offset > 63
|
||||
* - CAN_ERR_INVALID_LEN Length > 32
|
||||
* - CAN_ERR_MAXMESSAGES Already 10 receive messages defined
|
||||
* - CAN_ERR_MAXITEMS Already than MAX_ITEMS items total defined
|
||||
*/
|
||||
int Can::AddRecv(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset)
|
||||
{
|
||||
int res = Add(canRecvMap, param, canId, offsetBits, length, gain, offset);
|
||||
ConfigureFilters();
|
||||
return res;
|
||||
}
|
||||
|
||||
int Can::AddRecv(Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain)
|
||||
{
|
||||
return Can::AddRecv(param, canId, offsetBits, length, gain, 0);
|
||||
}
|
||||
|
||||
/** \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(uint32_t canId)
|
||||
{
|
||||
if (nextUserMessageIndex < MAX_USER_MESSAGES)
|
||||
{
|
||||
userIds[nextUserMessageIndex] = canId;
|
||||
nextUserMessageIndex++;
|
||||
ConfigureFilters();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/** \brief Remove all CAN Id from user message list
|
||||
*/
|
||||
void Can::ClearUserMessages()
|
||||
{
|
||||
nextUserMessageIndex = 0;
|
||||
ConfigureFilters();
|
||||
}
|
||||
|
||||
/** \brief Find first occurence of parameter in CAN map and output its mapping info
|
||||
*
|
||||
* Memory layout: SendMap, RecvMap, PosMap, CRC
|
||||
* Send/Recv maps point to items in PosMap. PosMap may point to next item
|
||||
*
|
||||
* \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, uint32_t& canId, uint8_t& offset, uint8_t& length, float& 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;
|
||||
}
|
||||
|
||||
/** \brief Save CAN mapping to flash
|
||||
*/
|
||||
void Can::Save()
|
||||
{
|
||||
uint32_t crc;
|
||||
uint32_t check = 0xFFFFFFFF;
|
||||
uint32_t baseAddress = GetFlashAddress();
|
||||
uint32_t *checkAddress = (uint32_t*)baseAddress;
|
||||
|
||||
isSaving = true;
|
||||
|
||||
for (int i = 0; i < FLASH_PAGE_SIZE / 4; i++, checkAddress++)
|
||||
check &= *checkAddress;
|
||||
|
||||
crc_reset();
|
||||
|
||||
flash_unlock();
|
||||
flash_set_ws(2);
|
||||
|
||||
if (check != 0xFFFFFFFF) //Only erase when needed
|
||||
flash_erase_page(baseAddress);
|
||||
|
||||
ReplaceParamEnumByUid(canSendMap);
|
||||
ReplaceParamEnumByUid(canRecvMap);
|
||||
|
||||
SaveToFlash(SENDMAP_ADDRESS(baseAddress), (uint32_t *)canSendMap, SENDMAP_WORDS);
|
||||
crc = SaveToFlash(RECVMAP_ADDRESS(baseAddress), (uint32_t *)canRecvMap, RECVMAP_WORDS);
|
||||
crc = SaveToFlash(POSMAP_ADDRESS(baseAddress), (uint32_t *)canPosMap, POSMAP_WORDS);
|
||||
SaveToFlash(CRC_ADDRESS(baseAddress), &crc, 1);
|
||||
flash_lock();
|
||||
|
||||
ReplaceParamUidByEnum(canSendMap);
|
||||
ReplaceParamUidByEnum(canRecvMap);
|
||||
|
||||
isSaving = false;
|
||||
}
|
||||
|
||||
/** \brief Send all defined messages
|
||||
*/
|
||||
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)
|
||||
{
|
||||
if (isSaving) return; //Only send mapped messages when not currently saving to flash
|
||||
|
||||
float val = Param::GetFloat((Param::PARAM_NUM)curPos->mapParam);
|
||||
|
||||
val *= curPos->gain;
|
||||
val += curPos->offset;
|
||||
int ival = val;
|
||||
ival &= ((1 << curPos->numBits) - 1);
|
||||
|
||||
if (curPos->offsetBits > 31)
|
||||
{
|
||||
data[1] |= ival << (curPos->offsetBits - 32);
|
||||
}
|
||||
else
|
||||
{
|
||||
data[0] |= ival << curPos->offsetBits;
|
||||
}
|
||||
}
|
||||
|
||||
Send(curMap->canId, data);
|
||||
}
|
||||
}
|
||||
|
||||
/** \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);
|
||||
removed += RemoveFromMap(canRecvMap, param);
|
||||
|
||||
return removed;
|
||||
}
|
||||
|
||||
/** \brief Init can hardware with given baud rate
|
||||
* Initializes the following sub systems:
|
||||
* - CAN hardware itself
|
||||
* - Appropriate GPIO pins
|
||||
* - Enables appropriate interrupts in NVIC
|
||||
*
|
||||
* \param baseAddr base address of CAN peripheral, CAN1 or CAN2
|
||||
* \param baudrate enum baudrates
|
||||
* \param remap use remapped IO pins
|
||||
* \return void
|
||||
*
|
||||
*/
|
||||
Can::Can(uint32_t baseAddr, enum baudrates baudrate, bool remap)
|
||||
: lastRxTimestamp(0), sendCnt(0), recvCallback(DummyCallback), nextUserMessageIndex(0), canDev(baseAddr)
|
||||
{
|
||||
Clear();
|
||||
LoadFromFlash();
|
||||
|
||||
switch (baseAddr)
|
||||
{
|
||||
case CAN1:
|
||||
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
|
||||
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:
|
||||
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);
|
||||
}
|
||||
|
||||
//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;
|
||||
}
|
||||
|
||||
nodeId = 1;
|
||||
// Reset CAN
|
||||
can_reset(canDev);
|
||||
|
||||
SetBaudrate(baudrate);
|
||||
ConfigureFilters();
|
||||
// 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
|
||||
*
|
||||
* \param baudrate enum baudrates
|
||||
* \return void
|
||||
*
|
||||
*/
|
||||
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(canDev,
|
||||
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);
|
||||
}
|
||||
|
||||
/** \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
|
||||
* \param len message length
|
||||
* \return void
|
||||
*
|
||||
*/
|
||||
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, canId > 0x7FF, 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++;
|
||||
}
|
||||
|
||||
if (sendCnt > 0)
|
||||
{
|
||||
can_enable_irq(canDev, CAN_IER_TMEIE);
|
||||
}
|
||||
}
|
||||
|
||||
void Can::IterateCanMap(void (*callback)(Param::PARAM_NUM, uint32_t, uint8_t, uint8_t, float, 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;
|
||||
}
|
||||
}
|
||||
|
||||
Can* Can::GetInterface(int index)
|
||||
{
|
||||
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];
|
||||
|
||||
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
|
||||
{
|
||||
ProcessSDO(data);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (isSaving) continue; //Only handle mapped messages when not currently saving to flash
|
||||
|
||||
CANIDMAP *recvMap = FindById(canRecvMap, id);
|
||||
|
||||
if (0 != recvMap)
|
||||
{
|
||||
forEachPosMap(curPos, recvMap)
|
||||
{
|
||||
float val;
|
||||
|
||||
if (curPos->offsetBits > 31)
|
||||
{
|
||||
val = (data[1] >> (curPos->offsetBits - 32)) & ((1 << curPos->numBits) - 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
val = (data[0] >> curPos->offsetBits) & ((1 << curPos->numBits) - 1);
|
||||
}
|
||||
val += curPos->offset;
|
||||
val *= curPos->gain;
|
||||
|
||||
if (Param::IsParam((Param::PARAM_NUM)curPos->mapParam))
|
||||
Param::Set((Param::PARAM_NUM)curPos->mapParam, FP_FROMFLT(val));
|
||||
else
|
||||
Param::SetFloat((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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Can::HandleTx()
|
||||
{
|
||||
SENDBUFFER* b = sendBuffer; //alias
|
||||
|
||||
while (sendCnt > 0 && can_transmit(canDev, b[sendCnt - 1].id, b[sendCnt - 1].id > 0x7FF, false, b[sendCnt - 1].len, (uint8_t*)b[sendCnt - 1].data) >= 0)
|
||||
sendCnt--;
|
||||
|
||||
if (sendCnt == 0)
|
||||
{
|
||||
can_disable_irq(canDev, CAN_IER_TMEIE);
|
||||
}
|
||||
}
|
||||
|
||||
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->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(paramIdx, 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(paramIdx);
|
||||
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(0x580 + nodeId, data);
|
||||
}
|
||||
|
||||
void Can::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;
|
||||
}
|
||||
|
||||
void Can::SetFilterBank29(int& idIndex, int& filterId, uint32_t* idList)
|
||||
{
|
||||
can_filter_id_list_32bit_init(
|
||||
filterId,
|
||||
(idList[0] << 3) | 0x4, //filter extended
|
||||
(idList[1] << 3) | 0x4,
|
||||
filterId & 1,
|
||||
true);
|
||||
idIndex = 0;
|
||||
filterId++;
|
||||
idList[0] = idList[1] = 0;
|
||||
}
|
||||
|
||||
void Can::ConfigureFilters()
|
||||
{
|
||||
uint16_t idList[IDS_PER_BANK] = { 0, 0, 0, 0 };
|
||||
uint32_t extIdList[EXT_IDS_PER_BANK] = { 0, 0 };
|
||||
int idIndex = 1, extIdIndex = 0;
|
||||
int filterId = canDev == CAN1 ? 0 : ((CAN_FMR(CAN2) >> 8) & 0x3F);
|
||||
|
||||
for (int i = 0; i < nextUserMessageIndex; i++)
|
||||
{
|
||||
if (userIds[i] > 0x7ff)
|
||||
{
|
||||
extIdList[extIdIndex] = userIds[i];
|
||||
extIdIndex++;
|
||||
}
|
||||
else
|
||||
{
|
||||
idList[idIndex] = userIds[i];
|
||||
idIndex++;
|
||||
}
|
||||
|
||||
if (idIndex == IDS_PER_BANK)
|
||||
{
|
||||
SetFilterBank(idIndex, filterId, idList);
|
||||
}
|
||||
if (extIdIndex == EXT_IDS_PER_BANK)
|
||||
{
|
||||
SetFilterBank29(extIdIndex, filterId, extIdList);
|
||||
}
|
||||
}
|
||||
|
||||
forEachCanMap(curMap, canRecvMap)
|
||||
{
|
||||
if (curMap->canId > 0x7ff)
|
||||
{
|
||||
extIdList[extIdIndex] = curMap->canId;
|
||||
extIdIndex++;
|
||||
}
|
||||
else
|
||||
{
|
||||
idList[idIndex] = curMap->canId;
|
||||
idIndex++;
|
||||
}
|
||||
|
||||
if (idIndex == IDS_PER_BANK)
|
||||
{
|
||||
SetFilterBank(idIndex, filterId, idList);
|
||||
}
|
||||
if (extIdIndex == EXT_IDS_PER_BANK)
|
||||
{
|
||||
SetFilterBank29(extIdIndex, filterId, extIdList);
|
||||
}
|
||||
}
|
||||
|
||||
//loop terminates before adding last set of filters
|
||||
if (idIndex > 0)
|
||||
{
|
||||
SetFilterBank(idIndex, filterId, idList);
|
||||
}
|
||||
if (extIdIndex > 0)
|
||||
{
|
||||
SetFilterBank29(extIdIndex, filterId, extIdList);
|
||||
}
|
||||
}
|
||||
|
||||
int Can::LoadFromFlash()
|
||||
{
|
||||
uint32_t baseAddress = GetFlashAddress();
|
||||
uint32_t storedCrc = *(uint32_t*)CRC_ADDRESS(baseAddress);
|
||||
uint32_t crc;
|
||||
|
||||
crc_reset();
|
||||
crc = crc_calculate_block((uint32_t*)baseAddress, SENDMAP_WORDS + RECVMAP_WORDS + POSMAP_WORDS);
|
||||
|
||||
if (storedCrc == crc)
|
||||
{
|
||||
memcpy32((int*)canSendMap, (int*)SENDMAP_ADDRESS(baseAddress), SENDMAP_WORDS);
|
||||
memcpy32((int*)canRecvMap, (int*)RECVMAP_ADDRESS(baseAddress), RECVMAP_WORDS);
|
||||
memcpy32((int*)canPosMap, (int*)POSMAP_ADDRESS(baseAddress), POSMAP_WORDS);
|
||||
ReplaceParamUidByEnum(canSendMap);
|
||||
ReplaceParamUidByEnum(canRecvMap);
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int Can::RemoveFromMap(CANIDMAP *canMap, Param::PARAM_NUM param)
|
||||
{
|
||||
int removed = 0;
|
||||
CANPOS *lastPosMap;
|
||||
|
||||
forEachCanMap(curMap, canMap)
|
||||
{
|
||||
lastPosMap = 0;
|
||||
|
||||
forEachPosMap(curPos, curMap)
|
||||
{
|
||||
if (curPos->mapParam == param)
|
||||
{
|
||||
if (lastPosMap != 0)
|
||||
{
|
||||
lastPosMap->next = curPos->next;
|
||||
}
|
||||
else
|
||||
{
|
||||
curMap->first = curPos->next;
|
||||
}
|
||||
}
|
||||
lastPosMap = curPos;
|
||||
}
|
||||
}
|
||||
|
||||
return removed;
|
||||
}
|
||||
|
||||
int Can::Add(CANIDMAP *canMap, Param::PARAM_NUM param, uint32_t canId, uint8_t offsetBits, uint8_t length, float gain, int8_t offset)
|
||||
{
|
||||
if (canId > 0x1fffffff) return CAN_ERR_INVALID_ID;
|
||||
if (offsetBits > 63) return CAN_ERR_INVALID_OFS;
|
||||
if (length > 32) return CAN_ERR_INVALID_LEN;
|
||||
|
||||
CANIDMAP *existingMap = FindById(canMap, canId);
|
||||
|
||||
if (0 == existingMap)
|
||||
{
|
||||
for (int i = 0; i < MAX_MESSAGES; i++)
|
||||
{
|
||||
if (canMap[i].first == MAX_ITEMS)
|
||||
{
|
||||
existingMap = &canMap[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (0 == existingMap)
|
||||
return CAN_ERR_MAXMESSAGES;
|
||||
|
||||
existingMap->canId = canId;
|
||||
}
|
||||
|
||||
int freeIndex;
|
||||
|
||||
for (freeIndex = 0; freeIndex < MAX_ITEMS && canPosMap[freeIndex].next != ITEM_UNSET; freeIndex++);
|
||||
|
||||
if (freeIndex == MAX_ITEMS)
|
||||
return CAN_ERR_MAXITEMS;
|
||||
|
||||
CANPOS* precedingItem = 0;
|
||||
|
||||
for (int precedingIndex = existingMap->first; precedingIndex != MAX_ITEMS; precedingIndex = canPosMap[precedingIndex].next)
|
||||
precedingItem = &canPosMap[precedingIndex];
|
||||
|
||||
CANPOS* freeItem = &canPosMap[freeIndex];
|
||||
freeItem->mapParam = param;
|
||||
freeItem->gain = gain;
|
||||
freeItem->offset = offset;
|
||||
freeItem->offsetBits = offsetBits;
|
||||
freeItem->numBits = length;
|
||||
freeItem->next = MAX_ITEMS;
|
||||
|
||||
if (precedingItem == 0) //first item for this can ID
|
||||
{
|
||||
existingMap->first = freeIndex;
|
||||
}
|
||||
else
|
||||
{
|
||||
precedingItem->next = freeIndex;
|
||||
}
|
||||
|
||||
int count = 0;
|
||||
|
||||
forEachCanMap(curMap, canMap)
|
||||
count++;
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
void Can::ClearMap(CANIDMAP *canMap)
|
||||
{
|
||||
for (int i = 0; i < MAX_MESSAGES; i++)
|
||||
{
|
||||
canMap[i].first = MAX_ITEMS;
|
||||
}
|
||||
|
||||
//Initialize also tail to ITEM_UNSET
|
||||
for (int i = 0; i < (MAX_ITEMS + 1); i++)
|
||||
{
|
||||
canPosMap[i].next = ITEM_UNSET;
|
||||
}
|
||||
}
|
||||
|
||||
Can::CANIDMAP* Can::FindById(CANIDMAP *canMap, uint32_t canId)
|
||||
{
|
||||
forEachCanMap(curMap, canMap)
|
||||
{
|
||||
if (curMap->canId == canId)
|
||||
return curMap;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t Can::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;
|
||||
}
|
||||
|
||||
int Can::CopyIdMapExcept(CANIDMAP *source, CANIDMAP *dest, Param::PARAM_NUM param)
|
||||
{
|
||||
int i = 0, removed = 0;
|
||||
int j = 0;
|
||||
|
||||
forEachCanMap(curMap, source)
|
||||
{
|
||||
bool discardId = true;
|
||||
|
||||
forEachPosMap(curPos, curMap)
|
||||
{
|
||||
if (curPos->mapParam != param)
|
||||
{
|
||||
discardId = false;
|
||||
canPosMap[j] = *curPos;
|
||||
j++;
|
||||
}
|
||||
else
|
||||
{
|
||||
removed++;
|
||||
}
|
||||
}
|
||||
|
||||
if (!discardId)
|
||||
{
|
||||
dest[i].canId = curMap->canId;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
return removed;
|
||||
}
|
||||
|
||||
void Can::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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Can::ReplaceParamUidByEnum(CANIDMAP *canMap)
|
||||
{
|
||||
forEachCanMap(curMap, canMap)
|
||||
{
|
||||
forEachPosMap(curPos, curMap)
|
||||
{
|
||||
Param::PARAM_NUM param = Param::NumFromId(curPos->mapParam);
|
||||
curPos->mapParam = param;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t Can::GetFlashAddress()
|
||||
{
|
||||
uint32_t flashSize = desig_get_flash_size();
|
||||
|
||||
//Always save CAN mapping to second-to-last flash page
|
||||
return FLASH_BASE + flashSize * 1024 - FLASH_PAGE_SIZE * (canDev == CAN1 ? CAN1_BLKNUM : CAN2_BLKNUM);
|
||||
}
|
||||
|
||||
/* 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();
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2017 Johannes Huebner <contact@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "stm32scheduler.h"
|
||||
|
||||
/* return CCRc of TIMt */
|
||||
#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 };
|
||||
|
||||
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);
|
||||
|
||||
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 * 100;
|
||||
|
||||
/* 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 < nextTask; i++)
|
||||
{
|
||||
if (timer_get_flag(timer, TIM_SR_CC1IF << i))
|
||||
{
|
||||
uint16_t start = timer_get_counter(timer);
|
||||
|
||||
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 < nextTask; i++)
|
||||
{
|
||||
int load = (10 * execTicks[i]) / periods[i];
|
||||
totalLoad += load;
|
||||
}
|
||||
return totalLoad;
|
||||
}
|
||||
@@ -0,0 +1,310 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2011 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "my_string.h"
|
||||
#include <libopencm3/cm3/common.h>
|
||||
#include <libopencm3/stm32/usart.h>
|
||||
#include <libopencm3/stm32/gpio.h>
|
||||
#include <libopencm3/stm32/dma.h>
|
||||
#include "terminal.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 },
|
||||
{ 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;
|
||||
|
||||
Terminal::Terminal(uint32_t usart, const TERM_CMD* commands, bool remap)
|
||||
: usart(usart),
|
||||
remap(remap),
|
||||
termCmds(commands),
|
||||
nodeId(1),
|
||||
enabled(true),
|
||||
txDmaEnabled(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 Terminal::Run()
|
||||
{
|
||||
int numRcvd = dma_get_number_of_data(DMA1, hw->dmarx);
|
||||
int currentIdx = bufSize - numRcvd;
|
||||
|
||||
if (0 == numRcvd)
|
||||
ResetDMA();
|
||||
|
||||
while (lastIdx < currentIdx) //echo
|
||||
usart_send_blocking(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 = 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
|
||||
{
|
||||
pCurCmd = CmdLookup(inBuf);
|
||||
}
|
||||
|
||||
ResetDMA();
|
||||
|
||||
if (NULL != pCurCmd)
|
||||
{
|
||||
usart_wait_send_ready(usart);
|
||||
pCurCmd->CmdFunc(this, args);
|
||||
}
|
||||
else if (currentIdx > 1 && enabled)
|
||||
{
|
||||
Send("Unknown command sequence\r\n");
|
||||
}
|
||||
}
|
||||
else if (inBuf[0] == '!' && NULL != pCurCmd)
|
||||
{
|
||||
ResetDMA();
|
||||
lastIdx = 0;
|
||||
pCurCmd->CmdFunc(this, args);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Terminal::SetNodeId(uint8_t id)
|
||||
{
|
||||
char one[] = { '1', 0 };
|
||||
char buf[4];
|
||||
nodeId = id;
|
||||
|
||||
if (nodeId != 1)
|
||||
{
|
||||
Send("Disabling terminal, type 'enableuart ");
|
||||
my_ltoa(buf, id, 10);
|
||||
Send(buf);
|
||||
Send("' 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).
|
||||
* 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.
|
||||
*/
|
||||
void Terminal::PutChar(char c)
|
||||
{
|
||||
if (!txDmaEnabled)
|
||||
{
|
||||
usart_send_blocking(usart, c);
|
||||
}
|
||||
else if (c == '\n' || curIdx == (bufSize - 1))
|
||||
{
|
||||
outBuf[curBuf][curIdx] = c;
|
||||
|
||||
while (!dma_get_interrupt_flag(DMA1, hw->dmatx, DMA_TCIF) && !firstSend);
|
||||
|
||||
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
|
||||
firstSend = false; //only needed once so we don't get stuck in the while loop above
|
||||
curIdx = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
outBuf[curBuf][curIdx] = c;
|
||||
curIdx++;
|
||||
}
|
||||
}
|
||||
|
||||
bool Terminal::KeyPressed()
|
||||
{
|
||||
return usart_get_flag(usart, USART_SR_RXNE);
|
||||
}
|
||||
|
||||
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);
|
||||
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++)
|
||||
{
|
||||
if (0 == my_strcmp(buf, pCmd->cmd))
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (NULL == pCmd->cmd)
|
||||
{
|
||||
pCmd = NULL;
|
||||
}
|
||||
return pCmd;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -0,0 +1,426 @@
|
||||
/*
|
||||
* This file is part of the libopeninv project.
|
||||
*
|
||||
* Copyright (C) 2021 Johannes Huebner <dev@johanneshuebner.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <libopencm3/cm3/scb.h>
|
||||
#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"
|
||||
|
||||
static Terminal* curTerm = NULL;
|
||||
|
||||
void TerminalCommands::ParamSet(Terminal* term, char* arg)
|
||||
{
|
||||
char *pParamVal;
|
||||
s32fp val;
|
||||
Param::PARAM_NUM idx;
|
||||
|
||||
arg = my_trim(arg);
|
||||
pParamVal = (char *)my_strchr(arg, ' ');
|
||||
|
||||
if (*pParamVal == 0)
|
||||
{
|
||||
fprintf(term, "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))
|
||||
{
|
||||
fprintf(term, "Set OK\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Value out of range\r\n");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Unknown parameter %s\r\n", arg);
|
||||
}
|
||||
}
|
||||
|
||||
void TerminalCommands::ParamGet(Terminal* term, 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);
|
||||
fprintf(term, "%f\r\n", val);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Unknown parameter: '%s'\r\n", arg);
|
||||
}
|
||||
|
||||
*comma = orig;
|
||||
arg = comma + 1;
|
||||
} while (',' == *comma);
|
||||
}
|
||||
|
||||
void TerminalCommands::ParamFlag(Terminal* term, char *arg)
|
||||
{
|
||||
char *pFlagVal;
|
||||
Param::PARAM_NUM idx;
|
||||
|
||||
arg = my_trim(arg);
|
||||
pFlagVal = (char *)my_strchr(arg, ' ');
|
||||
|
||||
if (*pFlagVal == 0)
|
||||
{
|
||||
fprintf(term, "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);
|
||||
}
|
||||
fprintf(term, "Flag change OK\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Unknown flag\r\n");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Unknown parameter %s\r\n", arg);
|
||||
}
|
||||
}
|
||||
|
||||
void TerminalCommands::ParamStream(Terminal* term, 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)
|
||||
{
|
||||
fprintf(term, "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
|
||||
{
|
||||
fprintf(term, "Unknown parameter\r\n");
|
||||
}
|
||||
} while (',' == *comma && curIndex < maxIndex);
|
||||
|
||||
maxIndex = curIndex;
|
||||
term->FlushInput();
|
||||
|
||||
while (!term->KeyPressed() && (repetitions > 0 || repetitions == -1))
|
||||
{
|
||||
comma = (char*)"";
|
||||
for (curIndex = 0; curIndex < maxIndex; curIndex++)
|
||||
{
|
||||
s32fp val = Param::Get(indexes[curIndex]);
|
||||
fprintf(term, "%s%f", comma, val);
|
||||
comma = (char*)",";
|
||||
}
|
||||
fprintf(term, "\r\n");
|
||||
if (repetitions != -1)
|
||||
repetitions--;
|
||||
}
|
||||
}
|
||||
|
||||
void TerminalCommands::PrintParamsJson(Terminal* term, char *arg)
|
||||
{
|
||||
arg = my_trim(arg);
|
||||
|
||||
const Param::Attributes *pAtr;
|
||||
char comma = ' ';
|
||||
bool printHidden = arg[0] == 'h';
|
||||
|
||||
fprintf(term, "{");
|
||||
for (uint32_t idx = 0; idx < Param::PARAM_LAST; idx++)
|
||||
{
|
||||
uint32_t canId;
|
||||
uint8_t canOffset, canLength;
|
||||
bool isRx;
|
||||
float canGain;
|
||||
pAtr = Param::GetAttrib((Param::PARAM_NUM)idx);
|
||||
|
||||
if ((Param::GetFlag((Param::PARAM_NUM)idx) & Param::FLAG_HIDDEN) == 0 || printHidden)
|
||||
{
|
||||
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))
|
||||
{
|
||||
fprintf(term, "\"canid\":%d,\"canoffset\":%d,\"canlength\":%d,\"cangain\":%f,\"isrx\":%s,",
|
||||
canId, canOffset, canLength, FP_FROMFLT(canGain), isRx ? "true" : "false");
|
||||
}
|
||||
|
||||
if (Param::IsParam((Param::PARAM_NUM)idx))
|
||||
{
|
||||
fprintf(term, "\"isparam\":true,\"minimum\":%f,\"maximum\":%f,\"default\":%f,\"category\":\"%s\",\"i\":%d}",
|
||||
pAtr->min, pAtr->max, pAtr->def, pAtr->category, idx);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "\"isparam\":false}");
|
||||
}
|
||||
comma = ',';
|
||||
}
|
||||
}
|
||||
fprintf(term, "\r\n}\r\n");
|
||||
}
|
||||
|
||||
//cantx param id offset len gain
|
||||
void TerminalCommands::MapCan(Terminal* term, 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')
|
||||
{
|
||||
while (curTerm != NULL); //lock
|
||||
curTerm = term;
|
||||
Can::GetInterface(0)->IterateCanMap(PrintCanMap);
|
||||
curTerm = NULL;
|
||||
return;
|
||||
}
|
||||
|
||||
if (arg[0] == 'c')
|
||||
{
|
||||
Can::GetInterface(0)->Clear();
|
||||
fprintf(term, "All message definitions cleared\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
op = arg[0];
|
||||
arg = (char *)my_strchr(arg, ' ');
|
||||
|
||||
if (0 == *arg)
|
||||
{
|
||||
fprintf(term, "Missing argument\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
arg = my_trim(arg);
|
||||
ending = (char *)my_strchr(arg, ' ');
|
||||
|
||||
if (*ending == 0 && op != 'd')
|
||||
{
|
||||
fprintf(term, "Missing argument\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
*ending = 0;
|
||||
paramIdx = Param::NumFromString(arg);
|
||||
arg = my_trim(ending + 1);
|
||||
|
||||
if (Param::PARAM_INVALID == paramIdx)
|
||||
{
|
||||
fprintf(term, "Unknown parameter\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (op == 'd')
|
||||
{
|
||||
result = Can::GetInterface(0)->Remove(paramIdx);
|
||||
fprintf(term, "%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))
|
||||
{
|
||||
fprintf(term, "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:
|
||||
fprintf(term, "Invalid CAN Id %x\r\n", values[0]);
|
||||
break;
|
||||
case CAN_ERR_INVALID_OFS:
|
||||
fprintf(term, "Invalid Offset %d\r\n", values[1]);
|
||||
break;
|
||||
case CAN_ERR_INVALID_LEN:
|
||||
fprintf(term, "Invalid length %d\r\n", values[2]);
|
||||
break;
|
||||
case CAN_ERR_MAXITEMS:
|
||||
fprintf(term, "Cannot map anymore items to CAN id %d\r\n", values[0]);
|
||||
break;
|
||||
case CAN_ERR_MAXMESSAGES:
|
||||
fprintf(term, "Max message count reached\r\n");
|
||||
break;
|
||||
default:
|
||||
fprintf(term, "CAN map successful, %d message%s active\r\n", result, result > 1 ? "s" : "");
|
||||
}
|
||||
}
|
||||
|
||||
void TerminalCommands::SaveParameters(Terminal* term, char *arg)
|
||||
{
|
||||
arg = arg;
|
||||
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)
|
||||
{
|
||||
arg = arg;
|
||||
if (0 == parm_load())
|
||||
{
|
||||
Param::Change((Param::PARAM_NUM)0);
|
||||
fprintf(term, "Parameters loaded\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(term, "Parameter CRC error\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
void TerminalCommands::Reset(Terminal* term, char *arg)
|
||||
{
|
||||
term = term;
|
||||
arg = arg;
|
||||
scb_reset_system();
|
||||
}
|
||||
|
||||
void TerminalCommands::PrintCanMap(Param::PARAM_NUM param, uint32_t canid, uint8_t offset, uint8_t length, float gain, bool rx)
|
||||
{
|
||||
const char* name = Param::GetAttrib(param)->name;
|
||||
fprintf(curTerm, "can ");
|
||||
|
||||
if (rx)
|
||||
fprintf(curTerm, "rx ");
|
||||
else
|
||||
fprintf(curTerm, "tx ");
|
||||
fprintf(curTerm, "%s %d %d %d %f\r\n", name, canid, offset, length, FP_FROMFLT(gain));
|
||||
}
|
||||
Reference in New Issue
Block a user