Floating-point converted to fixed-point
The follwing were converted to fixed-point - battery voltage - board temperature - filters for steer and speed - mixer calculation Starting from this moment, the firmware is floating point free, meaning it runs more efficiently.
This commit is contained in:
EmanuelFeru
parent
f6fc825e5f
commit
8771742558
|
|
@ -0,0 +1,195 @@
|
|||
/*
|
||||
* File: mixer.c
|
||||
*
|
||||
* Code generated for Simulink model 'mixer'.
|
||||
*
|
||||
* Model version : 1.1173
|
||||
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
|
||||
* C/C++ source code generated on : Wed Oct 16 19:40:02 2019
|
||||
*
|
||||
* Target selection: ert.tlc
|
||||
* Embedded hardware selection: ARM Compatible->ARM Cortex
|
||||
* Emulation hardware selection:
|
||||
* Differs from embedded hardware (MATLAB Host)
|
||||
* Code generation objectives:
|
||||
* 1. Execution efficiency
|
||||
* 2. RAM efficiency
|
||||
* Validation result: Not run
|
||||
*/
|
||||
|
||||
#include "mixer.h"
|
||||
#ifndef UCHAR_MAX
|
||||
#include <limits.h>
|
||||
#endif
|
||||
|
||||
#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
|
||||
#error Code was generated for compiler with different sized uchar/char. \
|
||||
Consider adjusting Test hardware word size settings on the \
|
||||
Hardware Implementation pane to match your compiler word sizes as \
|
||||
defined in limits.h of the compiler. Alternatively, you can \
|
||||
select the Test hardware is the same as production hardware option and \
|
||||
select the Enable portable word sizes option on the Code Generation > \
|
||||
Verification pane for ERT based targets, which will disable the \
|
||||
preprocessor word size checks.
|
||||
#endif
|
||||
|
||||
#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
|
||||
#error Code was generated for compiler with different sized ushort/short. \
|
||||
Consider adjusting Test hardware word size settings on the \
|
||||
Hardware Implementation pane to match your compiler word sizes as \
|
||||
defined in limits.h of the compiler. Alternatively, you can \
|
||||
select the Test hardware is the same as production hardware option and \
|
||||
select the Enable portable word sizes option on the Code Generation > \
|
||||
Verification pane for ERT based targets, which will disable the \
|
||||
preprocessor word size checks.
|
||||
#endif
|
||||
|
||||
#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
|
||||
#error Code was generated for compiler with different sized uint/int. \
|
||||
Consider adjusting Test hardware word size settings on the \
|
||||
Hardware Implementation pane to match your compiler word sizes as \
|
||||
defined in limits.h of the compiler. Alternatively, you can \
|
||||
select the Test hardware is the same as production hardware option and \
|
||||
select the Enable portable word sizes option on the Code Generation > \
|
||||
Verification pane for ERT based targets, which will disable the \
|
||||
preprocessor word size checks.
|
||||
#endif
|
||||
|
||||
#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
|
||||
#error Code was generated for compiler with different sized ulong/long. \
|
||||
Consider adjusting Test hardware word size settings on the \
|
||||
Hardware Implementation pane to match your compiler word sizes as \
|
||||
defined in limits.h of the compiler. Alternatively, you can \
|
||||
select the Test hardware is the same as production hardware option and \
|
||||
select the Enable portable word sizes option on the Code Generation > \
|
||||
Verification pane for ERT based targets, which will disable the \
|
||||
preprocessor word size checks.
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
|
||||
/* Skip this size verification because of preprocessor limitation */
|
||||
#if ( ULLONG_MAX != (0xFFFFFFFFFFFFFFFFULL) ) || ( LLONG_MAX != (0x7FFFFFFFFFFFFFFFLL) )
|
||||
#error Code was generated for compiler with different sized ulong_long/long_long. \
|
||||
Consider adjusting Test hardware word size settings on the \
|
||||
Hardware Implementation pane to match your compiler word sizes as \
|
||||
defined in limits.h of the compiler. Alternatively, you can \
|
||||
select the Test hardware is the same as production hardware option and \
|
||||
select the Enable portable word sizes option on the Code Generation > \
|
||||
Verification pane for ERT based targets, which will disable the \
|
||||
preprocessor word size checks.
|
||||
#endif
|
||||
#endif
|
||||
|
||||
extern void mixer_k(int16_T rtu_speed, int16_T rtu_steer, int16_T rtu_speed_coef,
|
||||
int16_T rtu_steer_coef, int16_T *rty_speedR, int16_T
|
||||
*rty_speedL);
|
||||
|
||||
/*===========*
|
||||
* Constants *
|
||||
*===========*/
|
||||
#define RT_PI 3.14159265358979323846
|
||||
#define RT_PIF 3.1415927F
|
||||
#define RT_LN_10 2.30258509299404568402
|
||||
#define RT_LN_10F 2.3025851F
|
||||
#define RT_LOG10E 0.43429448190325182765
|
||||
#define RT_LOG10EF 0.43429449F
|
||||
#define RT_E 2.7182818284590452354
|
||||
#define RT_EF 2.7182817F
|
||||
|
||||
/*
|
||||
* UNUSED_PARAMETER(x)
|
||||
* Used to specify that a function parameter (argument) is required but not
|
||||
* accessed by the function body.
|
||||
*/
|
||||
#ifndef UNUSED_PARAMETER
|
||||
# if defined(__LCC__)
|
||||
# define UNUSED_PARAMETER(x) /* do nothing */
|
||||
# else
|
||||
|
||||
/*
|
||||
* This is the semi-ANSI standard way of indicating that an
|
||||
* unused function parameter is required.
|
||||
*/
|
||||
# define UNUSED_PARAMETER(x) (void) (x)
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* Output and update for atomic system: '<Root>/mixer' */
|
||||
void mixer_k(int16_T rtu_speed, int16_T rtu_steer, int16_T rtu_speed_coef,
|
||||
int16_T rtu_steer_coef, int16_T *rty_speedR, int16_T *rty_speedL)
|
||||
{
|
||||
int16_T rtb_Divide1;
|
||||
int16_T rtb_Divide2;
|
||||
int32_T tmp;
|
||||
|
||||
/* Product: '<S1>/Divide1' */
|
||||
rtb_Divide1 = (int16_T)((rtu_speed * rtu_speed_coef) >> 14);
|
||||
|
||||
/* Product: '<S1>/Divide2' */
|
||||
rtb_Divide2 = (int16_T)((rtu_steer * rtu_steer_coef) >> 14);
|
||||
|
||||
/* Sum: '<S1>/Sum1' */
|
||||
tmp = rtb_Divide1 - rtb_Divide2;
|
||||
if (tmp > 32767) {
|
||||
tmp = 32767;
|
||||
} else {
|
||||
if (tmp < -32768) {
|
||||
tmp = -32768;
|
||||
}
|
||||
}
|
||||
|
||||
/* DataTypeConversion: '<S1>/Data Type Conversion2' incorporates:
|
||||
* Sum: '<S1>/Sum1'
|
||||
*/
|
||||
*rty_speedR = (int16_T)(tmp >> 4);
|
||||
|
||||
/* Sum: '<S1>/Sum2' */
|
||||
tmp = rtb_Divide1 + rtb_Divide2;
|
||||
if (tmp > 32767) {
|
||||
tmp = 32767;
|
||||
} else {
|
||||
if (tmp < -32768) {
|
||||
tmp = -32768;
|
||||
}
|
||||
}
|
||||
|
||||
/* DataTypeConversion: '<S1>/Data Type Conversion3' incorporates:
|
||||
* Sum: '<S1>/Sum2'
|
||||
*/
|
||||
*rty_speedL = (int16_T)(tmp >> 4);
|
||||
}
|
||||
|
||||
/* Model step function */
|
||||
void mixer_step(RT_MODEL *const rtM)
|
||||
{
|
||||
ExtU *rtU = (ExtU *) rtM->inputs;
|
||||
ExtY *rtY = (ExtY *) rtM->outputs;
|
||||
|
||||
/* Outputs for Atomic SubSystem: '<Root>/mixer' */
|
||||
|
||||
/* Inport: '<Root>/speed' incorporates:
|
||||
* Inport: '<Root>/speed_coef'
|
||||
* Inport: '<Root>/steer'
|
||||
* Inport: '<Root>/steer_coef'
|
||||
* Outport: '<Root>/speedL'
|
||||
* Outport: '<Root>/speedR'
|
||||
*/
|
||||
mixer_k(rtU->speed, rtU->steer, rtU->speed_coef, rtU->steer_coef, &rtY->speedR,
|
||||
&rtY->speedL);
|
||||
|
||||
/* End of Outputs for SubSystem: '<Root>/mixer' */
|
||||
}
|
||||
|
||||
/* Model initialize function */
|
||||
void mixer_initialize(RT_MODEL *const rtM)
|
||||
{
|
||||
/* (no initialization code required) */
|
||||
UNUSED_PARAMETER(rtM);
|
||||
}
|
||||
|
||||
/*
|
||||
* File trailer for generated code.
|
||||
*
|
||||
* [EOF]
|
||||
*/
|
||||
|
|
@ -0,0 +1,91 @@
|
|||
/*
|
||||
* File: mixer.h
|
||||
*
|
||||
* Code generated for Simulink model 'mixer'.
|
||||
*
|
||||
* Model version : 1.1173
|
||||
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
|
||||
* C/C++ source code generated on : Wed Oct 16 19:40:02 2019
|
||||
*
|
||||
* Target selection: ert.tlc
|
||||
* Embedded hardware selection: ARM Compatible->ARM Cortex
|
||||
* Emulation hardware selection:
|
||||
* Differs from embedded hardware (MATLAB Host)
|
||||
* Code generation objectives:
|
||||
* 1. Execution efficiency
|
||||
* 2. RAM efficiency
|
||||
* Validation result: Not run
|
||||
*/
|
||||
|
||||
#ifndef RTW_HEADER_mixer_h_
|
||||
#define RTW_HEADER_mixer_h_
|
||||
#ifndef mixer_COMMON_INCLUDES_
|
||||
# define mixer_COMMON_INCLUDES_
|
||||
#include "rtwtypes.h"
|
||||
#endif /* mixer_COMMON_INCLUDES_ */
|
||||
|
||||
/* Macros for accessing real-time model data structure */
|
||||
|
||||
/* Forward declaration for rtModel */
|
||||
typedef struct tag_RTM RT_MODEL;
|
||||
|
||||
/* External inputs (root inport signals with auto storage) */
|
||||
typedef struct {
|
||||
int16_T speed; /* '<Root>/speed' */
|
||||
int16_T steer; /* '<Root>/steer' */
|
||||
int16_T speed_coef; /* '<Root>/speed_coef' */
|
||||
int16_T steer_coef; /* '<Root>/steer_coef' */
|
||||
} ExtU;
|
||||
|
||||
/* External outputs (root outports fed by signals with auto storage) */
|
||||
typedef struct {
|
||||
int16_T speedR; /* '<Root>/speedR' */
|
||||
int16_T speedL; /* '<Root>/speedL' */
|
||||
} ExtY;
|
||||
|
||||
/* Real-time Model Data Structure */
|
||||
struct tag_RTM {
|
||||
ExtU *inputs;
|
||||
ExtY *outputs;
|
||||
};
|
||||
|
||||
/* Model entry point functions */
|
||||
extern void mixer_initialize(RT_MODEL *const rtM);
|
||||
extern void mixer_step(RT_MODEL *const rtM);
|
||||
|
||||
/*-
|
||||
* These blocks were eliminated from the model due to optimizations:
|
||||
*
|
||||
* Block '<S1>/Data Type Conversion4' : Unused code path elimination
|
||||
* Block '<S1>/Display' : Unused code path elimination
|
||||
* Block '<S1>/Display1' : Unused code path elimination
|
||||
* Block '<S1>/Display3' : Unused code path elimination
|
||||
*/
|
||||
|
||||
/*-
|
||||
* The generated code includes comments that allow you to trace directly
|
||||
* back to the appropriate location in the model. The basic format
|
||||
* is <system>/block_name, where system is the system number (uniquely
|
||||
* assigned by Simulink) and block_name is the name of the block.
|
||||
*
|
||||
* Note that this particular code originates from a subsystem build,
|
||||
* and has its own system numbers different from the parent model.
|
||||
* Refer to the system hierarchy for this subsystem below, and use the
|
||||
* MATLAB hilite_system command to trace the generated code back
|
||||
* to the parent model. For example,
|
||||
*
|
||||
* hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/mixer') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/mixer
|
||||
* hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/mixer/Kp') - opens and selects block Kp
|
||||
*
|
||||
* Here is the system hierarchy for this model
|
||||
*
|
||||
* '<Root>' : 'BLDCmotorControl_FOC_R2017b_fixdt'
|
||||
* '<S1>' : 'BLDCmotorControl_FOC_R2017b_fixdt/mixer'
|
||||
*/
|
||||
#endif /* RTW_HEADER_mixer_h_ */
|
||||
|
||||
/*
|
||||
* File trailer for generated code.
|
||||
*
|
||||
* [EOF]
|
||||
*/
|
||||
122
Inc/config.h
122
Inc/config.h
|
|
@ -32,30 +32,45 @@
|
|||
|
||||
// ############################### GENERAL ###############################
|
||||
|
||||
// How to calibrate: connect GND and RX of a 3.3v uart-usb adapter to the right sensor board cable (be careful not to use the red wire of the cable. 15v will destroye verything.). if you are using nunchuck, disable it temporarily. enable DEBUG_SERIAL_USART3 and DEBUG_SERIAL_ASCII use asearial terminal.
|
||||
/* How to calibrate: connect GND and RX of a 3.3v uart-usb adapter to the right sensor board cable
|
||||
* Be careful not to use the red wire of the cable. 15v will destroye verything.).
|
||||
* If you are using nunchuck, disable it temporarily. enable DEBUG_SERIAL_USART3 and DEBUG_SERIAL_ASCII use asearial terminal.
|
||||
*/
|
||||
|
||||
// Battery voltage calibration: connect power source. see <How to calibrate>. write value nr 5 to BAT_CALIB_ADC. make and flash firmware. then you can verify voltage on value 6 (devide it by 100.0 to get calibrated voltage).
|
||||
#define BAT_CALIB_REAL_VOLTAGE 43.0f // input voltage measured by multimeter
|
||||
/* Battery voltage calibration: connect power source. see <How to calibrate>.
|
||||
* Write value nr 5 to BAT_CALIB_ADC. make and flash firmware.
|
||||
* Then you can verify voltage on value 6 (to get calibrated voltage multiplied by 100).
|
||||
*/
|
||||
#define BAT_FILT_COEF 655 // battery voltage filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
|
||||
#define BAT_CALIB_REAL_VOLTAGE 4300 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300
|
||||
#define BAT_CALIB_ADC 1704 // adc-value measured by mainboard (value nr 5 on UART debug output)
|
||||
|
||||
#define BAT_NUMBER_OF_CELLS 10 // normal Hoverboard battery: 10s
|
||||
#define BAT_CELLS 10 // battery number of cells. Normal Hoverboard battery: 10s
|
||||
#define BAT_LOW_LVL1_ENABLE 0 // to beep or not to beep, 1 or 0
|
||||
#define BAT_LOW_LVL1 3.6f // gently beeps at this voltage level. [V/cell]
|
||||
#define BAT_LOW_LVL2_ENABLE 1 // to beep or not to beep, 1 or 0
|
||||
#define BAT_LOW_LVL2 3.5f // your battery is almost empty. Charge now! [V/cell]
|
||||
#define BAT_LOW_DEAD 3.37f // undervoltage poweroff. (while not driving) [V/cell]
|
||||
#define BAT_LOW_LVL1 (360 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // gently beeps at this voltage level. [V*100/cell]. In this case 3.60 V/cell
|
||||
#define BAT_LOW_LVL2 (350 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // your battery is almost empty. Charge now! [V*100/cell]. In this case 3.50 V/cell
|
||||
#define BAT_LOW_DEAD (337 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // undervoltage poweroff. (while not driving) [V*100/cell]. In this case 3.37 V/cell
|
||||
|
||||
// Board overheat detection: the sensor is inside the STM/GD chip. it is very inaccurate without calibration (up to 45°C). so only enable this funcion after calibration! let your board cool down. see <How to calibrate>. get the real temp of the chip by thermo cam or another temp-sensor taped on top of the chip and write it to TEMP_CAL_LOW_DEG_C. write debug value 8 to TEMP_CAL_LOW_ADC. drive around to warm up the board. it should be at least 20°C warmer. repeat it for the HIGH-values. enable warning and/or poweroff and make and flash firmware.
|
||||
|
||||
/* Board overheat detection: the sensor is inside the STM/GD chip.
|
||||
* It is very inaccurate without calibration (up to 45°C). So only enable this funcion after calibration!
|
||||
* Let your board cool down. see <How to calibrate>.
|
||||
* Get the real temp of the chip by thermo cam or another temp-sensor taped on top of the chip and write it to TEMP_CAL_LOW_DEG_C.
|
||||
* Write debug value 8 to TEMP_CAL_LOW_ADC. drive around to warm up the board. it should be at least 20°C warmer. repeat it for the HIGH-values.
|
||||
* Enable warning and/or poweroff and make and flash firmware.
|
||||
*/
|
||||
#define TEMP_FILT_COEF 655 // temperature filter coefficient in fixed-point. coef_fixedPoint = coef_floatingPoint * 2^16. In this case 655 = 0.01 * 2^16
|
||||
#define TEMP_CAL_LOW_ADC 1655 // temperature 1: ADC value
|
||||
#define TEMP_CAL_LOW_DEG_C 35.8f // temperature 1: measured temperature [°C]
|
||||
#define TEMP_CAL_LOW_DEG_C 358 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C
|
||||
#define TEMP_CAL_HIGH_ADC 1588 // temperature 2: ADC value
|
||||
#define TEMP_CAL_HIGH_DEG_C 48.9f // temperature 2: measured temperature [°C]
|
||||
#define TEMP_CAL_HIGH_DEG_C 489 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C
|
||||
#define TEMP_WARNING_ENABLE 0 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
|
||||
#define TEMP_WARNING 60 // annoying fast beeps [°C]
|
||||
#define TEMP_WARNING 600 // annoying fast beeps [°C * 10]. Here 60.0 °C
|
||||
#define TEMP_POWEROFF_ENABLE 0 // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION!
|
||||
#define TEMP_POWEROFF 65 // overheat poweroff. (while not driving) [°C]
|
||||
#define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C
|
||||
|
||||
#define INACTIVITY_TIMEOUT 8 // minutes of not driving until poweroff. it is not very precise.
|
||||
#define INACTIVITY_TIMEOUT 8 // minutes of not driving until poweroff. it is not very precise.
|
||||
|
||||
// ############################### LCD DEBUG ###############################
|
||||
|
||||
|
|
@ -72,7 +87,7 @@
|
|||
|
||||
// ###### CONTROL VIA UART (serial) ######
|
||||
//#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
|
||||
#define CONTROL_BAUD 19200 // control via usart from eg an Arduino or raspberry
|
||||
#define CONTROL_BAUD 19200 // control via usart from eg an Arduino or raspberry
|
||||
// for Arduino, use void loop(void){ Serial.write((uint8_t *) &steer, sizeof(steer)); Serial.write((uint8_t *) &speed, sizeof(speed));delay(20); }
|
||||
|
||||
// ###### CONTROL VIA RC REMOTE ######
|
||||
|
|
@ -81,7 +96,11 @@
|
|||
//#define PPM_NUM_CHANNELS 6 // total number of PPM channels to receive, even if they are not used.
|
||||
|
||||
// ###### CONTROL VIA TWO POTENTIOMETERS ######
|
||||
// ADC-calibration to cover the full poti-range: connect potis to left sensor board cable (0 to 3.3V) (do NOT use the red 15V wire in the cable!). see <How to calibrate>. turn the potis to minimum position, write value 1 to ADC1_MIN and value 2 to ADC2_MIN. turn to maximum position and repeat it for ADC?_MAX. make, flash and test it.
|
||||
/* ADC-calibration to cover the full poti-range:
|
||||
* Connect potis to left sensor board cable (0 to 3.3V) (do NOT use the red 15V wire in the cable!). see <How to calibrate>.
|
||||
* Turn the potis to minimum position, write value 1 to ADC1_MIN and value 2 to ADC2_MIN.
|
||||
* Turn to maximum position and repeat it for ADC?_MAX. make, flash and test it.
|
||||
*/
|
||||
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2!
|
||||
#define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095)
|
||||
#define ADC1_MAX 4095 // max ADC1-value while poti at maximum-position (0 - 4095)
|
||||
|
|
@ -89,8 +108,12 @@
|
|||
#define ADC2_MAX 4095 // max ADC2-value while poti at maximum-position (0 - 4095)
|
||||
|
||||
// ###### CONTROL VIA NINTENDO NUNCHUCK ######
|
||||
// left sensor board cable. keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuck, use the right one of the 2 types of nunchucks, add i2c pullups. use original nunchuck. most clones does not work very well.
|
||||
//#define CONTROL_NUNCHUCK // use nunchuck as input. disable DEBUG_SERIAL_USART3!
|
||||
/* left sensor board cable.
|
||||
* keep cable short, use shielded cable, use ferrits, stabalize voltage in nunchuck,
|
||||
* use the right one of the 2 types of nunchucks, add i2c pullups.
|
||||
* use original nunchuck. most clones does not work very well.
|
||||
*/
|
||||
// #define CONTROL_NUNCHUCK // use nunchuck as input. disable DEBUG_SERIAL_USART3!
|
||||
|
||||
|
||||
// ############################### MOTOR CONTROL (overwrite) #########################
|
||||
|
|
@ -103,46 +126,55 @@
|
|||
#define N_MOT_MAX 800 << 4 // [rpm] Maximum motor speed (change only the first number, the rest is needed for fixed-point conversion)
|
||||
|
||||
|
||||
// GENERAL NOTES:
|
||||
// 1. The above parameters are over-writing the default motor parameters. For all the available parameters check BLDC_controller_data.c
|
||||
// 2. The parameters are represented in fixed point data type for a more efficient code execution
|
||||
// 3. For calibrating the fixed-point parameters use the Fixed-Point Viewer tool (see <https://github.com/EmanuelFeru/FixedPointViewer>)
|
||||
// 4. For more details regarding the parameters and the working principle of the controller please consult the Simulink model
|
||||
// 5. A webview was created, so Matlab/Simulink installation is not needed, unless you want to regenerate the code
|
||||
|
||||
// NOTES Field weakening:
|
||||
// 1. In BLDC_controller_data.c you can find the field weakening Map as a function of speed: MAP = id_fieldWeak_M1, XAXIS = n_fieldWeak_XA
|
||||
// 2. The default calibration was experimentally calibrated to my particular needs
|
||||
// 3. If you re-calibrate the field weakening map please take all the safety measures! The motors can spin very fast!
|
||||
// 4. During the recalibration make sure the speed values in XAXIS are equally spaced for a correct Map interpolation.
|
||||
/* GENERAL NOTES:
|
||||
* 1. The above parameters are over-writing the default motor parameters. For all the available parameters check BLDC_controller_data.c
|
||||
* 2. The parameters are represented in fixed point data type for a more efficient code execution
|
||||
* 3. For calibrating the fixed-point parameters use the Fixed-Point Viewer tool (see <https://github.com/EmanuelFeru/FixedPointViewer>)
|
||||
* 4. For more details regarding the parameters and the working principle of the controller please consult the Simulink model
|
||||
* 5. A webview was created, so Matlab/Simulink installation is not needed, unless you want to regenerate the code
|
||||
*
|
||||
* NOTES Field weakening:
|
||||
* 1. In BLDC_controller_data.c you can find the field weakening Map as a function of speed: MAP = id_fieldWeak_M1, XAXIS = n_fieldWeak_XA
|
||||
* 2. The default calibration was experimentally calibrated to my particular needs
|
||||
* 3. If you re-calibrate the field weakening map please take all the safety measures! The motors can spin very fast!
|
||||
* 4. During the recalibration make sure the speed values in XAXIS are equally spaced for a correct Map interpolation.
|
||||
*/
|
||||
|
||||
|
||||
// ############################### DRIVING BEHAVIOR ###############################
|
||||
|
||||
// inputs:
|
||||
// - cmd1 and cmd2: analog normalized input values. -1000 to 1000
|
||||
// - button1 and button2: digital input values. 0 or 1
|
||||
// - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
|
||||
// outputs:
|
||||
// - speedR and speedL: normal driving -1000 to 1000
|
||||
/* Inputs:
|
||||
* - cmd1 and cmd2: analog normalized input values. -1000 to 1000
|
||||
* - button1 and button2: digital input values. 0 or 1
|
||||
* - adc_buffer.l_tx2 and adc_buffer.l_rx2: unfiltered ADC values (you do not need them). 0 to 4095
|
||||
* Outputs:
|
||||
* - speedR and speedL: normal driving -1000 to 1000
|
||||
*/
|
||||
|
||||
// Value of FILTER is in fixdt(0,16,16)
|
||||
// VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6554 = 0.1 * 2^16
|
||||
#define FILTER 6553 // 0.1f [-] lower value == softer filter [0, 65535] = [ 0.0 - 1.0].
|
||||
|
||||
// Value of COEFFICIENT is in fixdt(1,16,14)
|
||||
// If VAL_floatingPoint >= 0, VAL_fixedPoint = VAL_floatingPoint * 2^15
|
||||
// If VAL_floatingPoint < 0, VAL_fixedPoint = 2^16 + floor(VAL_floatingPoint * 2^15).
|
||||
#define SPEED_COEFFICIENT 16384 // 1.0f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14
|
||||
#define STEER_COEFFICIENT 8192 // 0.5f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 8192 = 0.5 * 2^15. If you do not want any steering, set it to 0.
|
||||
|
||||
#define FILTER 0.1f // lower value == softer filter. do not use values <0.01, you will get float precision issues.
|
||||
#define SPEED_COEFFICIENT 1.0f // higher value == stronger. 0.0 to ~2.0?
|
||||
#define STEER_COEFFICIENT 0.5f // higher value == stronger. if you do not want any steering, set it to 0.0; 0.0 to 1.0
|
||||
#define INVERT_R_DIRECTION
|
||||
#define INVERT_L_DIRECTION
|
||||
#define BEEPS_BACKWARD 0 // 0 or 1
|
||||
#define BEEPS_BACKWARD 0 // 0 or 1
|
||||
|
||||
// ###### SIMPLE BOBBYCAR ######
|
||||
// for better bobbycar code see: https://github.com/larsmm/hoverboard-firmware-hack-bbcar
|
||||
// #define FILTER 0.1f
|
||||
// #define SPEED_COEFFICIENT -1f
|
||||
// #define STEER_COEFFICIENT 0f
|
||||
// #define FILTER 6553 // 0.1f
|
||||
// #define SPEED_COEFFICIENT 49152 // -1.0f
|
||||
// #define STEER_COEFFICIENT 0 // 0.0f
|
||||
|
||||
// ###### ARMCHAIR ######
|
||||
// #define FILTER 0.05f
|
||||
// #define SPEED_COEFFICIENT 0.5f
|
||||
// #define STEER_COEFFICIENT -0.2f
|
||||
// #define FILTER 3276 // 0.05f
|
||||
// #define SPEED_COEFFICIENT 8192 // 0.5f
|
||||
// #define STEER_COEFFICIENT 62259 // -0.2f
|
||||
|
||||
// ############################### VALIDATE SETTINGS ###############################
|
||||
|
||||
|
|
|
|||
|
|
@ -158,5 +158,6 @@ typedef struct {
|
|||
} adc_buf_t;
|
||||
|
||||
// Define low-pass filter functions. Implementation is in main.c
|
||||
int16_t filtLowPass16(int16_t u, uint16_t coef, int16_t yPrev);
|
||||
int32_t filtLowPass32(int32_t u, uint16_t coef, int32_t yPrev);
|
||||
void filtLowPass16(int16_t u, uint16_t coef, int16_t *y);
|
||||
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
|
||||
void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL);
|
||||
16
Src/bldc.c
16
Src/bldc.c
|
|
@ -76,7 +76,9 @@ static int16_t offsetrr2 = 2000;
|
|||
static int16_t offsetdcl = 2000;
|
||||
static int16_t offsetdcr = 2000;
|
||||
|
||||
float batteryVoltage = BAT_NUMBER_OF_CELLS * 4.0;
|
||||
int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
|
||||
static int16_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 4; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point
|
||||
|
||||
|
||||
//scan 8 channels with 2ADCs @ 20 clk cycles per sample
|
||||
//meaning ~80 ADC clock cycles @ 8MHz until new DMA interrupt =~ 100KHz
|
||||
|
|
@ -98,27 +100,27 @@ void DMA1_Channel1_IRQHandler(void) {
|
|||
return;
|
||||
}
|
||||
|
||||
if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time
|
||||
batteryVoltage = batteryVoltage * 0.99f + ((float)adc_buffer.batt1 * ((float)BAT_CALIB_REAL_VOLTAGE / (float)BAT_CALIB_ADC)) * 0.01f;
|
||||
if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point
|
||||
filtLowPass16(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
|
||||
batVoltage = batVoltageFixdt >> 4; // convert fixed-point to integer
|
||||
}
|
||||
|
||||
// Get Left motor currents
|
||||
curL_phaA = (int16_t)(offsetrl1 - adc_buffer.rl1);
|
||||
curL_phaB = (int16_t)(offsetrl2 - adc_buffer.rl2);
|
||||
curL_DC = (int16_t)(adc_buffer.dcl - offsetdcl);
|
||||
curL_DC = (int16_t)(offsetdcl - adc_buffer.dcl);
|
||||
|
||||
// Get Right motor currents
|
||||
curR_phaB = (int16_t)(offsetrr1 - adc_buffer.rr1);
|
||||
curR_phaC = (int16_t)(offsetrr2 - adc_buffer.rr2);
|
||||
curR_DC = (int16_t)(adc_buffer.dcr - offsetdcr);
|
||||
curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr);
|
||||
|
||||
// Disable PWM when current limit is reached (current chopping)
|
||||
// This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
|
||||
if(ABS(curL_DC) > I_DC_MAX || timeout > TIMEOUT || enable == 0) {
|
||||
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
|
||||
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
} else {
|
||||
LEFT_TIM->BDTR |= TIM_BDTR_MOE;
|
||||
//HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
|
||||
}
|
||||
|
||||
if(ABS(curR_DC) > I_DC_MAX || timeout > TIMEOUT || enable == 0) {
|
||||
|
|
|
|||
158
Src/main.c
158
Src/main.c
|
|
@ -29,11 +29,11 @@
|
|||
|
||||
// Matlab includes and defines - from auto-code generation
|
||||
// ###############################################################################
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
#include "BLDC_controller.h" /* Model's header file */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
RT_MODEL rtM_Left_; /* Real-time model */
|
||||
RT_MODEL rtM_Right_; /* Real-time model */
|
||||
RT_MODEL rtM_Left_; /* Real-time model */
|
||||
RT_MODEL rtM_Right_; /* Real-time model */
|
||||
RT_MODEL *const rtM_Left = &rtM_Left_;
|
||||
RT_MODEL *const rtM_Right = &rtM_Right_;
|
||||
|
||||
|
|
@ -76,8 +76,10 @@ static volatile Serialcommand command;
|
|||
|
||||
static uint8_t button1, button2;
|
||||
|
||||
static int steer; // local variable for steering. -1000 to 1000
|
||||
static int speed; // local variable for speed. -1000 to 1000
|
||||
static int16_t steerFixdt; // local fixed-point variable for steering.
|
||||
static int16_t speedFixdt; // local fixed-point variable for speed.
|
||||
static int16_t steer; // local variable for steering. -1000 to 1000
|
||||
static int16_t speed; // local variable for speed. -1000 to 1000
|
||||
|
||||
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
|
||||
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
|
||||
|
|
@ -88,7 +90,7 @@ extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can
|
|||
extern uint8_t enable; // global variable for motor enable
|
||||
|
||||
extern volatile uint32_t timeout; // global variable for timeout
|
||||
extern float batteryVoltage; // global variable for battery voltage
|
||||
extern int16_t batVoltage; // global variable for battery voltage
|
||||
|
||||
static uint32_t inactivity_timeout_counter;
|
||||
|
||||
|
|
@ -196,8 +198,6 @@ int main(void) {
|
|||
|
||||
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
|
||||
|
||||
int lastSpeedL = 0, lastSpeedR = 0;
|
||||
int speedL = 0, speedR = 0;
|
||||
|
||||
#ifdef CONTROL_PPM
|
||||
PPM_Init();
|
||||
|
|
@ -235,8 +235,13 @@ int main(void) {
|
|||
LCD_WriteString(&lcd, "Initializing...");
|
||||
#endif
|
||||
|
||||
float board_temp_adc_filtered = (float)adc_buffer.temp;
|
||||
float board_temp_deg_c;
|
||||
|
||||
int16_t lastSpeedL = 0, lastSpeedR = 0;
|
||||
int16_t speedL = 0, speedR = 0;
|
||||
|
||||
int16_t board_temp_adcFixdt = adc_buffer.temp << 4; // Fixed-point filter output initialized with current ADC converted to fixed-point
|
||||
int16_t board_temp_adcFilt = adc_buffer.temp;
|
||||
int16_t board_temp_deg_c;
|
||||
|
||||
enable = 0; // initially motors are disabled for SAFETY
|
||||
|
||||
|
|
@ -262,8 +267,8 @@ int main(void) {
|
|||
|
||||
#ifdef CONTROL_ADC
|
||||
// ADC values range: 0-4095, see ADC-calibration in config.h
|
||||
cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) / (ADC1_MAX / 1000.0f); // ADC1
|
||||
cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) / (ADC2_MAX / 1000.0f); // ADC2
|
||||
cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) * 1000 / ADC1_MAX; // ADC1
|
||||
cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) * 1000 / ADC2_MAX; // ADC2
|
||||
|
||||
// use ADCs as button inputs:
|
||||
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
|
||||
|
|
@ -289,31 +294,33 @@ int main(void) {
|
|||
}
|
||||
|
||||
// ####### LOW-PASS FILTER #######
|
||||
steer = (int)(steer * (1.0f - FILTER) + cmd1 * FILTER);
|
||||
speed = (int)(speed * (1.0f - FILTER) + cmd2 * FILTER);
|
||||
filtLowPass16(cmd1, FILTER, &steerFixdt);
|
||||
filtLowPass16(cmd2, FILTER, &speedFixdt);
|
||||
steer = steerFixdt >> 4; // convert fixed-point to integer
|
||||
speed = speedFixdt >> 4; // convert fixed-point to integer
|
||||
|
||||
// ####### MIXER #######
|
||||
speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
|
||||
speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
|
||||
|
||||
// speedR = CLAMP((int)(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT), -1000, 1000);
|
||||
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
|
||||
mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
|
||||
|
||||
#ifdef ADDITIONAL_CODE
|
||||
ADDITIONAL_CODE;
|
||||
#endif
|
||||
|
||||
|
||||
// ####### SET OUTPUTS (if the target change less than +/- 50) #######
|
||||
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
|
||||
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
|
||||
#ifdef INVERT_R_DIRECTION
|
||||
pwmr = speedR;
|
||||
#else
|
||||
pwmr = -speedR;
|
||||
#endif
|
||||
#ifdef INVERT_L_DIRECTION
|
||||
pwml = -speedL;
|
||||
#else
|
||||
pwml = speedL;
|
||||
#endif
|
||||
#ifdef INVERT_R_DIRECTION
|
||||
pwmr = speedR;
|
||||
#else
|
||||
pwmr = -speedR;
|
||||
#endif
|
||||
#ifdef INVERT_L_DIRECTION
|
||||
pwml = -speedL;
|
||||
#else
|
||||
pwml = speedL;
|
||||
#endif
|
||||
}
|
||||
|
||||
lastSpeedL = speedL;
|
||||
|
|
@ -322,8 +329,9 @@ int main(void) {
|
|||
|
||||
if (inactivity_timeout_counter % 25 == 0) {
|
||||
// ####### CALC BOARD TEMPERATURE #######
|
||||
board_temp_adc_filtered = board_temp_adc_filtered * 0.99f + (float)adc_buffer.temp * 0.01f;
|
||||
board_temp_deg_c = ((float)TEMP_CAL_HIGH_DEG_C - (float)TEMP_CAL_LOW_DEG_C) / ((float)TEMP_CAL_HIGH_ADC - (float)TEMP_CAL_LOW_ADC) * (board_temp_adc_filtered - (float)TEMP_CAL_LOW_ADC) + (float)TEMP_CAL_LOW_DEG_C;
|
||||
filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
|
||||
board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer
|
||||
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
|
||||
|
||||
// ####### DEBUG SERIAL OUT #######
|
||||
#ifdef CONTROL_ADC
|
||||
|
|
@ -335,13 +343,13 @@ int main(void) {
|
|||
setScopeChannel(2, (int16_t)rtY_Right.n_mot); // 3: Real motor speed [rpm]
|
||||
setScopeChannel(3, (int16_t)rtY_Left.n_mot); // 4: Real motor speed [rpm]
|
||||
setScopeChannel(4, (int16_t)adc_buffer.batt1); // 5: for battery voltage calibration
|
||||
setScopeChannel(5, (int16_t)(batteryVoltage * 100.0f)); // 6: for verifying battery voltage calibration
|
||||
setScopeChannel(6, (int16_t)board_temp_adc_filtered); // 7: for board temperature calibration
|
||||
setScopeChannel(5, (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC)); // 6: for verifying battery voltage calibration
|
||||
setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration
|
||||
setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration
|
||||
consoleScope();
|
||||
}
|
||||
|
||||
HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
|
||||
HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
|
||||
// ####### POWEROFF BY POWER-BUTTON #######
|
||||
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
|
||||
enable = 0;
|
||||
|
|
@ -351,15 +359,15 @@ HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
|
|||
|
||||
|
||||
// ####### BEEP AND EMERGENCY POWEROFF #######
|
||||
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batteryVoltage < ((float)BAT_LOW_DEAD * (float)BAT_NUMBER_OF_CELLS) && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3
|
||||
if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && abs(speed) < 20) || (batVoltage < BAT_LOW_DEAD && abs(speed) < 20)) { // poweroff before mainboard burns OR low bat 3
|
||||
poweroff();
|
||||
} else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot
|
||||
buzzerFreq = 4;
|
||||
buzzerPattern = 1;
|
||||
} else if (batteryVoltage < ((float)BAT_LOW_LVL1 * (float)BAT_NUMBER_OF_CELLS) && batteryVoltage > ((float)BAT_LOW_LVL2 * (float)BAT_NUMBER_OF_CELLS) && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep
|
||||
} else if (batVoltage < BAT_LOW_LVL1 && batVoltage >= BAT_LOW_LVL2 && BAT_LOW_LVL1_ENABLE) { // low bat 1: slow beep
|
||||
buzzerFreq = 5;
|
||||
buzzerPattern = 42;
|
||||
} else if (batteryVoltage < ((float)BAT_LOW_LVL2 * (float)BAT_NUMBER_OF_CELLS) && batteryVoltage > ((float)BAT_LOW_DEAD * (float)BAT_NUMBER_OF_CELLS) && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep
|
||||
} else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep
|
||||
buzzerFreq = 5;
|
||||
buzzerPattern = 6;
|
||||
} else if (errCode_Left || errCode_Right) { // beep in case of Motor error - fast beep
|
||||
|
|
@ -436,32 +444,27 @@ void SystemClock_Config(void) {
|
|||
* Max: 2047.9375
|
||||
* Min: -2048
|
||||
* Res: 0.0625
|
||||
* coef: [0,65535U] = fixdt(0,16,16)
|
||||
*
|
||||
* Call function example:
|
||||
* Inputs: u = int16
|
||||
* Outputs: y = fixdt(1,16,4)
|
||||
* Parameters: coef = fixdt(0,16,16) = [0,65535U]
|
||||
*
|
||||
* Example:
|
||||
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
|
||||
* y = filtLowPass16(u, 52429, y);
|
||||
* filtLowPass16(u, 52429, &y);
|
||||
* yint = y >> 4; // the integer output is the fixed-point ouput shifted by 4 bits
|
||||
*/
|
||||
int16_t filtLowPass16(int16_t u, uint16_t coef, int16_t yPrev)
|
||||
void filtLowPass16(int16_t u, uint16_t coef, int16_t *y)
|
||||
{
|
||||
int32_t tmp;
|
||||
int16_t y;
|
||||
|
||||
tmp = (((int16_t)(u << 4) * coef) >> 16) +
|
||||
(((int32_t)(65535U - coef) * yPrev) >> 16);
|
||||
(((int32_t)(65535U - coef) * (*y)) >> 16);
|
||||
|
||||
// Overflow protection
|
||||
if (tmp > 32767) {
|
||||
tmp = 32767;
|
||||
} else {
|
||||
if (tmp < -32768) {
|
||||
tmp = -32768;
|
||||
}
|
||||
}
|
||||
tmp = CLAMP(tmp, -32768, 32767);
|
||||
|
||||
y = (int16_t)tmp;
|
||||
|
||||
return y;
|
||||
*y = (int16_t)tmp;
|
||||
}
|
||||
|
||||
// ===========================================================
|
||||
|
|
@ -469,30 +472,61 @@ int16_t filtLowPass16(int16_t u, uint16_t coef, int16_t yPrev)
|
|||
* Max: 32767.99998474121
|
||||
* Min: -32768
|
||||
* Res: 1.52587890625e-5
|
||||
* coef: [0,65535U] = fixdt(0,16,16)
|
||||
*
|
||||
* Call function example:
|
||||
* Inputs: u = int32
|
||||
* Outputs: y = fixdt(1,32,16)
|
||||
* Parameters: coef = fixdt(0,16,16) = [0,65535U]
|
||||
*
|
||||
* Example:
|
||||
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
|
||||
* y = filtLowPass16(u, 52429, y);
|
||||
* filtLowPass16(u, 52429, &y);
|
||||
* yint = y >> 16; // the integer output is the fixed-point ouput shifted by 16 bits
|
||||
*/
|
||||
int32_t filtLowPass32(int32_t u, uint16_t coef, int32_t yPrev)
|
||||
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y)
|
||||
{
|
||||
int32_t q0;
|
||||
int32_t q1;
|
||||
int32_t y;
|
||||
int32_t tmp;
|
||||
|
||||
q0 = (int32_t)(((int64_t)(u << 16) * coef) >> 16);
|
||||
q1 = (int32_t)(((int64_t)(65535U - coef) * yPrev) >> 16);
|
||||
q1 = (int32_t)(((int64_t)(65535U - coef) * (*y)) >> 16);
|
||||
|
||||
// Overflow protection
|
||||
if ((q0 < 0) && (q1 < MIN_int32_T - q0)) {
|
||||
y = MIN_int32_T;
|
||||
tmp = MIN_int32_T;
|
||||
} else if ((q0 > 0) && (q1 > MAX_int32_T - q0)) {
|
||||
y = MAX_int32_T;
|
||||
tmp = MAX_int32_T;
|
||||
} else {
|
||||
y = q0 + q1;
|
||||
tmp = q0 + q1;
|
||||
}
|
||||
|
||||
return y;
|
||||
*y = tmp;
|
||||
}
|
||||
|
||||
// ===========================================================
|
||||
/* mixerFcn(rtu_speed, rtu_steer, &rty_speedR, &rty_speedL);
|
||||
* Inputs: rtu_speed, rtu_steer = fixdt(1,16,4)
|
||||
* Outputs: rty_speedR, rty_speedL = int16_t
|
||||
* Parameters: SPEED_COEFFICIENT, STEER_COEFFICIENT = fixdt(0,16,15)
|
||||
*/
|
||||
void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL)
|
||||
{
|
||||
int16_t prodSpeed;
|
||||
int16_t prodSteer;
|
||||
int32_t tmp;
|
||||
|
||||
prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14);
|
||||
prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14);
|
||||
|
||||
tmp = prodSpeed - prodSteer;
|
||||
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
|
||||
*rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int
|
||||
*rty_speedR = CLAMP(*rty_speedR, -1000, 1000);
|
||||
|
||||
tmp = prodSpeed + prodSteer;
|
||||
tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
|
||||
*rty_speedL = (int16_t)(tmp >> 4); // Convert from fixed-point to int
|
||||
*rty_speedL = CLAMP(*rty_speedL, -1000, 1000);
|
||||
}
|
||||
|
||||
// ===========================================================
|
||||
Loading…
Reference in New Issue