Skudak/stm32-openinverter-teslacharger
2
0
Fork 0
Code Issues Pull Requests 2 Actions Packages Projects Releases Wiki Activity

adding dependencies

Browse Source
This commit is contained in:
Janosch
2022-10-10 12:39:30 +01:00
parent 0e8e1e4db6
commit 2327ae7aab
867 changed files with 137599 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libopeninv/src/anain.cpp
+171
View File
@@ -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;
}
libopeninv/src/crc8.cpp
+74
View File
@@ -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)) };
libopeninv/src/digio.cpp
+71
View File
@@ -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);
}
libopeninv/src/errormessage.cpp
+125
View File
@@ -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);
}
libopeninv/src/foc.cpp
+177
View File
@@ -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;
}
libopeninv/src/fu.cpp
+73
View File
@@ -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);
}
libopeninv/src/linbus.cpp
+167
View File
@@ -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;
}
libopeninv/src/my_fp.c
+141
View File
@@ -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;
}
libopeninv/src/my_string.c
+148
View File
@@ -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;
}
libopeninv/src/param_save.cpp
+126
View File
@@ -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;
}
libopeninv/src/params.cpp
+246
View File
@@ -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];
}
}
libopeninv/src/picontroller.cpp
+53
View File
@@ -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;
}
libopeninv/src/printf.cpp
+239
View File
@@ -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 );
}
libopeninv/src/sine_core.cpp
+191
View File
@@ -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;
}
/** @} */
libopeninv/src/stm32_can.cpp
+978
View File
@@ -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();
}
libopeninv/src/stm32scheduler.cpp
+90
View File
@@ -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;
}
libopeninv/src/terminal.cpp
+310
View File
@@ -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);
}
libopeninv/src/terminalcommands.cpp
+426
View File
@@ -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));
}