diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c deleted file mode 100644 index 1ddb411..0000000 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.c +++ /dev/null @@ -1,181 +0,0 @@ -/* - * File: filtLowPass.c - * - * Code generated for Simulink model 'filtLowPass'. - * - * Model version : 1.1167 - * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Oct 6 22:11:53 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 "filtLowPass.h" -#ifndef UCHAR_MAX -#include -#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 int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass - *localDW); - -/*===========* - * 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: '/filtLowPass' */ -int16_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) -{ - int32_T tmp; - int16_T rty_y_0; - - /* Outputs for Atomic SubSystem: '/Low_Pass_Filter1' */ - /* Sum: '/Sum1' incorporates: - * DataTypeConversion: '/Data Type Conversion' - * Product: '/Divide1' - * Product: '/Divide2' - * Sum: '/Sum5' - * UnitDelay: '/UnitDelay3' - */ - tmp = (((int16_T)(rtu_u << 4) * rtu_coef) >> 16) + (((int32_T)(65535U - - rtu_coef) * localDW->UnitDelay3_DSTATE) >> 16); - if (tmp > 32767) { - tmp = 32767; - } else { - if (tmp < -32768) { - tmp = -32768; - } - } - - rty_y_0 = (int16_T)tmp; - - /* Update for UnitDelay: '/UnitDelay3' incorporates: - * Sum: '/Sum1' - */ - localDW->UnitDelay3_DSTATE = (int16_T)tmp; - - /* End of Outputs for SubSystem: '/Low_Pass_Filter1' */ - return rty_y_0; -} - -/* Model step function */ -void filtLowPass_step(RT_MODEL *const rtM) -{ - DW *rtDW = ((DW *) rtM->dwork); - ExtU *rtU = (ExtU *) rtM->inputs; - ExtY *rtY = (ExtY *) rtM->outputs; - - /* Outputs for Atomic SubSystem: '/filtLowPass' */ - - /* Outport: '/y' incorporates: - * Inport: '/coef' - * Inport: '/u' - */ - rtY->y = (int16_T) filtLowPass_l(rtU->u, rtU->coef, &rtDW->filtLowPass_l2); - - /* End of Outputs for SubSystem: '/filtLowPass' */ -} - -/* Model initialize function */ -void filtLowPass_initialize(RT_MODEL *const rtM) -{ - /* (no initialization code required) */ - UNUSED_PARAMETER(rtM); -} - -/* - * File trailer for generated code. - * - * [EOF] - */ diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h deleted file mode 100644 index b137f33..0000000 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix16En4/filtLowPass.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * File: filtLowPass.h - * - * Code generated for Simulink model 'filtLowPass'. - * - * Model version : 1.1167 - * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Oct 6 22:11:53 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_filtLowPass_h_ -#define RTW_HEADER_filtLowPass_h_ -#ifndef filtLowPass_COMMON_INCLUDES_ -# define filtLowPass_COMMON_INCLUDES_ -#include "rtwtypes.h" -#endif /* filtLowPass_COMMON_INCLUDES_ */ - -/* Macros for accessing real-time model data structure */ - -/* Forward declaration for rtModel */ -typedef struct tag_RTM RT_MODEL; - -/* Block signals and states (auto storage) for system '/filtLowPass' */ -typedef struct { - int16_T UnitDelay3_DSTATE; /* '/UnitDelay3' */ -} DW_filtLowPass; - -/* Block signals and states (auto storage) for system '' */ -typedef struct { - DW_filtLowPass filtLowPass_l2; /* '/filtLowPass' */ -} DW; - -/* External inputs (root inport signals with auto storage) */ -typedef struct { - int16_T u; /* '/u' */ - uint16_T coef; /* '/coef' */ -} ExtU; - -/* External outputs (root outports fed by signals with auto storage) */ -typedef struct { - int16_T y; /* '/y' */ -} ExtY; - -/* Real-time Model Data Structure */ -struct tag_RTM { - ExtU *inputs; - ExtY *outputs; - DW *dwork; -}; - -/* Model entry point functions */ -extern void filtLowPass_initialize(RT_MODEL *const rtM); -extern void filtLowPass_step(RT_MODEL *const rtM); - -/*- - * The generated code includes comments that allow you to trace directly - * back to the appropriate location in the model. The basic format - * is /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/filtLowPass') - opens subsystem BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass - * hilite_system('BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Kp') - opens and selects block Kp - * - * Here is the system hierarchy for this model - * - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1' - */ -#endif /* RTW_HEADER_filtLowPass_h_ */ - -/* - * File trailer for generated code. - * - * [EOF] - */ diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c index 76cc256..8e2cae0 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.c @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1165 + * Model version : 1.1257 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Oct 6 22:00:52 2019 + * C/C++ source code generated on : Mon Feb 24 20:31:06 2020 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -81,7 +81,7 @@ preprocessor word size checks. #endif #endif -extern int32_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass +extern int32_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW); /*===========* @@ -115,37 +115,35 @@ extern int32_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass #endif /* Output and update for atomic system: '/filtLowPass' */ -int32_T filtLowPass_l(int16_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) +int32_T filtLowPass_l(int32_T rtu_u, uint16_T rtu_coef, DW_filtLowPass *localDW) { - int32_T q0; - int32_T q1; + int32_T rtb_UnitDelay1; + int64_T tmp; int32_T rty_y_0; - /* Outputs for Atomic SubSystem: '/Low_Pass_Filter1' */ - /* Sum: '/Sum1' incorporates: + /* UnitDelay: '/UnitDelay1' */ + rtb_UnitDelay1 = localDW->UnitDelay1_DSTATE; + + /* Product: '/Divide3' incorporates: * DataTypeConversion: '/Data Type Conversion' - * Product: '/Divide1' - * Product: '/Divide2' - * Sum: '/Sum5' - * UnitDelay: '/UnitDelay3' + * Sum: '/Sum2' */ - q0 = (int32_T)(((int64_T)(rtu_u << 16) * rtu_coef) >> 16); - q1 = (int32_T)(((int64_T)(65535U - rtu_coef) * localDW->UnitDelay3_DSTATE) >> - 16); - if ((q0 < 0) && (q1 < MIN_int32_T - q0)) { - rty_y_0 = MIN_int32_T; - } else if ((q0 > 0) && (q1 > MAX_int32_T - q0)) { - rty_y_0 = MAX_int32_T; + tmp = ((int64_T)((rtu_u << 4) - (rtb_UnitDelay1 >> 12)) * rtu_coef) >> 4; + if (tmp > 2147483647LL) { + tmp = 2147483647LL; } else { - rty_y_0 = q0 + q1; + if (tmp < -2147483648LL) { + tmp = -2147483648LL; + } } - /* Update for UnitDelay: '/UnitDelay3' incorporates: - * Sum: '/Sum1' + /* Sum: '/Sum3' incorporates: + * Product: '/Divide3' */ - localDW->UnitDelay3_DSTATE = rty_y_0; + rty_y_0 = (int32_T)tmp + rtb_UnitDelay1; - /* End of Outputs for SubSystem: '/Low_Pass_Filter1' */ + /* Update for UnitDelay: '/UnitDelay1' */ + localDW->UnitDelay1_DSTATE = rty_y_0; return rty_y_0; } diff --git a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h index f773172..0422d95 100644 --- a/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h +++ b/01_Matlab/99_RecycleBin/Filter_fixdt/filtLowPass_ert_rtw_sFix32En16/filtLowPass.h @@ -3,9 +3,9 @@ * * Code generated for Simulink model 'filtLowPass'. * - * Model version : 1.1165 + * Model version : 1.1257 * Simulink Coder version : 8.13 (R2017b) 24-Jul-2017 - * C/C++ source code generated on : Sun Oct 6 22:00:52 2019 + * C/C++ source code generated on : Mon Feb 24 20:31:06 2020 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex @@ -31,7 +31,7 @@ typedef struct tag_RTM RT_MODEL; /* Block signals and states (auto storage) for system '/filtLowPass' */ typedef struct { - int32_T UnitDelay3_DSTATE; /* '/UnitDelay3' */ + int32_T UnitDelay1_DSTATE; /* '/UnitDelay1' */ } DW_filtLowPass; /* Block signals and states (auto storage) for system '' */ @@ -41,7 +41,7 @@ typedef struct { /* External inputs (root inport signals with auto storage) */ typedef struct { - int16_T u; /* '/u' */ + int32_T u; /* '/u' */ uint16_T coef; /* '/coef' */ } ExtU; @@ -61,6 +61,12 @@ struct tag_RTM { extern void filtLowPass_initialize(RT_MODEL *const rtM); extern void filtLowPass_step(RT_MODEL *const rtM); +/*- + * These blocks were eliminated from the model due to optimizations: + * + * Block '/Scope1' : 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 @@ -80,7 +86,7 @@ extern void filtLowPass_step(RT_MODEL *const rtM); * * '' : 'BLDCmotorControl_FOC_R2017b_fixdt' * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass' - * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter1' + * '' : 'BLDCmotorControl_FOC_R2017b_fixdt/filtLowPass/Low_Pass_Filter' */ #endif /* RTW_HEADER_filtLowPass_h_ */ diff --git a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx index f8f04ca..0adcb37 100644 Binary files a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx and b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx differ diff --git a/02_Arduino/hoverserial/hoverserial.ino b/02_Arduino/hoverserial/hoverserial.ino index df05d57..7382521 100644 --- a/02_Arduino/hoverserial/hoverserial.ino +++ b/02_Arduino/hoverserial/hoverserial.ino @@ -25,7 +25,7 @@ // ########################## DEFINES ########################## #define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) -#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication +#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication #define TIME_SEND 100 // [ms] Sending time interval #define SPEED_MAX_TEST 300 // [-] Maximum speed for testing //#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) @@ -52,12 +52,11 @@ typedef struct{ uint16_t start; int16_t cmd1; int16_t cmd2; - int16_t speedR; - int16_t speedL; int16_t speedR_meas; int16_t speedL_meas; int16_t batVoltage; int16_t boardTemp; + uint16_t cmdLed; uint16_t checksum; } SerialFeedback; SerialFeedback Feedback; @@ -92,7 +91,7 @@ void Receive() // Check for new data availability in the Serial buffer if (HoverSerial.available()) { incomingByte = HoverSerial.read(); // Read the incoming byte - bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame + bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev; // Construct the start frame } else { return; @@ -107,9 +106,9 @@ void Receive() // Copy received data if (bufStartFrame == START_FRAME) { // Initialize if new data is detected p = (byte *)&NewFeedback; - *p++ = incomingBytePrev; - *p++ = incomingByte; - idx = 2; + *p++ = incomingBytePrev; + *p++ = incomingByte; + idx = 2; } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data *p++ = incomingByte; idx++; @@ -118,8 +117,8 @@ void Receive() // Check if we reached the end of the package if (idx == sizeof(SerialFeedback)) { uint16_t checksum; - checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL - ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp); + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas + ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed); // Check validity of the new data if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { @@ -129,12 +128,11 @@ void Receive() // Print data to built-in Serial Serial.print("1: "); Serial.print(Feedback.cmd1); Serial.print(" 2: "); Serial.print(Feedback.cmd2); - Serial.print(" 3: "); Serial.print(Feedback.speedR); - Serial.print(" 4: "); Serial.print(Feedback.speedL); - Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); - Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); - Serial.print(" 7: "); Serial.print(Feedback.batVoltage); - Serial.print(" 8: "); Serial.println(Feedback.boardTemp); + Serial.print(" 3: "); Serial.print(Feedback.speedR_meas); + Serial.print(" 4: "); Serial.print(Feedback.speedL_meas); + Serial.print(" 5: "); Serial.print(Feedback.batVoltage); + Serial.print(" 6: "); Serial.print(Feedback.boardTemp); + Serial.print(" 7: "); Serial.println(Feedback.cmdLed); } else { Serial.println("Non-valid data skipped"); } @@ -161,7 +159,7 @@ void loop(void) // Send commands if (iTimeSend > timeNow) return; iTimeSend = timeNow + TIME_SEND; - Send(0, abs(iTest)); + Send(0, SPEED_MAX_TEST - 2*abs(iTest)); // Calculate test command signal iTest += 10; diff --git a/Inc/config.h b/Inc/config.h index 5b37992..12b402e 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -16,7 +16,7 @@ //#define VARIANT_PPM // Variant for RC-Remote with PPM-Sum Signal //#define VARIANT_IBUS // Variant for RC-Remotes with FLYSKY IBUS //#define VARIANT_HOVERCAR // Variant for HOVERCAR build - //#define VARIANT_HOVERBOARD // Variant for HOVERBOARD build + //#define VARIANT_HOVERBOARD // Variant for HOVERBOARD build //#define VARIANT_TRANSPOTTER // Variant for TRANSPOTTER build https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter https://hackaday.io/project/161891-transpotter-ng #endif // ########################### END OF VARIANT SELECTION ############################ @@ -67,11 +67,15 @@ #define BAT_CALIB_REAL_VOLTAGE 3970 // input voltage measured by multimeter (multiplied by 100). In this case 43.00 V * 100 = 4300 #define BAT_CALIB_ADC 1492 // adc-value measured by mainboard (value nr 5 on UART debug output) #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_LVL2_ENABLE 1 // to beep or not to beep, 1 or 0 -#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 +#define BAT_LVL2_ENABLE 0 // to beep or not to beep, 1 or 0 +#define BAT_LVL1_ENABLE 1 // to beep or not to beep, 1 or 0 +#define BAT_BLINK_INTERVAL 80 // battery led blink interval (80 loops * 5ms ~= 400ms) +#define BAT_LVL5 (390 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Green blink: no beep +#define BAT_LVL4 (380 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Yellow: no beep +#define BAT_LVL3 (370 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Yellow blink: no beep +#define BAT_LVL2 (360 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Red: gently beep at this voltage level. [V*100/cell]. In this case 3.60 V/cell +#define BAT_LVL1 (350 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // Red blink: fast beep. Your battery is almost empty. Charge now! [V*100/cell]. In this case 3.50 V/cell +#define BAT_DEAD (337 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // All leds off: undervoltage poweroff. (while not driving) [V*100/cell]. In this case 3.37 V/cell // ######################## END OF BATTERY ############################### @@ -135,24 +139,16 @@ #define PHASE_ADV_MAX 25 // [deg] Maximum Phase Advance angle (only for SIN). Higher angle results in higher maximum speed. #define FIELD_WEAK_HI 1500 // [-] Input target High threshold for reaching maximum Field Weakening / Phase Advance. Do NOT set this higher than 1500. #define FIELD_WEAK_LO 1000 // [-] Input target Low threshold for starting Field Weakening / Phase Advance. Do NOT set this higher than 1000. - -// Data checks - Do NOT touch -#if (FIELD_WEAK_ENA == 0) - #undef FIELD_WEAK_HI - #define FIELD_WEAK_HI 1000 // [-] This prevents the input target going beyond 1000 when Field Weakening is not enabled -#endif -#define INPUT_MAX MAX( 1000, FIELD_WEAK_HI) // [-] Defines the Input target maximum limitation -#define INPUT_MIN MIN(-1000,-FIELD_WEAK_HI) // [-] Defines the Input target minimum limitation -#define INPUT_MID INPUT_MAX / 2 // ########################### END OF MOTOR CONTROL ######################## // ############################## DEFAULT SETTINGS ############################ // Default settings will be applied at the end of this config file if not set before -#define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise. -#define BEEPS_BACKWARD 1 // 0 or 1 -// #define SUPPORT_BUTTONS // Define for buttons support on ADC, Nunchuck +#define INACTIVITY_TIMEOUT 8 // Minutes of not driving until poweroff. it is not very precise. +#define BEEPS_BACKWARD 1 // 0 or 1 +#define FLASH_WRITE_KEY 0x1234 // Flash writing key, used when writing data to flash memory +// #define SUPPORT_BUTTONS // Define for buttons support on ADC, Nunchuck /* FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16 * Value of COEFFICIENT is in fixdt(1,16,14) @@ -191,8 +187,8 @@ */ // #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! -#if defined(VARIANT_ADC) || defined(VARIANT_HOVERCAR) -#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! +#if defined(VARIANT_ADC) + #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #endif #ifndef VARIANT_TRANSPOTTER @@ -238,8 +234,11 @@ // ############################ VARIANT_USART SETTINGS ############################ #ifdef VARIANT_USART + // #define SIDEBOARD_SERIAL_USART2 // #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino // #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + + // #define SIDEBOARD_SERIAL_USART3 #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #endif @@ -302,15 +301,18 @@ #define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2! #define ADC_PROTECT_ENA // ADC Protection Enable flag. Use this flag to make sure the ADC is protected when GND or Vcc wire is disconnected #define ADC_PROTECT_TIMEOUT 30 // ADC Protection: number of wrong / missing input commands before safety state is taken - #define ADC_PROTECT_THRESH 400 // ADC Protection threshold below/above the MIN/MAX ADC values + #define ADC_PROTECT_THRESH 300 // ADC Protection threshold below/above the MIN/MAX ADC values #define ADC1_MIN 1000 // min ADC1-value while poti at minimum-position (0 - 4095) #define ADC1_MAX 2500 // max ADC1-value while poti at maximum-position (0 - 4095) #define ADC2_MIN 500 // min ADC2-value while poti at minimum-position (0 - 4095) #define ADC2_MAX 2200 // max ADC2-value while poti at maximum-position (0 - 4095) #define SPEED_COEFFICIENT 16384 // 1.0f #define STEER_COEFFICIENT 0 // 0.0f - //#define INVERT_R_DIRECTION // Invert rotation of right motor - //#define INVERT_L_DIRECTION // Invert rotation of left motor + // #define INVERT_R_DIRECTION // Invert rotation of right motor + // #define INVERT_L_DIRECTION // Invert rotation of left motor + #define SIDEBOARD_SERIAL_USART3 + #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + // #define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! #endif // Multiple tap detection: default DOUBLE Tap on Brake pedal (4 pulses) @@ -323,12 +325,13 @@ // ############################ VARIANT_HOVERBOARD SETTINGS ############################ -// ##### ! NOT IMPLEMENTED YET ! ##### +// Communication: [DONE] +// Balancing controller: [TODO] #ifdef VARIANT_HOVERBOARD - // #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino - // #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! - #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! For Arduino control check the hoverSerial.ino - #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define SIDEBOARD_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! + #define FEEDBACK_SERIAL_USART2 + #define SIDEBOARD_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuk or lcd) is used! + #define FEEDBACK_SERIAL_USART3 #endif // ######################## END OF VARIANT_HOVERBOARD SETTINGS ######################### @@ -354,28 +357,26 @@ // ########################### UART SETIINGS ############################ -#if defined(FEEDBACK_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) || \ - defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) - #define START_FRAME 0xAAAA // [-] Start frame definition for serial commands +#if defined(FEEDBACK_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || \ + defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) + #define SERIAL_START_FRAME 0xABCD // [-] Start frame definition for serial commands #define SERIAL_TIMEOUT 160 // [-] Serial timeout duration for the received data. 160 ~= 0.8 sec. Calculation: 0.8 sec / 0.005 sec #endif -#if defined(FEEDBACK_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) +#if defined(FEEDBACK_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) #ifndef USART2_BAUD #define USART2_BAUD 38400 // UART2 baud rate (long wired cable) #endif #define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #endif -#if defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) +#if defined(FEEDBACK_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) #define USART3_BAUD 38400 // UART3 baud rate (short wired cable) #define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B #endif -#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) - #define UART_DMA_CHANNEL DMA1_Channel7 -#endif - -#if defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) - #define UART_DMA_CHANNEL DMA1_Channel2 +#if defined(DEBUG_SERIAL_USART2) + #define UART_DMA_CHANNEL_TX DMA1_Channel7 +#elif defined(DEBUG_SERIAL_USART3) + #define UART_DMA_CHANNEL_TX DMA1_Channel2 #endif // ########################### UART SETIINGS ############################ @@ -408,8 +409,12 @@ #error CONTROL_SERIAL_USART2 and CONTROL_SERIAL_USART3 not allowed, choose one. #endif -#if defined(FEEDBACK_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART3) - #error FEEDBACK_SERIAL_USART2 and FEEDBACK_SERIAL_USART3 not allowed, choose one. +#if defined(CONTROL_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART2) + #error CONTROL_SERIAL_USART2 and SIDEBOARD_SERIAL_USART2 not allowed, choose one. +#endif + +#if defined(CONTROL_SERIAL_USART3) && defined(SIDEBOARD_SERIAL_USART3) + #error CONTROL_SERIAL_USART3 and SIDEBOARD_SERIAL_USART3 not allowed, choose one. #endif #if defined(DEBUG_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2) @@ -424,19 +429,19 @@ #error DEBUG_SERIAL_USART2 and DEBUG_SERIAL_USART3 not allowed, choose one. #endif -#if defined(CONTROL_ADC) && (defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)) +#if defined(CONTROL_ADC) && (defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)) #error CONTROL_ADC and SERIAL_USART2 not allowed. It is on the same cable. #endif -#if (defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2)) && defined(CONTROL_PPM) +#if (defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)) && defined(CONTROL_PPM) #error CONTROL_PPM and SERIAL_USART2 not allowed. It is on the same cable. #endif -#if (defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3)) && defined(CONTROL_NUNCHUK) +#if (defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)) && defined(CONTROL_NUNCHUK) #error CONTROL_NUNCHUK and SERIAL_USART3 not allowed. It is on the same cable. #endif -#if (defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3)) && defined(DEBUG_I2C_LCD) +#if (defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)) && defined(DEBUG_I2C_LCD) #error DEBUG_I2C_LCD and SERIAL_USART3 not allowed. It is on the same cable. #endif @@ -444,12 +449,12 @@ #error only 1 input method allowed. use CONTROL_PPM or CONTROL_ADC or CONTROL_NUNCHUK. #endif -#if defined(ADC_PROTECT_ENA) && ((ADC1_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC1_MAX + ADC_PROTECT_THRESH) >= 4096) +#if defined(ADC_PROTECT_ENA) && ((ADC1_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC1_MAX + ADC_PROTECT_THRESH) >= 4095) #warning ADC1 Protection NOT possible! Adjust the ADC thresholds. #undef ADC_PROTECT_ENA #endif -#if defined(ADC_PROTECT_ENA) && ((ADC2_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC2_MAX + ADC_PROTECT_THRESH) >= 4096) +#if defined(ADC_PROTECT_ENA) && ((ADC2_MIN - ADC_PROTECT_THRESH) <= 0 || (ADC2_MAX + ADC_PROTECT_THRESH) >= 4095) #warning ADC2 Protection NOT possible! Adjust the ADC thresholds. #undef ADC_PROTECT_ENA #endif diff --git a/Inc/defines.h b/Inc/defines.h index 065466f..fd3e0b8 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -150,33 +150,16 @@ typedef struct { uint16_t dcr; uint16_t dcl; - uint16_t rl1; - uint16_t rl2; - uint16_t rr1; - uint16_t rr2; + uint16_t rlA; + uint16_t rlB; + uint16_t rrB; + uint16_t rrC; uint16_t batt1; uint16_t l_tx2; uint16_t temp; uint16_t l_rx2; } adc_buf_t; -typedef struct { - uint32_t t_timePrev; - uint8_t z_pulseCntPrev; - uint8_t b_hysteresis; - uint8_t b_multipleTap; -} MultipleTap; - -// Define Beep functions -void longBeep(uint8_t freq); -void shortBeep(uint8_t freq); - -// Define additional functions. Implementation is in main.c -void filtLowPass32(int16_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); -void rateLimiter16(int16_t u, int16_t rate, int16_t *y); -void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x); - // Define I2C, Nunchuk, PPM functions void I2C_Init(void); void Nunchuk_Init(void); @@ -185,5 +168,15 @@ uint8_t Nunchuk_Ping(void); void PPM_Init(void); void PPM_ISR_Callback(void); +// Sideboard definitions +#define LED1_SET (0x01) +#define LED2_SET (0x02) +#define LED3_SET (0x04) +#define LED4_SET (0x08) +#define LED5_SET (0x10) +#define SENSOR1_SET (0x01) +#define SENSOR2_SET (0x02) +#define SENSOR_MPU (0x04) + #endif diff --git a/Inc/eeprom.h b/Inc/eeprom.h index 6955187..d992b27 100644 --- a/Inc/eeprom.h +++ b/Inc/eeprom.h @@ -178,12 +178,12 @@ #define PAGE_SIZE (uint32_t)FLASH_PAGE_SIZE /* Page size */ /* EEPROM start address in Flash */ -#define EEPROM_START_ADDRESS ((uint32_t)ADDR_FLASH_PAGE_32) /* EEPROM emulation start address */ +#define EEPROM_START_ADDRESS ((uint32_t)ADDR_FLASH_PAGE_64) /* EEPROM emulation start address */ /* Pages 0 and 1 base and end addresses */ #define PAGE0_BASE_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + 0x0000)) #define PAGE0_END_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + (PAGE_SIZE - 1))) -#define PAGE0_ID ADDR_FLASH_PAGE_32 +#define PAGE0_ID ADDR_FLASH_PAGE_64 #define PAGE1_BASE_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + 0x10000)) #define PAGE1_END_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + 0x10000 + PAGE_SIZE - 1)) @@ -209,7 +209,7 @@ #define PAGE_FULL ((uint8_t)0x80) /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x03) +#define NB_OF_VAR ((uint8_t)0x09) /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/Inc/setup.h b/Inc/setup.h index e6c31b4..b7dd9d0 100644 --- a/Inc/setup.h +++ b/Inc/setup.h @@ -19,7 +19,9 @@ * along with this program. If not, see . */ -#pragma once +// Define to prevent recursive inclusion +#ifndef SETUP_H +#define SETUP_H #include "stm32f1xx_hal.h" @@ -29,3 +31,6 @@ void MX_ADC1_Init(void); void MX_ADC2_Init(void); void UART2_Init(void); void UART3_Init(void); + +#endif + diff --git a/Inc/util.h b/Inc/util.h new file mode 100644 index 0000000..3e864c8 --- /dev/null +++ b/Inc/util.h @@ -0,0 +1,96 @@ +/** + * This file is part of the hoverboard-firmware-hack project. + * + * Copyright (C) 2020-2021 Emanuel FERU + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . +*/ + +// Define to prevent recursive inclusion +#ifndef UTIL_H +#define UTIL_H + +#include + +#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) + #ifdef CONTROL_IBUS + typedef struct{ + uint8_t start; + uint8_t type; + uint8_t channels[IBUS_NUM_CHANNELS*2]; + uint8_t checksuml; + uint8_t checksumh; + } Serialcommand; + #else + typedef struct{ + uint16_t start; + int16_t steer; + int16_t speed; + uint16_t checksum; + } Serialcommand; + #endif +#endif +#if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) + typedef struct{ + uint16_t start; + int16_t roll; + int16_t pitch; + int16_t yaw; + uint16_t sensors; + uint16_t checksum; + } SerialSideboard; +#endif + +// Initialization Functions +void BLDC_Init(void); +void Input_Lim_Init(void); +void Input_Init(void); + +// General Functions +void poweronMelody(void); +void shortBeep(uint8_t freq); +void shortBeepMany(uint8_t cnt); +void longBeep(uint8_t freq); +void calcAvgSpeed(void); +void adcCalibLim(void); +void updateCurSpdLim(void); +void saveConfig(void); + +// Poweroff Functions +void poweroff(void); +void poweroffPressCheck(void); + +// Read Command Function +void readCommand(void); + +// Sideboard functions +void sideboardLeds(uint8_t *leds); +void sideboardSensors(uint8_t sensors); + +// Filtering Functions +void filtLowPass32(int32_t u, uint16_t coef, int32_t *y); +void rateLimiter16(int16_t u, int16_t rate, int16_t *y); +void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t *rty_speedL); + +// Multiple Tap Function +typedef struct { + uint32_t t_timePrev; + uint8_t z_pulseCntPrev; + uint8_t b_hysteresis; + uint8_t b_multipleTap; +} MultipleTap; +void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x); + +#endif + diff --git a/MDK-ARM/mainboard-hack.uvoptx b/MDK-ARM/mainboard-hack.uvoptx index c04c5d7..0dc5c18 100644 --- a/MDK-ARM/mainboard-hack.uvoptx +++ b/MDK-ARM/mainboard-hack.uvoptx @@ -75,7 +75,7 @@ 1 0 - 1 + 0 18 @@ -840,7 +840,7 @@ 1 0 - 0 + 1 18 @@ -1247,7 +1247,7 @@ Startup - 0 + 1 0 0 0 @@ -1267,7 +1267,7 @@ Src - 0 + 1 0 0 0 @@ -1403,6 +1403,30 @@ 0 0 + + 2 + 13 + 1 + 0 + 0 + 0 + ..\Src\util.c + util.c + 0 + 0 + + + 2 + 14 + 5 + 0 + 0 + 0 + ..\Inc\config.h + config.h + 0 + 0 + @@ -1413,7 +1437,7 @@ 0 3 - 13 + 15 1 0 0 @@ -1425,7 +1449,7 @@ 3 - 14 + 16 1 0 0 @@ -1437,7 +1461,7 @@ 3 - 15 + 17 1 0 0 @@ -1449,7 +1473,7 @@ 3 - 16 + 18 1 0 0 @@ -1461,7 +1485,7 @@ 3 - 17 + 19 1 0 0 @@ -1473,7 +1497,7 @@ 3 - 18 + 20 1 0 0 @@ -1485,7 +1509,7 @@ 3 - 19 + 21 1 0 0 @@ -1497,7 +1521,7 @@ 3 - 20 + 22 1 0 0 @@ -1509,7 +1533,7 @@ 3 - 21 + 23 1 0 0 @@ -1521,7 +1545,7 @@ 3 - 22 + 24 1 0 0 @@ -1533,7 +1557,7 @@ 3 - 23 + 25 1 0 0 @@ -1545,7 +1569,7 @@ 3 - 24 + 26 1 0 0 @@ -1557,7 +1581,7 @@ 3 - 25 + 27 1 0 0 @@ -1569,7 +1593,7 @@ 3 - 26 + 28 1 0 0 @@ -1581,7 +1605,7 @@ 3 - 27 + 29 1 0 0 @@ -1593,7 +1617,7 @@ 3 - 28 + 30 1 0 0 @@ -1613,7 +1637,7 @@ 0 4 - 29 + 31 1 0 0 diff --git a/MDK-ARM/mainboard-hack.uvprojx b/MDK-ARM/mainboard-hack.uvprojx index fd662e8..8958233 100644 --- a/MDK-ARM/mainboard-hack.uvprojx +++ b/MDK-ARM/mainboard-hack.uvprojx @@ -444,6 +444,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -984,6 +994,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -1592,6 +1612,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -2200,6 +2230,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -2808,6 +2848,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -3416,6 +3466,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -4024,6 +4084,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + @@ -4564,6 +4634,16 @@ 1 ..\Src\stm32f1xx_it.c + + util.c + 1 + ..\Src\util.c + + + config.h + 5 + ..\Inc\config.h + diff --git a/Makefile b/Makefile index 82bd7aa..6880e1e 100644 --- a/Makefile +++ b/Makefile @@ -38,6 +38,7 @@ Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c \ Src/system_stm32f1xx.c \ Src/setup.c \ Src/control.c \ +Src/util.c \ Src/main.c \ Src/bldc.c \ Src/eeprom.c \ diff --git a/README.md b/README.md index 33cfaff..0fc0a63 100644 --- a/README.md +++ b/README.md @@ -83,7 +83,7 @@ Each motor is constantly monitored for errors. These errors are: - **Error 002**: Hall sensor short circuit - **Error 004**: Motor NOT able to spin (Possible causes: motor phase disconnected, MOSFET defective, operational Amplifier defective, motor blocked) -The error codes above are reported for each motor in the variables **errCode_Left** and **errCode_Right** for Left motor (long wired motor) and Right motor (short wired motor), respectively. In case of error, the motor power is reduced to 0, while an audible (fast beep) can be heard to notify the user. +The error codes above are reported for each motor in the variables **rtY_Left.z_errCode** and **rtY_Right.z_errCode** for Left motor (long wired motor) and Right motor (short wired motor), respectively. In case of error, the motor power is reduced to 0, while an audible (fast beep) can be heard to notify the user. ### Demo videos @@ -164,6 +164,7 @@ This firmware offers currently these variants (selectable in [platformio.ini](/p - **VARIANT_PPM**: This is when you want to use a RC remote control with PPM Sum signal - **VARIANT_IBUS**: This is when you want to use a RC remote control with Flysky IBUS protocol connected to the Left sensor cable. - **VARIANT_HOVERCAR**: In this variant the motors are controlled by two pedals brake and throttle. Reverse is engaged by double tapping on the brake pedal at standstill. +- **VARIANT_HOVERBOARD**: In this variant the mainboard reads the sideboards data. The sideboards need to be flashed with the hacked version. Only balancing controller is still to be implemented. - **VARIANT_TRANSPOTTER**: This build is for transpotter which is a hoverboard based transportation system. For more details on how to build it check [here](https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter) and [here](https://hackaday.io/project/161891-transpotter-ng). Of course the firmware can be further customized for other needs or projects. diff --git a/Src/bldc.c b/Src/bldc.c index 7e72a27..680d70f 100644 --- a/Src/bldc.c +++ b/Src/bldc.c @@ -26,6 +26,7 @@ #include "defines.h" #include "setup.h" #include "config.h" +#include "util.h" // Matlab includes and defines - from auto-code generation // ############################################################################### @@ -35,11 +36,11 @@ extern RT_MODEL *const rtM_Left; extern RT_MODEL *const rtM_Right; -extern DW rtDW_Left; /* Observable states */ +extern DW rtDW_Left; /* Observable states */ extern ExtU rtU_Left; /* External inputs */ extern ExtY rtY_Left; /* External outputs */ -extern DW rtDW_Right; /* Observable states */ +extern DW rtDW_Right; /* Observable states */ extern ExtU rtU_Right; /* External inputs */ extern ExtY rtY_Right; /* External outputs */ // ############################################################################### @@ -50,8 +51,6 @@ extern uint8_t ctrlModReq; static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV); int16_t curL_phaA = 0, curL_phaB = 0, curL_DC = 0; int16_t curR_phaB = 0, curR_phaC = 0, curR_DC = 0; -uint8_t errCode_Left = 0; -uint8_t errCode_Right = 0; volatile int pwml = 0; volatile int pwmr = 0; @@ -70,10 +69,10 @@ static uint8_t enableFin = 0; static const uint16_t pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000 static uint16_t offsetcount = 0; -static int16_t offsetrl1 = 2000; -static int16_t offsetrl2 = 2000; -static int16_t offsetrr1 = 2000; -static int16_t offsetrr2 = 2000; +static int16_t offsetrlA = 2000; +static int16_t offsetrlB = 2000; +static int16_t offsetrrB = 2000; +static int16_t offsetrrC = 2000; static int16_t offsetdcl = 2000; static int16_t offsetdcr = 2000; @@ -91,10 +90,10 @@ void DMA1_Channel1_IRQHandler(void) { if(offsetcount < 2000) { // calibrate ADC offsets offsetcount++; - offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2; - offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2; - offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2; - offsetrr2 = (adc_buffer.rr2 + offsetrr2) / 2; + offsetrlA = (adc_buffer.rlA + offsetrlA) / 2; + offsetrlB = (adc_buffer.rlB + offsetrlB) / 2; + offsetrrB = (adc_buffer.rrB + offsetrrB) / 2; + offsetrrC = (adc_buffer.rrC + offsetrrC) / 2; offsetdcl = (adc_buffer.dcl + offsetdcl) / 2; offsetdcr = (adc_buffer.dcr + offsetdcr) / 2; return; @@ -102,17 +101,17 @@ void DMA1_Channel1_IRQHandler(void) { if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer + batVoltage = (int16_t)(batVoltageFixdt >> 16); // 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_phaA = (int16_t)(offsetrlA - adc_buffer.rlA); + curL_phaB = (int16_t)(offsetrlB - adc_buffer.rlB); 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_phaB = (int16_t)(offsetrrB - adc_buffer.rrB); + curR_phaC = (int16_t)(offsetrrC - adc_buffer.rrC); curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr); // Disable PWM when current limit is reached (current chopping) @@ -152,7 +151,7 @@ void DMA1_Channel1_IRQHandler(void) { OverrunFlag = true; /* Make sure to stop BOTH motors in case of an error */ - enableFin = enable && !errCode_Left && !errCode_Right; + enableFin = enable && !rtY_Left.z_errCode && !rtY_Right.z_errCode; // ========================= LEFT MOTOR ============================ // Get hall sensors values @@ -178,7 +177,7 @@ void DMA1_Channel1_IRQHandler(void) { ul = rtY_Left.DC_phaA; vl = rtY_Left.DC_phaB; wl = rtY_Left.DC_phaC; - errCode_Left = rtY_Left.z_errCode; + // errCodeLeft = rtY_Left.z_errCode; // motSpeedLeft = rtY_Left.n_mot; // motAngleLeft = rtY_Left.a_elecAngle; @@ -213,7 +212,7 @@ void DMA1_Channel1_IRQHandler(void) { ur = rtY_Right.DC_phaA; vr = rtY_Right.DC_phaB; wr = rtY_Right.DC_phaC; - errCode_Right = rtY_Right.z_errCode; + // errCodeRight = rtY_Right.z_errCode; // motSpeedRight = rtY_Right.n_mot; // motAngleRight = rtY_Right.a_elecAngle; diff --git a/Src/comms.c b/Src/comms.c index d5c146d..bab9dde 100644 --- a/Src/comms.c +++ b/Src/comms.c @@ -30,11 +30,11 @@ void consoleScope(void) { uart_buf[8] = CLAMP(ch_buf[7]+127, 0, 255); uart_buf[9] = '\n'; - if(UART_DMA_CHANNEL->CNDTR == 0) { - UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; - UART_DMA_CHANNEL->CNDTR = 10; - UART_DMA_CHANNEL->CMAR = (uint32_t)uart_buf; - UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; + if(UART_DMA_CHANNEL_TX->CNDTR == 0) { + UART_DMA_CHANNEL_TX->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL_TX->CNDTR = 10; + UART_DMA_CHANNEL_TX->CMAR = (uint32_t)uart_buf; + UART_DMA_CHANNEL_TX->CCR |= DMA_CCR_EN; } #endif @@ -45,11 +45,11 @@ void consoleScope(void) { "1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i 8:%i\r\n", ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3], ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]); - if(UART_DMA_CHANNEL->CNDTR == 0) { - UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; - UART_DMA_CHANNEL->CNDTR = strLength; - UART_DMA_CHANNEL->CMAR = (uint32_t)uart_buf; - UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; + if(UART_DMA_CHANNEL_TX->CNDTR == 0) { + UART_DMA_CHANNEL_TX->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL_TX->CNDTR = strLength; + UART_DMA_CHANNEL_TX->CMAR = (uint32_t)uart_buf; + UART_DMA_CHANNEL_TX->CCR |= DMA_CCR_EN; } #endif } @@ -57,11 +57,11 @@ void consoleScope(void) { void consoleLog(char *message) { #if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3) - if(UART_DMA_CHANNEL->CNDTR == 0) { - UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; - UART_DMA_CHANNEL->CNDTR = strlen((char *)(uintptr_t)message); - UART_DMA_CHANNEL->CMAR = (uint32_t)message; - UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; + if(UART_DMA_CHANNEL_TX->CNDTR == 0) { + UART_DMA_CHANNEL_TX->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL_TX->CNDTR = strlen((char *)(uintptr_t)message); + UART_DMA_CHANNEL_TX->CMAR = (uint32_t)message; + UART_DMA_CHANNEL_TX->CCR |= DMA_CCR_EN; } #endif } diff --git a/Src/control.c b/Src/control.c index 1c52dcd..7a40aa3 100644 --- a/Src/control.c +++ b/Src/control.c @@ -114,12 +114,12 @@ void Nunchuk_Read(void) { timeout = 0; } -#ifndef TRANSPOTTER - if (timeout > 3) { - HAL_Delay(50); - Nunchuk_Init(); - } -#endif + #ifndef TRANSPOTTER + if (timeout > 3) { + HAL_Delay(50); + Nunchuk_Init(); + } + #endif //setScopeChannel(0, (int)nunchuk_data[0]); //setScopeChannel(1, (int)nunchuk_data[1]); diff --git a/Src/main.c b/Src/main.c index ecccba0..a20e421 100644 --- a/Src/main.c +++ b/Src/main.c @@ -25,146 +25,45 @@ #include "defines.h" #include "setup.h" #include "config.h" +#include "util.h" #include "comms.h" -#include "eeprom.h" - -#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) - #include "hd44780.h" -#endif - - -// Matlab includes and defines - from auto-code generation -// ############################################################################### -#include "BLDC_controller.h" /* Model's header file */ +#include "BLDC_controller.h" /* BLDC's header file */ #include "rtwtypes.h" -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_; - -extern P rtP_Left; /* Block parameters (auto storage) */ -DW rtDW_Left; /* Observable states */ -ExtU rtU_Left; /* External inputs */ -ExtY rtY_Left; /* External outputs */ - -P rtP_Right; /* Block parameters (auto storage) */ -DW rtDW_Right; /* Observable states */ -ExtU rtU_Right; /* External inputs */ -ExtY rtY_Right; /* External outputs */ - -extern uint8_t errCode_Left; /* Global variable to handle Motor error codes */ -extern uint8_t errCode_Right; /* Global variable to handle Motor error codes */ -// ############################################################################### +#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) +#include "hd44780.h" +#endif void SystemClock_Config(void); -void poweroff(void); +//------------------------------------------------------------------------ +// Global variables set externally +//------------------------------------------------------------------------ extern TIM_HandleTypeDef htim_left; extern TIM_HandleTypeDef htim_right; extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; extern volatile adc_buf_t adc_buffer; #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) - LCD_PCF8574_HandleTypeDef lcd; -#endif -extern I2C_HandleTypeDef hi2c2; -#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) \ - || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) - extern UART_HandleTypeDef huart2; - extern UART_HandleTypeDef huart3; - static UART_HandleTypeDef huart; -#endif - -#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) + extern LCD_PCF8574_HandleTypeDef lcd; extern uint8_t LCDerrorFlag; #endif -#ifdef VARIANT_TRANSPOTTER - uint8_t nunchuk_connected = 0; - float steering; - int feedforward; +// Matlab defines - from auto-code generation +//--------------- +extern P rtP_Left; /* Block parameters (auto storage) */ +extern P rtP_Right; /* Block parameters (auto storage) */ +extern ExtY rtY_Left; /* External outputs */ +extern ExtY rtY_Right; /* External outputs */ +//--------------- - void saveConfig(void); +extern int16_t cmd1; // normalized input value. -1000 to 1000 +extern int16_t cmd2; // normalized input value. -1000 to 1000 - /* Virtual address defined by the user: 0xFFFF value is prohibited */ - uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; - uint16_t VarDataTab[NB_OF_VAR] = {0}; - uint16_t VarValue = 0; - uint16_t saveValue = 0; - - uint16_t counter = 0; -#else - uint8_t nunchuk_connected = 1; - uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy address to avoid warnings -#endif - -#if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA) -static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection -#endif -static uint8_t timeoutFlagADC = 0; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) - -#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) - #ifdef CONTROL_IBUS - static uint16_t ibus_chksum; - static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS]; - - typedef struct{ - uint8_t start; - uint8_t type; - uint8_t channels[IBUS_NUM_CHANNELS*2]; - uint8_t checksuml; - uint8_t checksumh; - } Serialcommand; - #else - typedef struct{ - uint16_t start; - int16_t steer; - int16_t speed; - uint16_t checksum; - } Serialcommand; - #endif -static volatile Serialcommand command; -static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command -#endif -static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) - -#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) -typedef struct{ - uint16_t start; - int16_t cmd1; - int16_t cmd2; - int16_t speedR; - int16_t speedL; - int16_t speedR_meas; - int16_t speedL_meas; - int16_t batVoltage; - int16_t boardTemp; - uint16_t checksum; -} SerialFeedback; -static SerialFeedback Feedback; -#endif - -#ifdef SUPPORT_BUTTONS -static uint8_t button1, button2; -#endif - -uint8_t ctrlModReqRaw = CTRL_MOD_REQ; -uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request -static int16_t speed; // local variable for speed. -1000 to 1000 -#ifndef VARIANT_TRANSPOTTER - static int cmd1; // normalized input value. -1000 to 1000 - static int cmd2; // normalized input value. -1000 to 1000 - static int16_t steer; // local variable for steering. -1000 to 1000 - static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter - static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter - static int32_t steerFixdt; // local fixed-point variable for steering low-pass filter - static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter -#endif -static MultipleTap MultipleTapBreak; // define multiple tap functionality for the Break pedal - -static int16_t speedAvg; // average measured speed -static int16_t speedAvgAbs; // average measured speed in absolute +extern int16_t speedAvg; // Average measured speed +extern int16_t speedAvgAbs; // Average measured speed in absolute +extern uint8_t timeoutFlagADC; // Timeout Flag for for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) +extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) extern volatile int pwml; // global variable for pwm left. -1000 to 1000 extern volatile int pwmr; // global variable for pwm right. -1000 to 1000 @@ -177,28 +76,67 @@ extern uint8_t enable; // global variable for motor enable extern volatile uint32_t timeout; // global variable for timeout extern int16_t batVoltage; // global variable for battery voltage -static uint32_t inactivity_timeout_counter; -static uint32_t main_loop_counter; - -extern uint8_t nunchuk_data[6]; -#ifdef CONTROL_PPM -extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1]; +#if defined(SIDEBOARD_SERIAL_USART2) +extern SerialSideboard Sideboard_L; +#endif +#if defined(SIDEBOARD_SERIAL_USART3) +extern SerialSideboard Sideboard_R; #endif -void poweroff(void) { - // if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons - buzzerPattern = 0; - enable = 0; - consoleLog("-- Motors disabled --\r\n"); - for (int i = 0; i < 8; i++) { - buzzerFreq = (uint8_t)i; - HAL_Delay(100); - } - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); - while(1) {} - // } -} +//------------------------------------------------------------------------ +// Global variables set here in main.c +//------------------------------------------------------------------------ +uint8_t backwardDrive; +volatile uint32_t main_loop_counter; + +//------------------------------------------------------------------------ +// Local variables +//------------------------------------------------------------------------ +#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t batVoltage; + int16_t boardTemp; + uint16_t cmdLed; + uint16_t checksum; +} SerialFeedback; +static SerialFeedback Feedback; +#endif +#if defined(FEEDBACK_SERIAL_USART2) +static uint8_t sideboard_leds_L; +#endif +#if defined(FEEDBACK_SERIAL_USART3) +static uint8_t sideboard_leds_R; +#endif + +#ifdef VARIANT_TRANSPOTTER + extern uint8_t nunchuk_connected; + extern float setDistance; + + static uint8_t checkRemote = 0; + static uint16_t distance; + static float steering; + static int distanceErr; + static int lastDistance = 0; + static uint16_t transpotter_counter = 0; +#endif + +static int16_t speed; // local variable for speed. -1000 to 1000 +#ifndef VARIANT_TRANSPOTTER + static int16_t steer; // local variable for steering. -1000 to 1000 + static int16_t steerRateFixdt; // local fixed-point variable for steering rate limiter + static int16_t speedRateFixdt; // local fixed-point variable for speed rate limiter + static int32_t steerFixdt; // local fixed-point variable for steering low-pass filter + static int32_t speedFixdt; // local fixed-point variable for speed low-pass filter +#endif + +static uint32_t inactivity_timeout_counter; +static MultipleTap MultipleTapBreak; // define multiple tap functionality for the Break pedal int main(void) { @@ -229,143 +167,20 @@ int main(void) { MX_TIM_Init(); MX_ADC1_Init(); MX_ADC2_Init(); + BLDC_Init(); // BLDC Controller Init + Input_Lim_Init(); // Input Limitations Init + Input_Init(); // Input Init HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); HAL_ADC_Start(&hadc1); - HAL_ADC_Start(&hadc2); - -// Matlab Init -// ############################################################################### - - /* Set BLDC controller parameters */ - rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases = {iA, iB} -> do NOT change - rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL; - rtP_Left.b_diagEna = DIAG_ENA; - rtP_Left.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) - rtP_Left.n_max = N_MOT_MAX << 4; // fixdt(1,16,4) - rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA; - rtP_Left.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) - rtP_Left.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4) - rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) - - rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters - rtP_Right.b_selPhaABCurrMeas = 0; // Left motor measured current phases = {iB, iC} -> do NOT change - - /* Pack LEFT motor data into RTM */ - rtM_Left->defaultParam = &rtP_Left; - rtM_Left->dwork = &rtDW_Left; - rtM_Left->inputs = &rtU_Left; - rtM_Left->outputs = &rtY_Left; - - /* Pack RIGHT motor data into RTM */ - rtM_Right->defaultParam = &rtP_Right; - rtM_Right->dwork = &rtDW_Right; - rtM_Right->inputs = &rtU_Right; - rtM_Right->outputs = &rtY_Right; - - /* Initialize BLDC controllers */ - BLDC_controller_initialize(rtM_Left); - BLDC_controller_initialize(rtM_Right); - -// ############################################################################### - - for (int i = 8; i >= 0; i--) { - buzzerFreq = (uint8_t)i; - HAL_Delay(100); - } - buzzerFreq = 0; + HAL_ADC_Start(&hadc2); + poweronMelody(); HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); - #ifdef VARIANT_TRANSPOTTER - int lastDistance = 0; - enable = 1; - uint8_t checkRemote = 0; - - HAL_FLASH_Unlock(); - - /* EEPROM Init */ - EE_Init(); - - EE_ReadVariable(VirtAddVarTab[0], &saveValue); - - HAL_FLASH_Lock(); - float setDistance = saveValue / 1000.0; - if (setDistance < 0.2) { - setDistance = 1.0; - } - #endif - - #ifdef CONTROL_PPM - PPM_Init(); - #endif - - #ifdef CONTROL_NUNCHUK - I2C_Init(); - Nunchuk_Init(); - #endif - - #if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) - UART2_Init(); - huart = huart2; - #endif - #if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) - UART3_Init(); - huart = huart3; - #endif - #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) - HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); - #endif - - #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) - I2C_Init(); - HAL_Delay(50); - lcd.pcf8574.PCF_I2C_ADDRESS = 0x27; - lcd.pcf8574.PCF_I2C_TIMEOUT = 5; - lcd.pcf8574.i2c = hi2c2; - lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2; - lcd.type = TYPE0; - - if(LCD_Init(&lcd)!=LCD_OK){ - // error occured - //TODO while(1); - } - - LCD_ClearDisplay(&lcd); - HAL_Delay(5); - LCD_SetLocation(&lcd, 0, 0); - #ifdef VARIANT_TRANSPOTTER - LCD_WriteString(&lcd, "TranspOtter V2.1"); - #else - LCD_WriteString(&lcd, "Hover V2.0"); - #endif - LCD_SetLocation(&lcd, 0, 1); - LCD_WriteString(&lcd, "Initializing..."); - #endif - - #if defined(VARIANT_TRANSPOTTER) && defined(SUPPORT_LCD) - LCD_ClearDisplay(&lcd); - HAL_Delay(5); - LCD_SetLocation(&lcd, 0, 1); - LCD_WriteString(&lcd, "Bat:"); - LCD_SetLocation(&lcd, 8, 1); - LCD_WriteString(&lcd, "V"); - - LCD_SetLocation(&lcd, 15, 1); - LCD_WriteString(&lcd, "A"); - - LCD_SetLocation(&lcd, 0, 0); - LCD_WriteString(&lcd, "Len:"); - LCD_SetLocation(&lcd, 8, 0); - LCD_WriteString(&lcd, "m("); - LCD_SetLocation(&lcd, 14, 0); - LCD_WriteString(&lcd, "m)"); - #endif - + int16_t speedL = 0, speedR = 0; int16_t lastSpeedL = 0, lastSpeedR = 0; - int16_t speedL = 0, speedR = 0; int32_t board_temp_adcFixdt = adc_buffer.temp << 20; // Fixed-point filter output initialized with current ADC converted to fixed-point int16_t board_temp_adcFilt = adc_buffer.temp; @@ -373,252 +188,20 @@ int main(void) { while(1) { - HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms + HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms - #ifdef VARIANT_TRANSPOTTER - if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - HAL_Delay(10); - } - shortBeep(5); - HAL_Delay(300); - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - HAL_Delay(10); - } - longBeep(5); - HAL_Delay(350); - poweroff(); - } else { - setDistance += 0.25; - if (setDistance > 2.6) { - setDistance = 0.5; - } - shortBeep(setDistance / 0.25); - saveValue = setDistance * 1000; - saveConfig(); - } - } - - #ifdef GAMETRAK_CONNECTION_NORMAL - uint16_t distance = CLAMP((adc_buffer.l_rx2) - 180, 0, 4095); - steering = (adc_buffer.l_tx2 - 2048) / 2048.0; - #endif - #ifdef GAMETRAK_CONNECTION_ALTERNATE - uint16_t distance = CLAMP((adc_buffer.l_tx2) - 180, 0, 4095); - steering = (adc_buffer.l_rx2 - 2048) / 2048.0; - #endif - - feedforward = ((distance - (int)(setDistance * 1345))); - - if (nunchuk_connected == 0) { - speedL = speedL * 0.8f + (CLAMP(feedforward + ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f); - speedR = speedR * 0.8f + (CLAMP(feedforward - ((steering)*((float)MAX(ABS(feedforward), 50)) * ROT_P), -850, 850) * -0.2f); - if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) { - if (distance - (int)(setDistance * 1345) > 0) { - enable = 1; - } - if (distance - (int)(setDistance * 1345) > -300) { - #ifdef INVERT_R_DIRECTION - pwmr = speedR; - #endif - #ifndef INVERT_R_DIRECTION - pwmr = -speedR; - #endif - #ifdef INVERT_L_DIRECTION - pwml = -speedL; - #endif - #ifndef INVERT_L_DIRECTION - pwml = speedL; - #endif - - if (checkRemote) { - if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) { - //enable = 1; - } else { - enable = 0; - } - } - } else { - enable = 0; - } - } - lastSpeedL = speedL; - lastSpeedR = speedR; - - timeout = 0; - } - #endif - - #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) - if (nunchuk_connected != 0) { - Nunchuk_Read(); - cmd1 = CLAMP((nunchuk_data[0] - 127) * 8, INPUT_MIN, INPUT_MAX); // x - axis. Nunchuk joystick readings range 30 - 230 - cmd2 = CLAMP((nunchuk_data[1] - 128) * 8, INPUT_MIN, INPUT_MAX); // y - axis - - #ifdef SUPPORT_BUTTONS - button1 = (uint8_t)nunchuk_data[5] & 1; - button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; - #endif - } - #endif - - #ifdef CONTROL_PPM - cmd1 = CLAMP((ppm_captured_value[0] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP((ppm_captured_value[1] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); - #ifdef SUPPORT_BUTTONS - button1 = ppm_captured_value[5] > INPUT_MID; - button2 = 0; - #endif - // float scale = ppm_captured_value[2] / 1000.0f; // not used for now, uncomment if needed - #endif - - #ifdef CONTROL_ADC - // ADC values range: 0-4095, see ADC-calibration in config.h - #ifdef ADC1_MID_POT - cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID) * INPUT_MAX / (ADC1_MAX - ADC1_MID), 0, INPUT_MAX) - -CLAMP((ADC1_MID - adc_buffer.l_tx2) * INPUT_MAX / (ADC1_MID - ADC1_MIN), 0, INPUT_MAX); // ADC1 - #else - cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN) * INPUT_MAX / (ADC1_MAX - ADC1_MIN), 0, INPUT_MAX); // ADC1 - #endif - - #ifdef ADC2_MID_POT - cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID) * INPUT_MAX / (ADC2_MAX - ADC2_MID), 0, INPUT_MAX) - -CLAMP((ADC2_MID - adc_buffer.l_rx2) * INPUT_MAX / (ADC2_MID - ADC2_MIN), 0, INPUT_MAX); // ADC2 - #else - cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN) * INPUT_MAX / (ADC2_MAX - ADC2_MIN), 0, INPUT_MAX); // ADC2 - #endif - - #ifdef ADC_PROTECT_ENA - if (adc_buffer.l_tx2 >= (ADC1_MIN - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX + ADC_PROTECT_THRESH) && - adc_buffer.l_rx2 >= (ADC2_MIN - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX + ADC_PROTECT_THRESH)) { - if (timeoutFlagADC) { // Check for previous timeout flag - if (timeoutCntADC-- <= 0) // Timeout de-qualification - timeoutFlagADC = 0; // Timeout flag cleared - } else { - timeoutCntADC = 0; // Reset the timeout counter - } - } else { - if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification - timeoutFlagADC = 1; // Timeout detected - timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value - } - } - - if (timeoutFlagADC) { // In case of timeout bring the system to a Safe State - ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way - cmd1 = 0; - cmd2 = 0; - } else { - ctrlModReq = ctrlModReqRaw; // Follow the Mode request - } - #endif - - // use ADCs as button inputs: - #ifdef SUPPORT_BUTTONS - button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 - button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2 - #endif - - timeout = 0; - #endif - - #if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3 - - // Handle received data validity, timeout and fix out-of-sync if necessary - #ifdef CONTROL_IBUS - ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; - for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { - ibus_chksum -= command.channels[i]; - } - if (command.start == IBUS_LENGTH && command.type == IBUS_COMMAND && ibus_chksum == (uint16_t)((command.checksumh << 8) + command.checksuml)) { - if (timeoutFlagSerial) { // Check for previous timeout flag - if (timeoutCntSerial-- <= 0) // Timeout de-qualification - timeoutFlagSerial = 0; // Timeout flag cleared - } else { - for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { - ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 - } - cmd1 = CLAMP((ibus_captured_value[0] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP((ibus_captured_value[1] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); - command.start = 0xFF; // Change the Start Frame for timeout detection in the next cycle - timeoutCntSerial = 0; // Reset the timeout counter - } - } else { - if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlagSerial = 1; // Timeout detected - timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value - } - // Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA - if (main_loop_counter % 25 == 0 && command.start != IBUS_LENGTH && command.start != 0xFF) { - HAL_UART_DMAStop(&huart); - HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); - command.start = 0xFF; // Change the Start Frame to avoid entering again here if no data is received - } - } - #else - if (command.start == START_FRAME && command.checksum == (uint16_t)(command.start ^ command.steer ^ command.speed)) { - if (timeoutFlagSerial) { // Check for previous timeout flag - if (timeoutCntSerial-- <= 0) // Timeout de-qualification - timeoutFlagSerial = 0; // Timeout flag cleared - } else { - cmd1 = CLAMP((int16_t)command.steer, INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP((int16_t)command.speed, INPUT_MIN, INPUT_MAX); - command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle - timeoutCntSerial = 0; // Reset the timeout counter - } - } else { - if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification - timeoutFlagSerial = 1; // Timeout detected - timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value - } - // Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA - if (main_loop_counter % 25 == 0 && command.start != START_FRAME && command.start != 0xFFFF) { - HAL_UART_DMAStop(&huart); - HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); - command.start = 0xFFFF; // Change the Start Frame to avoid entering again here if no data is received - } - } - #endif - - if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State - ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way - cmd1 = 0; - cmd2 = 0; - } else { - ctrlModReq = ctrlModReqRaw; // Follow the Mode request - } - timeout = 0; - - #endif - - // Calculate measured average speed. The minus sign (-) is beacause motors spin in opposite directions - #if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) - speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2; - #elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) - speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2; - #elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) - speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2; - #elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) - speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2; - #endif - - // Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1) - if ((SPEED_COEFFICIENT & (1 << 16)) >> 16) { - speedAvg = -speedAvg; - } - speedAvgAbs = abs(speedAvg); + readCommand(); // Read Command: cmd1, cmd2 + calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs #ifndef VARIANT_TRANSPOTTER // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### - if (enable == 0 && (!errCode_Left && !errCode_Right) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ + if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ shortBeep(6); // make 2 beeps indicating the motor enable shortBeep(4); HAL_Delay(100); enable = 1; // enable motors } - // ####### VARIANT_HOVERCAR ####### + // ####### VARIANT_HOVERCAR ####### #ifdef VARIANT_HOVERCAR // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15) uint16_t speedBlend; @@ -647,8 +230,8 @@ int main(void) { rateLimiter16(cmd2, RATE, &speedRateFixdt); filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); - steer = (int16_t)(steerFixdt >> 20); // convert fixed-point to integer - speed = (int16_t)(speedFixdt >> 20); // convert fixed-point to integer + steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer + speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer // ####### VARIANT_HOVERCAR ####### #ifdef VARIANT_HOVERCAR @@ -660,15 +243,10 @@ int main(void) { #endif // ####### 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), INPUT_MIN, INPUT_MA); + // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above - #ifdef ADDITIONAL_CODE - ADDITIONAL_CODE; - #endif - - // ####### 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 @@ -684,25 +262,54 @@ int main(void) { } #endif - lastSpeedL = speedL; - lastSpeedR = speedR; - #ifdef VARIANT_TRANSPOTTER + distance = CLAMP(cmd1 - 180, 0, 4095); + steering = (cmd2 - 2048) / 2048.0; + distanceErr = distance - (int)(setDistance * 1345); + + if (nunchuk_connected == 0) { + speedL = speedL * 0.8f + (CLAMP(distanceErr + (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); + speedR = speedR * 0.8f + (CLAMP(distanceErr - (steering*((float)MAX(ABS(distanceErr), 50)) * ROT_P), -850, 850) * -0.2f); + if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50)) { + if (distanceErr > 0) { + enable = 1; + } + if (distanceErr > -300) { + #ifdef INVERT_R_DIRECTION + pwmr = speedR; + #else + pwmr = -speedR; + #endif + #ifdef INVERT_L_DIRECTION + pwml = -speedL; + #else + pwml = speedL; + #endif + + if (checkRemote) { + if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) { + //enable = 1; + } else { + enable = 0; + } + } + } else { + enable = 0; + } + } + timeout = 0; + } + if (timeout > TIMEOUT) { pwml = 0; pwmr = 0; enable = 0; #ifdef SUPPORT_LCD - LCD_SetLocation(&lcd, 0, 0); - LCD_WriteString(&lcd, "Len:"); - LCD_SetLocation(&lcd, 8, 0); - LCD_WriteString(&lcd, "m("); - LCD_SetLocation(&lcd, 14, 0); - LCD_WriteString(&lcd, "m)"); + LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Len:"); + LCD_SetLocation(&lcd, 8, 0); LCD_WriteString(&lcd, "m("); + LCD_SetLocation(&lcd, 14, 0); LCD_WriteString(&lcd, "m)"); #endif - HAL_Delay(1000); - nunchuk_connected = 0; } @@ -712,23 +319,20 @@ int main(void) { #ifdef SUPPORT_LCD LCD_ClearDisplay(&lcd); HAL_Delay(5); - LCD_SetLocation(&lcd, 0, 0); - LCD_WriteString(&lcd, "Emergency Off!"); - LCD_SetLocation(&lcd, 0, 1); - LCD_WriteString(&lcd, "Keeper too fast."); + LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Emergency Off!"); + LCD_SetLocation(&lcd, 0, 1); LCD_WriteString(&lcd, "Keeper too fast."); #endif poweroff(); } #ifdef SUPPORT_NUNCHUK - if (counter % 500 == 0) { + if (transpotter_counter % 500 == 0) { if (nunchuk_connected == 0 && enable == 0) { if (Nunchuk_Ping()) { HAL_Delay(500); Nunchuk_Init(); #ifdef SUPPORT_LCD - LCD_SetLocation(&lcd, 0, 0); - LCD_WriteString(&lcd, "Nunchuk Control"); + LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Nunchuk Control"); #endif timeout = 0; HAL_Delay(1000); @@ -739,40 +343,43 @@ int main(void) { #endif #ifdef SUPPORT_LCD - if (counter % 100 == 0) { + if (transpotter_counter % 100 == 0) { if (LCDerrorFlag == 1 && enable == 0) { } else { if (nunchuk_connected == 0) { - LCD_SetLocation(&lcd, 4, 0); - LCD_WriteFloat(&lcd,distance/1345.0,2); - LCD_SetLocation(&lcd, 10, 0); - LCD_WriteFloat(&lcd,setDistance,2); + LCD_SetLocation(&lcd, 4, 0); LCD_WriteFloat(&lcd,distance/1345.0,2); + LCD_SetLocation(&lcd, 10, 0); LCD_WriteFloat(&lcd,setDistance,2); } - LCD_SetLocation(&lcd, 4, 1); - LCD_WriteFloat(&lcd,batVoltage, 1); - LCD_SetLocation(&lcd, 11, 1); - //LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2); + LCD_SetLocation(&lcd, 4, 1); LCD_WriteFloat(&lcd,batVoltage, 1); + // LCD_SetLocation(&lcd, 11, 1); LCD_WriteFloat(&lcd,MAX(ABS(currentR), ABS(currentL)),2); } } #endif - - counter++; + transpotter_counter++; #endif + // ####### SIDEBOARDS HANDLING ####### + #if defined(SIDEBOARD_SERIAL_USART2) + sideboardLeds(&sideboard_leds_L); + sideboardSensors((uint8_t)Sideboard_L.sensors); + #endif + #if defined(SIDEBOARD_SERIAL_USART3) + sideboardLeds(&sideboard_leds_R); + sideboardSensors((uint8_t)Sideboard_R.sensors); + #endif // ####### CALC BOARD TEMPERATURE ####### filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt); - board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // convert fixed-point to integer + board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 16); // 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; - if (main_loop_counter % 25 == 0) { // Send data periodically - - // ####### DEBUG SERIAL OUT ####### - #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + // ####### DEBUG SERIAL OUT ####### + #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) + if (main_loop_counter % 25 == 0) { // Send data periodically every 125 ms #ifdef CONTROL_ADC - setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 - setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 + setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1 + setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2 #endif setScopeChannel(2, (int16_t)speedR); // 3: output command: [-1000, 1000] setScopeChannel(3, (int16_t)speedL); // 4: output command: [-1000, 1000] @@ -781,68 +388,75 @@ int main(void) { 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(); - - // ####### FEEDBACK SERIAL OUT ####### - #elif defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) - if(UART_DMA_CHANNEL->CNDTR == 0) { - Feedback.start = (uint16_t)START_FRAME; - Feedback.cmd1 = (int16_t)cmd1; - Feedback.cmd2 = (int16_t)cmd2; - Feedback.speedR = (int16_t)speedR; - Feedback.speedL = (int16_t)speedL; - Feedback.speedR_meas = (int16_t)rtY_Left.n_mot; - Feedback.speedL_meas = (int16_t)rtY_Right.n_mot; - Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); - Feedback.boardTemp = (int16_t)board_temp_deg_c; - Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL - ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp); - - UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; - UART_DMA_CHANNEL->CNDTR = sizeof(Feedback); - UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback; - UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; - } - #endif - } - - HAL_GPIO_TogglePin(LED_PORT, LED_PIN); - // ####### POWEROFF BY POWER-BUTTON ####### - if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; // disable motors - while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released - if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) - __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags - } else { - poweroff(); // release power-latch } - } + #endif + // ####### FEEDBACK SERIAL OUT ####### + #if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3) + if (main_loop_counter % 2 == 0) { // Send data periodically every 10 ms + Feedback.start = (uint16_t)SERIAL_START_FRAME; + Feedback.cmd1 = (int16_t)cmd1; + Feedback.cmd2 = (int16_t)cmd2; + Feedback.speedR_meas = (int16_t)rtY_Right.n_mot; + Feedback.speedL_meas = (int16_t)rtY_Left.n_mot; + Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); + Feedback.boardTemp = (int16_t)board_temp_deg_c; + + #if defined(FEEDBACK_SERIAL_USART2) + if(DMA1_Channel7->CNDTR == 0) { + Feedback.cmdLed = (uint16_t)sideboard_leds_L; + Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas + ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); + DMA1_Channel7->CCR &= ~DMA_CCR_EN; + DMA1_Channel7->CNDTR = sizeof(Feedback); + DMA1_Channel7->CMAR = (uint32_t)&Feedback; + DMA1_Channel7->CCR |= DMA_CCR_EN; + } + #endif + #if defined(FEEDBACK_SERIAL_USART3) + if(DMA1_Channel2->CNDTR == 0) { + Feedback.cmdLed = (uint16_t)sideboard_leds_R; + Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas + ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); + DMA1_Channel2->CCR &= ~DMA_CCR_EN; + DMA1_Channel2->CNDTR = sizeof(Feedback); + DMA1_Channel2->CMAR = (uint32_t)&Feedback; + DMA1_Channel2->CCR |= DMA_CCR_EN; + } + #endif + } + #endif + + // ####### POWEROFF BY POWER-BUTTON ####### + poweroffPressCheck(); // ####### BEEP AND EMERGENCY POWEROFF ####### - if (errCode_Left || errCode_Right) { // disable motors and beep in case of Motor error - fast beep + if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3 + poweroff(); + } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) { // disable motors and beep in case of Motor error - fast beep enable = 0; buzzerFreq = 8; buzzerPattern = 1; - } else if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_LOW_DEAD && speedAvgAbs < 20)) { // poweroff before mainboard burns OR low bat 3 - poweroff(); + } else if (timeoutFlagADC || timeoutFlagSerial) { // beep in case of ADC or Serial timeout - fast beep + buzzerFreq = 24; + buzzerPattern = 1; } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) { // beep if mainboard gets hot buzzerFreq = 4; buzzerPattern = 1; - } 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 (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep + } else if (BAT_LVL1_ENABLE && batVoltage < BAT_LVL1) { // low bat 1: fast beep buzzerFreq = 5; buzzerPattern = 6; - } else if (timeoutFlagADC || timeoutFlagSerial) { // beep in case of ADC or Serial timeout - fast beep - buzzerFreq = 24; - buzzerPattern = 1; + } else if (BAT_LVL2_ENABLE && batVoltage < BAT_LVL2) { // low bat 2: slow beep + buzzerFreq = 5; + buzzerPattern = 42; } else if (BEEPS_BACKWARD && ((speed < -50 && speedAvg < 0) || MultipleTapBreak.b_multipleTap)) { // backward beep buzzerFreq = 5; buzzerPattern = 1; + backwardDrive = 1; } else { // do not beep buzzerFreq = 0; buzzerPattern = 0; + backwardDrive = 0; } @@ -856,169 +470,17 @@ int main(void) { poweroff(); } + // HAL_GPIO_TogglePin(LED_PORT, LED_PIN); // This is to measure the main() loop duration with an oscilloscope connected to LED_PIN + // Update main loop states + lastSpeedL = speedL; + lastSpeedR = speedR; main_loop_counter++; timeout++; } } -#ifdef VARIANT_TRANSPOTTER - void saveConfig() { - HAL_FLASH_Unlock(); - EE_WriteVariable(VirtAddVarTab[0], saveValue); - HAL_FLASH_Lock(); - } -#endif - -void longBeep(uint8_t freq){ - buzzerFreq = freq; - HAL_Delay(500); - buzzerFreq = 0; -} - -void shortBeep(uint8_t freq){ - buzzerFreq = freq; - HAL_Delay(100); - buzzerFreq = 0; -} // =========================================================== - /* Low pass filter fixed-point 32 bits: fixdt(1,32,20) - * Max: 2047.9375 - * Min: -2048 - * Res: 0.0625 - * - * Inputs: u = int16 - * Outputs: y = fixdt(1,32,20) - * 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) - * filtLowPass16(u, 52429, &y); - * yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits - */ -void filtLowPass32(int16_t u, uint16_t coef, int32_t *y) -{ - int32_t tmp; - - tmp = (int16_t)(u << 4) - (*y >> 16); - tmp = CLAMP(tmp, -32768, 32767); // Overflow protection - *y = coef * tmp + (*y); -} - -// =========================================================== - /* 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,14) - */ -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, INPUT_MIN, INPUT_MAX); - - 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, INPUT_MIN, INPUT_MAX); -} - -// =========================================================== - /* rateLimiter16(int16_t u, int16_t rate, int16_t *y); - * Inputs: u = int16 - * Outputs: y = fixdt(1,16,4) - * Parameters: rate = fixdt(1,16,4) = [0, 32767] Do NOT make rate negative (>32767) - */ -void rateLimiter16(int16_t u, int16_t rate, int16_t *y) -{ - int16_t q0; - int16_t q1; - - q0 = (u << 4) - *y; - - if (q0 > rate) { - q0 = rate; - } else { - q1 = -rate; - if (q0 < q1) { - q0 = q1; - } - } - - *y = q0 + *y; -} - -// =========================================================== - /* multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) - * This function detects multiple tap presses, such as double tapping, triple tapping, etc. - * Inputs: u = int16_t (input signal); timeNow = uint32_t (current time) - * Outputs: x->b_multipleTap (get the output here) - */ -void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) -{ - uint8_t b_timeout; - uint8_t b_hyst; - uint8_t b_pulse; - uint8_t z_pulseCnt; - uint8_t z_pulseCntRst; - uint32_t t_time; - - // Detect hysteresis - if (x->b_hysteresis) { - b_hyst = (u > MULTIPLE_TAP_LO); - } else { - b_hyst = (u > MULTIPLE_TAP_HI); - } - - // Detect pulse - b_pulse = (b_hyst != x->b_hysteresis); - - // Save time when first pulse is detected - if (b_hyst && b_pulse && (x->z_pulseCntPrev == 0)) { - t_time = timeNow; - } else { - t_time = x->t_timePrev; - } - - // Create timeout boolean - b_timeout = (timeNow - t_time > MULTIPLE_TAP_TIMEOUT); - - // Create pulse counter - if ((!b_hyst) && (x->z_pulseCntPrev == 0)) { - z_pulseCnt = 0U; - } else { - z_pulseCnt = b_pulse; - } - - // Reset counter if we detected complete tap presses OR there is a timeout - if ((x->z_pulseCntPrev >= MULTIPLE_TAP_NR) || b_timeout) { - z_pulseCntRst = 0U; - } else { - z_pulseCntRst = x->z_pulseCntPrev; - } - z_pulseCnt = z_pulseCnt + z_pulseCntRst; - - // Check if complete tap presses are detected AND no timeout - if ((z_pulseCnt >= MULTIPLE_TAP_NR) && (!b_timeout)) { - x->b_multipleTap = !x->b_multipleTap; // Toggle output - } - - // Update states - x->z_pulseCntPrev = z_pulseCnt; - x->b_hysteresis = b_hyst; - x->t_timePrev = t_time; -} - -// =========================================================== - /** System Clock Configuration */ void SystemClock_Config(void) { diff --git a/Src/setup.c b/Src/setup.c index 2072762..c96d0a8 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -53,11 +53,12 @@ DMA_HandleTypeDef hdma_usart3_tx; volatile adc_buf_t adc_buffer; -#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) +#if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || \ + defined(FEEDBACK_SERIAL_USAR2) || defined(DEBUG_SERIAL_USART2) void UART2_Init(void) { /* The code below is commented out - otwerwise Serial Receive does not work */ - // #ifdef CONTROL_SERIAL_USART2 + // #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) // /* DMA1_Channel6_IRQn interrupt configuration */ // HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 6); // HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn); @@ -81,7 +82,7 @@ void UART2_Init(void) { huart2.Init.Parity = UART_PARITY_NONE; huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart2.Init.OverSampling = UART_OVERSAMPLING_16; - #if defined(CONTROL_SERIAL_USART2) + #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) huart2.Init.Mode = UART_MODE_TX_RX; #elif defined(DEBUG_SERIAL_USART2) huart2.Init.Mode = UART_MODE_TX; @@ -99,7 +100,7 @@ void UART2_Init(void) { GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - #ifdef CONTROL_SERIAL_USART2 + #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) GPIO_InitStruct.Pin = GPIO_PIN_3; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; //GPIO_MODE_AF_PP; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -127,7 +128,7 @@ void UART2_Init(void) { hdma_usart2_tx.Init.Priority = DMA_PRIORITY_LOW; HAL_DMA_Init(&hdma_usart2_tx); - #ifdef CONTROL_SERIAL_USART2 + #if defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) __HAL_LINKDMA(&huart2, hdmatx, hdma_usart2_tx); #endif #if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) @@ -139,11 +140,12 @@ void UART2_Init(void) { } #endif -#if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) +#if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) || \ + defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) void UART3_Init(void) { /* The code below is commented out - otwerwise Serial Receive does not work */ - // #ifdef CONTROL_SERIAL_USART3 + // #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) // /* DMA1_Channel3_IRQn interrupt configuration */ // HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 3); // HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn); @@ -167,7 +169,7 @@ void UART3_Init(void) { huart3.Init.Parity = UART_PARITY_NONE; huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE; huart3.Init.OverSampling = UART_OVERSAMPLING_16; - #if defined(CONTROL_SERIAL_USART3) + #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) huart3.Init.Mode = UART_MODE_TX_RX; #elif defined(DEBUG_SERIAL_USART3) huart3.Init.Mode = UART_MODE_TX; @@ -185,7 +187,7 @@ void UART3_Init(void) { GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - #ifdef CONTROL_SERIAL_USART3 + #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) GPIO_InitStruct.Pin = GPIO_PIN_11; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); @@ -213,7 +215,7 @@ void UART3_Init(void) { hdma_usart3_tx.Init.Priority = DMA_PRIORITY_LOW; HAL_DMA_Init(&hdma_usart3_tx); - #ifdef CONTROL_SERIAL_USART3 + #if defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) __HAL_LINKDMA(&huart3, hdmatx, hdma_usart3_tx); #endif #if defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) diff --git a/Src/util.c b/Src/util.c new file mode 100644 index 0000000..2db082c --- /dev/null +++ b/Src/util.c @@ -0,0 +1,1109 @@ +/** + * This file is part of the hoverboard-firmware-hack project. + * + * Copyright (C) 2020-2021 Emanuel FERU + * + * 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 . +*/ + +// Includes +#include // for abs() +#include +#include "stm32f1xx_hal.h" +#include "defines.h" +#include "setup.h" +#include "config.h" +#include "comms.h" +#include "eeprom.h" +#include "util.h" +#include "BLDC_controller.h" +#include "rtwtypes.h" + +#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) +#include "hd44780.h" +#endif + +/* =========================== Variable Definitions =========================== */ + +//------------------------------------------------------------------------ +// Global variables set externally +//------------------------------------------------------------------------ +extern volatile adc_buf_t adc_buffer; +extern I2C_HandleTypeDef hi2c2; +#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) \ + || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) + extern UART_HandleTypeDef huart2; + extern UART_HandleTypeDef huart3; + static UART_HandleTypeDef huart; +#endif + +extern int16_t batVoltage; +extern uint8_t backwardDrive; +extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7... +extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7... + +extern uint8_t enable; // global variable for motor enable + +extern uint8_t nunchuk_data[6]; +extern volatile uint32_t timeout; // global variable for timeout +extern volatile uint32_t main_loop_counter; + +#ifdef CONTROL_PPM +extern volatile uint16_t ppm_captured_value[PPM_NUM_CHANNELS+1]; +#endif + + +//------------------------------------------------------------------------ +// Global variables set here in util.c +//------------------------------------------------------------------------ +// Matlab defines - from auto-code generation +//--------------- +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_; + +extern P rtP_Left; /* Block parameters (auto storage) */ +DW rtDW_Left; /* Observable states */ +ExtU rtU_Left; /* External inputs */ +ExtY rtY_Left; /* External outputs */ + +P rtP_Right; /* Block parameters (auto storage) */ +DW rtDW_Right; /* Observable states */ +ExtU rtU_Right; /* External inputs */ +ExtY rtY_Right; /* External outputs */ +//--------------- + +int16_t cmd1; // normalized input value. -1000 to 1000 +int16_t cmd2; // normalized input value. -1000 to 1000 + +int16_t speedAvg; // average measured speed +int16_t speedAvgAbs; // average measured speed in absolute +uint8_t timeoutFlagADC = 0; // Timeout Flag for ADC Protection: 0 = OK, 1 = Problem detected (line disconnected or wrong ADC data) +uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data) + +uint8_t ctrlModReqRaw = CTRL_MOD_REQ; +uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request + +#if defined(SIDEBOARD_SERIAL_USART2) +SerialSideboard Sideboard_L; +static SerialSideboard Sideboard_Lnew; +static uint8_t timeoutFlagSerial_L = 0; +static int16_t timeoutCntSerial_L = 0; +#endif +#if defined(SIDEBOARD_SERIAL_USART3) +SerialSideboard Sideboard_R; +static SerialSideboard Sideboard_Rnew; +static uint8_t timeoutFlagSerial_R = 0; +static int16_t timeoutCntSerial_R = 0; +#endif +#if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) +static uint8_t sensor1_prev; // holds the previous sensor1 state +static uint8_t sensor2_prev; // holds the previous sensor2 state +static uint8_t sensor1_index; // holds the press index number for sensor1 +static uint8_t sensor2_index; // holds the press index number for sensor2 +#endif + +#if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) +LCD_PCF8574_HandleTypeDef lcd; +#endif + +#if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) +uint8_t nunchuk_connected = 1; +#else +uint8_t nunchuk_connected = 0; +#endif + +#ifdef VARIANT_TRANSPOTTER +float setDistance; +uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1337}; // Virtual address defined by the user: 0xFFFF value is prohibited +static uint16_t saveValue = 0; +static uint8_t saveValue_valid = 0; +#elif defined(CONTROL_ADC) +uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1308}; +#else +uint16_t VirtAddVarTab[NB_OF_VAR] = {0x1300}; // Dummy virtual address to avoid warnings +#endif + + +//------------------------------------------------------------------------ +// Local variables +//------------------------------------------------------------------------ +static int16_t INPUT_MAX; // [-] Input target maximum limitation +static int16_t INPUT_MIN; // [-] Input target minimum limitation +static int16_t INPUT_MID; // [-] Input target middle + +#if defined(CONTROL_ADC) && defined(ADC_PROTECT_ENA) +static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection +#endif + +#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) +static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command +static volatile Serialcommand command; + #ifdef CONTROL_IBUS + static uint16_t ibus_chksum; + static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS]; + #endif +#endif + +#ifdef SUPPORT_BUTTONS +static uint8_t button1, button2; +#endif + +#ifdef CONTROL_ADC + static uint8_t cur_spd_valid = 0; + static uint8_t adc_cal_valid = 0; + static uint16_t ADC1_MIN_CAL = ADC1_MIN; + static uint16_t ADC1_MAX_CAL = ADC1_MAX; + static uint16_t ADC2_MIN_CAL = ADC2_MIN; + static uint16_t ADC2_MAX_CAL = ADC2_MAX; + #ifdef ADC1_MID_POT + static uint16_t ADC1_MID_CAL = ADC1_MID; + #else + static uint16_t ADC1_MID_CAL = 0; + #endif + #ifdef ADC1_MID_POT + static uint16_t ADC2_MID_CAL = ADC2_MID; + #else + static uint16_t ADC2_MID_CAL = 0; + #endif +#endif + +#ifdef VARIANT_HOVERCAR + static uint8_t brakePressed; +#endif + + +/* =========================== Initialization Functions =========================== */ + +void BLDC_Init(void) { + /* Set BLDC controller parameters */ + rtP_Left.b_selPhaABCurrMeas = 1; // Left motor measured current phases {Green, Blue} = {iA, iB} -> do NOT change + rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL; + rtP_Left.b_diagEna = DIAG_ENA; + rtP_Left.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) + rtP_Left.n_max = N_MOT_MAX << 4; // fixdt(1,16,4) + rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA; + rtP_Left.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4) + rtP_Left.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4) + rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) + rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) + + rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters + rtP_Right.b_selPhaABCurrMeas = 0; // Right motor measured current phases {Blue, Yellow} = {iB, iC} -> do NOT change + + /* Pack LEFT motor data into RTM */ + rtM_Left->defaultParam = &rtP_Left; + rtM_Left->dwork = &rtDW_Left; + rtM_Left->inputs = &rtU_Left; + rtM_Left->outputs = &rtY_Left; + + /* Pack RIGHT motor data into RTM */ + rtM_Right->defaultParam = &rtP_Right; + rtM_Right->dwork = &rtDW_Right; + rtM_Right->inputs = &rtU_Right; + rtM_Right->outputs = &rtY_Right; + + /* Initialize BLDC controllers */ + BLDC_controller_initialize(rtM_Left); + BLDC_controller_initialize(rtM_Right); +} + +void Input_Lim_Init(void) { // Input Limitations - ! Do NOT touch ! + if (rtP_Left.b_fieldWeakEna || rtP_Right.b_fieldWeakEna) { + INPUT_MAX = MAX( 1000, FIELD_WEAK_HI); + INPUT_MIN = MIN(-1000,-FIELD_WEAK_HI); + } else { + INPUT_MAX = 1000; + INPUT_MIN = -1000; + } + INPUT_MID = INPUT_MAX / 2; +} + +void Input_Init(void) { + #ifdef CONTROL_PPM + PPM_Init(); + #endif + + #ifdef CONTROL_NUNCHUK + I2C_Init(); + Nunchuk_Init(); + #endif + + #if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) + UART2_Init(); + huart = huart2; + #endif + #if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART2) + UART3_Init(); + huart = huart3; + #endif + #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) + HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); + #endif + #if defined(SIDEBOARD_SERIAL_USART2) + HAL_UART_Receive_DMA(&huart2, (uint8_t *)&Sideboard_Lnew, sizeof(Sideboard_Lnew)); + #endif + #if defined(SIDEBOARD_SERIAL_USART3) + HAL_UART_Receive_DMA(&huart3, (uint8_t *)&Sideboard_Rnew, sizeof(Sideboard_Rnew)); + #endif + + #ifdef CONTROL_ADC + + uint16_t writeCheck, i_max, n_max; + HAL_FLASH_Unlock(); + EE_Init(); /* EEPROM Init */ + EE_ReadVariable(VirtAddVarTab[0], &writeCheck); + if (writeCheck == FLASH_WRITE_KEY) { + EE_ReadVariable(VirtAddVarTab[1], &ADC1_MIN_CAL); + EE_ReadVariable(VirtAddVarTab[2], &ADC1_MAX_CAL); + EE_ReadVariable(VirtAddVarTab[3], &ADC1_MID_CAL); + EE_ReadVariable(VirtAddVarTab[4], &ADC2_MIN_CAL); + EE_ReadVariable(VirtAddVarTab[5], &ADC2_MAX_CAL); + EE_ReadVariable(VirtAddVarTab[6], &ADC2_MID_CAL); + EE_ReadVariable(VirtAddVarTab[7], &i_max); + EE_ReadVariable(VirtAddVarTab[8], &n_max); + rtP_Left.i_max = i_max; + rtP_Left.n_max = n_max; + rtP_Right.i_max = i_max; + rtP_Right.n_max = n_max; + } + HAL_FLASH_Lock(); + + #endif + + #ifdef VARIANT_TRANSPOTTER + enable = 1; + + HAL_FLASH_Unlock(); + EE_Init(); /* EEPROM Init */ + EE_ReadVariable(VirtAddVarTab[0], &saveValue); + HAL_FLASH_Lock(); + + setDistance = saveValue / 1000.0; + if (setDistance < 0.2) { + setDistance = 1.0; + } + #endif + + #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) + I2C_Init(); + HAL_Delay(50); + lcd.pcf8574.PCF_I2C_ADDRESS = 0x27; + lcd.pcf8574.PCF_I2C_TIMEOUT = 5; + lcd.pcf8574.i2c = hi2c2; + lcd.NUMBER_OF_LINES = NUMBER_OF_LINES_2; + lcd.type = TYPE0; + + if(LCD_Init(&lcd)!=LCD_OK) { + // error occured + //TODO while(1); + } + + LCD_ClearDisplay(&lcd); + HAL_Delay(5); + LCD_SetLocation(&lcd, 0, 0); + #ifdef VARIANT_TRANSPOTTER + LCD_WriteString(&lcd, "TranspOtter V2.1"); + #else + LCD_WriteString(&lcd, "Hover V2.0"); + #endif + LCD_SetLocation(&lcd, 0, 1); LCD_WriteString(&lcd, "Initializing..."); + #endif + + #if defined(VARIANT_TRANSPOTTER) && defined(SUPPORT_LCD) + LCD_ClearDisplay(&lcd); + HAL_Delay(5); + LCD_SetLocation(&lcd, 0, 1); LCD_WriteString(&lcd, "Bat:"); + LCD_SetLocation(&lcd, 8, 1); LCD_WriteString(&lcd, "V"); + LCD_SetLocation(&lcd, 15, 1); LCD_WriteString(&lcd, "A"); + LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Len:"); + LCD_SetLocation(&lcd, 8, 0); LCD_WriteString(&lcd, "m("); + LCD_SetLocation(&lcd, 14, 0); LCD_WriteString(&lcd, "m)"); + #endif +} + + + +/* =========================== General Functions =========================== */ + +void poweronMelody(void) { + for (int i = 8; i >= 0; i--) { + buzzerFreq = (uint8_t)i; + HAL_Delay(100); + } + buzzerFreq = 0; +} + +void shortBeep(uint8_t freq) { + buzzerFreq = freq; + HAL_Delay(100); + buzzerFreq = 0; +} + +void shortBeepMany(uint8_t cnt) { + for(uint8_t i = 0; i < cnt; i++) { + shortBeep(i + 5); + } +} + +void longBeep(uint8_t freq) { + buzzerFreq = freq; + HAL_Delay(500); + buzzerFreq = 0; +} + +void calcAvgSpeed(void) { + // Calculate measured average speed. The minus sign (-) is because motors spin in opposite directions + #if !defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) + speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2; + #elif !defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) + speedAvg = ( rtY_Left.n_mot + rtY_Right.n_mot) / 2; + #elif defined(INVERT_L_DIRECTION) && !defined(INVERT_R_DIRECTION) + speedAvg = (-rtY_Left.n_mot - rtY_Right.n_mot) / 2; + #elif defined(INVERT_L_DIRECTION) && defined(INVERT_R_DIRECTION) + speedAvg = (-rtY_Left.n_mot + rtY_Right.n_mot) / 2; + #endif + + // Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1) + if (SPEED_COEFFICIENT & (1 << 16)) { + speedAvg = -speedAvg; + } + speedAvgAbs = abs(speedAvg); +} + + /* + * Auto-calibration of the ADC Limits + * This function finds the Minimum, Maximum, and Middle for the ADC input + * Procedure: + * - press the power button for more than 5 sec and release after the beep sound + * - move the potentiometers freely to the min and max limits repeatedly + * - release potentiometers to the resting postion + * - press the power button to confirm or wait for the 20 sec timeout + */ +void adcCalibLim(void) { + if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning + return; + } + #ifdef CONTROL_ADC + consoleLog("ADC calibration started... "); + + // Inititalization: MIN = a high values, MAX = a low value, + int32_t adc1_fixdt = adc_buffer.l_tx2 << 16; + int32_t adc2_fixdt = adc_buffer.l_rx2 << 16; + uint16_t adc_cal_timeout = 0; + uint16_t ADC1_MIN_temp = 4095; + uint16_t ADC1_MID_temp = 0; + uint16_t ADC1_MAX_temp = 0; + uint16_t ADC2_MIN_temp = 4095; + uint16_t ADC2_MID_temp = 0; + uint16_t ADC2_MAX_temp = 0; + + adc_cal_valid = 1; + + // Extract MIN, MAX and MID from ADC while the power button is not pressed + while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && adc_cal_timeout < 4000) { // 20 sec timeout + filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); + filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); + ADC1_MID_temp = (uint16_t)CLAMP(adc1_fixdt >> 16, 0, 4095); // convert fixed-point to integer + ADC2_MID_temp = (uint16_t)CLAMP(adc2_fixdt >> 16, 0, 4095); + ADC1_MIN_temp = MIN(ADC1_MIN_temp, ADC1_MID_temp); + ADC1_MAX_temp = MAX(ADC1_MAX_temp, ADC1_MID_temp); + ADC2_MIN_temp = MIN(ADC2_MIN_temp, ADC2_MID_temp); + ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); + adc_cal_timeout++; + HAL_Delay(5); + } + + // ADC calibration checks + #ifdef ADC_PROTECT_ENA + if ((ADC1_MIN_temp + 150 - ADC_PROTECT_THRESH) > 0 && (ADC1_MAX_temp - 150 + ADC_PROTECT_THRESH) < 4095 && + (ADC2_MIN_temp + 150 - ADC_PROTECT_THRESH) > 0 && (ADC2_MAX_temp - 150 + ADC_PROTECT_THRESH) < 4095) { + adc_cal_valid = 1; + } else { + adc_cal_valid = 0; + consoleLog("FAIL (ADC out-of-range protection not possible)\n"); + } + #endif + + // Add final ADC margin to have exact 0 and MAX at the minimum and maximum ADC value + if (adc_cal_valid && (ADC1_MAX_temp - ADC1_MIN_temp) > 500 && (ADC2_MAX_temp - ADC2_MIN_temp) > 500) { + ADC1_MIN_CAL = ADC1_MIN_temp + 150; + ADC1_MID_CAL = ADC1_MID_temp; + ADC1_MAX_CAL = ADC1_MAX_temp - 150; + ADC2_MIN_CAL = ADC2_MIN_temp + 150; + ADC2_MID_CAL = ADC2_MID_temp; + ADC2_MAX_CAL = ADC2_MAX_temp - 150; + consoleLog("OK\n"); + } else { + adc_cal_valid = 0; + consoleLog("FAIL (Pots travel too short)\n"); + } + + #endif +} + + + /* + * Update Maximum Motor Current Limit (via ADC1) and Maximum Speed Limit (via ADC2) + * Procedure: + * - press the power button for more than 5 sec and immediatelly after the beep sound press one more time shortly + * - move and hold the pots to a desired limit position for Current and Speed + * - press the power button to confirm or wait for the 10 sec timeout + */ +void updateCurSpdLim(void) { + if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning + return; + } + #ifdef CONTROL_ADC + consoleLog("Torque and Speed limits update started... "); + + int32_t adc1_fixdt = adc_buffer.l_tx2 << 16; + int32_t adc2_fixdt = adc_buffer.l_rx2 << 16; + uint16_t cur_spd_timeout = 0; + uint16_t cur_factor; // fixdt(0,16,16) + uint16_t spd_factor; // fixdt(0,16,16) + + // Wait for the power button press + while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout < 2000) { // 10 sec timeout + filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); + filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); + cur_spd_timeout++; + HAL_Delay(5); + } + + // Calculate scaling factors + cur_factor = CLAMP((adc1_fixdt - (ADC1_MIN_CAL << 16)) / (ADC1_MAX_CAL - ADC1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A + spd_factor = CLAMP((adc2_fixdt - (ADC2_MIN_CAL << 16)) / (ADC2_MAX_CAL - ADC2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm + + // Update maximum limits + rtP_Left.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) + rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4) + rtP_Right.i_max = rtP_Left.i_max; + rtP_Right.n_max = rtP_Left.n_max; + + cur_spd_valid = 1; + consoleLog("OK\n"); + + #endif +} + + /* + * Save Configuration to Flash + * This function makes sure data is not lost after power-off + */ +void saveConfig() { + #ifdef VARIANT_TRANSPOTTER + if (saveValue_valid) { + HAL_FLASH_Unlock(); + EE_WriteVariable(VirtAddVarTab[0], saveValue); + HAL_FLASH_Lock(); + } + #endif + #ifdef CONTROL_ADC + if (adc_cal_valid || cur_spd_valid) { + HAL_FLASH_Unlock(); + EE_WriteVariable(VirtAddVarTab[0], FLASH_WRITE_KEY); + EE_WriteVariable(VirtAddVarTab[1], ADC1_MIN_CAL); + EE_WriteVariable(VirtAddVarTab[2], ADC1_MAX_CAL); + EE_WriteVariable(VirtAddVarTab[3], ADC1_MID_CAL); + EE_WriteVariable(VirtAddVarTab[4], ADC2_MIN_CAL); + EE_WriteVariable(VirtAddVarTab[5], ADC2_MAX_CAL); + EE_WriteVariable(VirtAddVarTab[6], ADC2_MID_CAL); + EE_WriteVariable(VirtAddVarTab[7], rtP_Left.i_max); + EE_WriteVariable(VirtAddVarTab[8], rtP_Left.n_max); + HAL_FLASH_Lock(); + } + #endif +} + + + +/* =========================== Poweroff Functions =========================== */ + +void poweroff(void) { + buzzerPattern = 0; + enable = 0; + consoleLog("-- Motors disabled --\r\n"); + for (int i = 0; i < 8; i++) { + buzzerFreq = (uint8_t)i; + HAL_Delay(100); + } + saveConfig(); + HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); + while(1) {} +} + + +void poweroffPressCheck(void) { + #if defined(CONTROL_ADC) + if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; + uint16_t cnt_press = 0; + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + HAL_Delay(10); + if (cnt_press++ == 5 * 100) { shortBeep(5); } + } + if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec + HAL_Delay(300); + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { // Double press: Adjust Max Current, Max Speed + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + longBeep(8); + updateCurSpdLim(); + shortBeep(5); + } else { // Long press: Calibrate ADC Limits + longBeep(16); + adcCalibLim(); + shortBeep(5); + } + } else { // Short press: power off + poweroff(); + } + } + #elif defined(VARIANT_TRANSPOTTER) + if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + shortBeep(5); + HAL_Delay(300); + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } + longBeep(5); + HAL_Delay(350); + poweroff(); + } else { + setDistance += 0.25; + if (setDistance > 2.6) { + setDistance = 0.5; + } + shortBeep(setDistance / 0.25); + saveValue = setDistance * 1000; + saveValue_valid = 1; + } + } + #else + if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { + enable = 0; // disable motors + while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released + if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger) + __HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags + } else { + poweroff(); // release power-latch + } + } + #endif +} + + + +/* =========================== Read Command Function =========================== */ + +void readCommand(void) { + + #if defined(CONTROL_NUNCHUK) || defined(SUPPORT_NUNCHUK) + if (nunchuk_connected != 0) { + Nunchuk_Read(); + cmd1 = CLAMP((nunchuk_data[0] - 127) * 8, INPUT_MIN, INPUT_MAX); // x - axis. Nunchuk joystick readings range 30 - 230 + cmd2 = CLAMP((nunchuk_data[1] - 128) * 8, INPUT_MIN, INPUT_MAX); // y - axis + + #ifdef SUPPORT_BUTTONS + button1 = (uint8_t)nunchuk_data[5] & 1; + button2 = (uint8_t)(nunchuk_data[5] >> 1) & 1; + #endif + } + #endif + + #ifdef CONTROL_PPM + cmd1 = CLAMP((ppm_captured_value[0] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP((ppm_captured_value[1] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); + #ifdef SUPPORT_BUTTONS + button1 = ppm_captured_value[5] > INPUT_MID; + button2 = 0; + #endif + // float scale = ppm_captured_value[2] / 1000.0f; // not used for now, uncomment if needed + #endif + + #ifdef CONTROL_ADC + // ADC values range: 0-4095, see ADC-calibration in config.h + #ifdef ADC1_MID_POT + cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MID_CAL) * INPUT_MAX / (ADC1_MAX_CAL - ADC1_MID_CAL), 0, INPUT_MAX) + -CLAMP((ADC1_MID_CAL - adc_buffer.l_tx2) * INPUT_MAX / (ADC1_MID_CAL - ADC1_MIN_CAL), 0, INPUT_MAX); // ADC1 + #else + cmd1 = CLAMP((adc_buffer.l_tx2 - ADC1_MIN_CAL) * INPUT_MAX / (ADC1_MAX_CAL - ADC1_MIN_CAL), 0, INPUT_MAX); // ADC1 + #endif + + #ifdef ADC2_MID_POT + cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MID_CAL) * INPUT_MAX / (ADC2_MAX_CAL - ADC2_MID_CAL), 0, INPUT_MAX) + -CLAMP((ADC2_MID_CAL - adc_buffer.l_rx2) * INPUT_MAX / (ADC2_MID_CAL - ADC2_MIN_CAL), 0, INPUT_MAX); // ADC2 + #else + cmd2 = CLAMP((adc_buffer.l_rx2 - ADC2_MIN_CAL) * INPUT_MAX / (ADC2_MAX_CAL - ADC2_MIN_CAL), 0, INPUT_MAX); // ADC2 + #endif + + #ifdef ADC_PROTECT_ENA + if (adc_buffer.l_tx2 >= (ADC1_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_tx2 <= (ADC1_MAX_CAL + ADC_PROTECT_THRESH) && + adc_buffer.l_rx2 >= (ADC2_MIN_CAL - ADC_PROTECT_THRESH) && adc_buffer.l_rx2 <= (ADC2_MAX_CAL + ADC_PROTECT_THRESH)) { + if (timeoutFlagADC) { // Check for previous timeout flag + if (timeoutCntADC-- <= 0) // Timeout de-qualification + timeoutFlagADC = 0; // Timeout flag cleared + } else { + timeoutCntADC = 0; // Reset the timeout counter + } + } else { + if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification + timeoutFlagADC = 1; // Timeout detected + timeoutCntADC = ADC_PROTECT_TIMEOUT; // Limit timout counter value + } + } + + if (timeoutFlagADC) { // In case of timeout bring the system to a Safe State + ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way + cmd1 = 0; + cmd2 = 0; + } else { + ctrlModReq = ctrlModReqRaw; // Follow the Mode request + } + #endif + + // use ADCs as button inputs: + #ifdef SUPPORT_BUTTONS + button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1 + button2 = (uint8_t)(adc_buffer.l_rx2 > 2000); // ADC2 + #endif + + timeout = 0; + #endif + + #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) + // Handle received data validity, timeout and fix out-of-sync if necessary + #ifdef CONTROL_IBUS + ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; + for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { + ibus_chksum -= command.channels[i]; + } + if (command.start == IBUS_LENGTH && command.type == IBUS_COMMAND && ibus_chksum == (uint16_t)((command.checksumh << 8) + command.checksuml)) { + if (timeoutFlagSerial) { // Check for previous timeout flag + if (timeoutCntSerial-- <= 0) // Timeout de-qualification + timeoutFlagSerial = 0; // Timeout flag cleared + } else { + for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) { + ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, INPUT_MAX); // 1000-2000 -> 0-1000 + } + cmd1 = CLAMP((ibus_captured_value[0] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP((ibus_captured_value[1] - INPUT_MID) * 2, INPUT_MIN, INPUT_MAX); + command.start = 0xFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial = 0; // Reset the timeout counter + } + } else { + if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial = 1; // Timeout detected + timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value + } + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 30 == 0) { + HAL_UART_DMAStop(&huart); + HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); + } + } + #else + if (command.start == SERIAL_START_FRAME && command.checksum == (uint16_t)(command.start ^ command.steer ^ command.speed)) { + if (timeoutFlagSerial) { // Check for previous timeout flag + if (timeoutCntSerial-- <= 0) // Timeout de-qualification + timeoutFlagSerial = 0; // Timeout flag cleared + } else { + cmd1 = CLAMP((int16_t)command.steer, INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP((int16_t)command.speed, INPUT_MIN, INPUT_MAX); + command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial = 0; // Reset the timeout counter + } + } else { + if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial = 1; // Timeout detected + timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value + } + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 30 == 0) { + HAL_UART_DMAStop(&huart); + HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command)); + } + } + #endif + + if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State + ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way + cmd1 = 0; + cmd2 = 0; + } else { + ctrlModReq = ctrlModReqRaw; // Follow the Mode request + } + timeout = 0; + #endif + + + #ifdef SIDEBOARD_SERIAL_USART2 + if (Sideboard_Lnew.start == SERIAL_START_FRAME && Sideboard_Lnew.checksum == (uint16_t)(Sideboard_Lnew.start ^ Sideboard_Lnew.roll ^ Sideboard_Lnew.pitch ^ Sideboard_Lnew.yaw ^ Sideboard_Lnew.sensors)) { + if (timeoutFlagSerial_L) { // Check for previous timeout flag + if (timeoutCntSerial_L-- <= 0) // Timeout de-qualification + timeoutFlagSerial_L = 0; // Timeout flag cleared + } else { + memcpy(&Sideboard_L, &Sideboard_Lnew, sizeof(Sideboard_L)); // Copy the new data + Sideboard_Lnew.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial_L = 0; // Reset the timeout counter + } + } else { + if (timeoutCntSerial_L++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial_L = 1; // Timeout detected + timeoutCntSerial_L = SERIAL_TIMEOUT; // Limit timout counter value + } + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 30 == 0) { + HAL_UART_DMAStop(&huart2); + HAL_UART_Receive_DMA(&huart2, (uint8_t *)&Sideboard_Lnew, sizeof(Sideboard_Lnew)); + } + } + timeoutFlagSerial = timeoutFlagSerial_L; + #endif + #ifdef SIDEBOARD_SERIAL_USART3 + if (Sideboard_Rnew.start == SERIAL_START_FRAME && Sideboard_Rnew.checksum == (uint16_t)(Sideboard_Rnew.start ^ Sideboard_Rnew.roll ^ Sideboard_Rnew.pitch ^ Sideboard_Rnew.yaw ^ Sideboard_Rnew.sensors)) { + if (timeoutFlagSerial_R) { // Check for previous timeout flag + if (timeoutCntSerial_R-- <= 0) // Timeout de-qualification + timeoutFlagSerial_R = 0; // Timeout flag cleared + } else { + memcpy(&Sideboard_R, &Sideboard_Rnew, sizeof(Sideboard_R)); // Copy the new data + Sideboard_Rnew.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle + timeoutCntSerial_R = 0; // Reset the timeout counter + } + } else { + if (timeoutCntSerial_R++ >= SERIAL_TIMEOUT) { // Timeout qualification + timeoutFlagSerial_R = 1; // Timeout detected + timeoutCntSerial_R = SERIAL_TIMEOUT; // Limit timout counter value + } + // Most probably we are out-of-sync. Try to re-sync by reseting the DMA + if (main_loop_counter % 30 == 0) { + HAL_UART_DMAStop(&huart3); + HAL_UART_Receive_DMA(&huart3, (uint8_t *)&Sideboard_Rnew, sizeof(Sideboard_Rnew)); + Sideboard_Rnew.start = 0xFFFF; // Change the Start Frame to avoid entering again here if no data is received + } + } + timeoutFlagSerial = timeoutFlagSerial_R; + #endif + #if defined(SIDEBOARD_SERIAL_USART2) && defined(SIDEBOARD_SERIAL_USART3) + timeoutFlagSerial = timeoutFlagSerial_L | timeoutFlagSerial_R; + #endif + + #ifdef VARIANT_HOVERCAR + brakePressed = (uint8_t)(cmd1 > 50); + #endif + + #ifdef VARIANT_TRANSPOTTER + #ifdef GAMETRAK_CONNECTION_NORMAL + cmd1 = adc_buffer.l_rx2; + cmd2 = adc_buffer.l_tx2; + #endif + #ifdef GAMETRAK_CONNECTION_ALTERNATE + cmd1 = adc_buffer.l_tx2; + cmd2 = adc_buffer.l_rx2; + #endif + #endif + + + +} + + + +/* =========================== Sideboard Functions =========================== */ + +/* + * Sideboard LEDs Handling + * This function manages the leds behavior connected to the sideboard + */ +void sideboardLeds(uint8_t *leds) { + #if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) + // Enable flag: use LED4 (bottom Blue) + // enable == 1, turn on led + // enable == 0, blink led + if (enable) { + *leds |= LED4_SET; + } else if (!enable && (main_loop_counter % 20 == 0)) { + *leds ^= LED4_SET; + } + + // Backward Drive: use LED5 (upper Blue) + // backwardDrive == 1, blink led + // backwardDrive == 0, turn off led + if (backwardDrive && (main_loop_counter % 50 == 0)) { + *leds ^= LED5_SET; + } + + // Brake: use LED5 (upper Blue) + // brakePressed == 1, turn on led + // brakePressed == 0, turn off led + #ifdef VARIANT_HOVERCAR + if (brakePressed) { + *leds |= LED5_SET; + } else if (!brakePressed && !backwardDrive) { + *leds &= ~LED5_SET; + } + #endif + + // Battery Level Indicator: use LED1, LED2, LED3 + if (main_loop_counter % BAT_BLINK_INTERVAL == 0) { // | RED (LED1) | YELLOW (LED3) | GREEN (LED2) | + if (batVoltage < BAT_DEAD) { // | 0 | 0 | 0 | + *leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET; + } else if (batVoltage < BAT_LVL1) { // | B | 0 | 0 | + *leds ^= LED1_SET; + *leds &= ~LED3_SET & ~LED2_SET; + } else if (batVoltage < BAT_LVL2) { // | 1 | 0 | 0 | + *leds |= LED1_SET; + *leds &= ~LED3_SET & ~LED2_SET; + } else if (batVoltage < BAT_LVL3) { // | 0 | B | 0 | + *leds ^= LED3_SET; + *leds &= ~LED1_SET & ~LED2_SET; + } else if (batVoltage < BAT_LVL4) { // | 0 | 1 | 0 | + *leds |= LED3_SET; + *leds &= ~LED1_SET & ~LED2_SET; + } else if (batVoltage < BAT_LVL5) { // | 0 | 0 | B | + *leds ^= LED2_SET; + *leds &= ~LED1_SET & ~LED3_SET; + } else { // | 0 | 0 | 1 | + *leds |= LED2_SET; + *leds &= ~LED1_SET & ~LED3_SET; + } + } + + // Error handling + // Critical error: LED1 on (RED) + high pitch beep (hadled in main) + // Soft error: LED3 on (YELLOW) + low pitch beep (hadled in main) + if (rtY_Left.z_errCode || rtY_Right.z_errCode) { + *leds |= LED1_SET; + *leds &= ~LED3_SET & ~LED2_SET; + } + if (timeoutFlagADC || timeoutFlagSerial) { + *leds |= LED3_SET; + *leds &= ~LED1_SET & ~LED2_SET; + } + #endif +} + +/* + * Sideboard Sensor Handling + * This function manages the sideboards photo sensors. + * In non-hoverboard variants, the sensors are used as push buttons. + */ +void sideboardSensors(uint8_t sensors) { + #if !defined(VARIANT_HOVERBOARD) && (defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3)) + uint8_t sensor1_rising_edge, sensor2_rising_edge; + sensor1_rising_edge = (sensors & SENSOR1_SET) && !sensor1_prev; + sensor2_rising_edge = (sensors & SENSOR2_SET) && !sensor2_prev; + sensor1_prev = sensors & SENSOR1_SET; + sensor2_prev = sensors & SENSOR2_SET; + + // Control MODE and Control Type Handling: use Sensor1 as push button + if (sensor1_rising_edge) { + sensor1_index++; + if (sensor1_index > 4) { sensor1_index = 0; } + switch (sensor1_index) { + case 0: // FOC VOLTAGE + rtP_Left.z_ctrlTypSel = 2; + rtP_Right.z_ctrlTypSel = 2; + ctrlModReqRaw = 1; + break; + case 1: // FOC SPEED + ctrlModReqRaw = 2; + break; + case 2: // FOC TORQUE + ctrlModReqRaw = 3; + break; + case 3: // SINUSOIDAL + rtP_Left.z_ctrlTypSel = 1; + rtP_Right.z_ctrlTypSel = 1; + break; + case 4: // COMMUTATION + rtP_Left.z_ctrlTypSel = 0; + rtP_Right.z_ctrlTypSel = 0; + break; + } + shortBeepMany(sensor1_index + 1); + } + + // Field Weakening: use Sensor2 as push button + if (sensor2_rising_edge) { + sensor2_index++; + if (sensor2_index > 1) { sensor2_index = 0; } + switch (sensor2_index) { + case 0: // FW Disabled + rtP_Left.b_fieldWeakEna = 0; + rtP_Right.b_fieldWeakEna = 0; + Input_Lim_Init(); + break; + case 1: // FW Enabled + rtP_Left.b_fieldWeakEna = 1; + rtP_Right.b_fieldWeakEna = 1; + Input_Lim_Init(); + break; + } + shortBeepMany(sensor2_index + 1); + } + #endif +} + + + +/* =========================== Filtering Functions =========================== */ + + /* Low pass filter fixed-point 32 bits: fixdt(1,32,20) + * Max: 2047.9375 + * Min: -2048 + * Res: 0.0625 + * + * Inputs: u = int16 or 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) + * filtLowPass16(u, 52429, &y); + * yint = (int16_t)(y >> 16); // the integer output is the fixed-point ouput shifted by 16 bits + */ + void filtLowPass32(int32_t u, uint16_t coef, int32_t *y) { + int64_t tmp; + tmp = ((int64_t)((u << 4) - (*y >> 12)) * coef) >> 4; + tmp = CLAMP(tmp, -2147483648LL, 2147483647LL); // Overflow protection: 2147483647LL = 2^31 - 1 + *y = (int32_t)tmp + (*y); +} + // Old filter + // Inputs: u = int16 + // Outputs: y = fixdt(1,32,20) + // Parameters: coef = fixdt(0,16,16) = [0,65535U] + // yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits + // void filtLowPass32(int16_t u, uint16_t coef, int32_t *y) { + // int32_t tmp; + // tmp = (int16_t)(u << 4) - (*y >> 16); + // tmp = CLAMP(tmp, -32768, 32767); // Overflow protection + // *y = coef * tmp + (*y); + // } + + + /* rateLimiter16(int16_t u, int16_t rate, int16_t *y); + * Inputs: u = int16 + * Outputs: y = fixdt(1,16,4) + * Parameters: rate = fixdt(1,16,4) = [0, 32767] Do NOT make rate negative (>32767) + */ +void rateLimiter16(int16_t u, int16_t rate, int16_t *y) { + int16_t q0; + int16_t q1; + + q0 = (u << 4) - *y; + + if (q0 > rate) { + q0 = rate; + } else { + q1 = -rate; + if (q0 < q1) { + q0 = q1; + } + } + + *y = q0 + *y; +} + + + /* 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,14) + */ +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, INPUT_MIN, INPUT_MAX); + + 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, INPUT_MIN, INPUT_MAX); +} + + + +/* =========================== Multiple Tap Function =========================== */ + + /* multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) + * This function detects multiple tap presses, such as double tapping, triple tapping, etc. + * Inputs: u = int16_t (input signal); timeNow = uint32_t (current time) + * Outputs: x->b_multipleTap (get the output here) + */ +void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) { + uint8_t b_timeout; + uint8_t b_hyst; + uint8_t b_pulse; + uint8_t z_pulseCnt; + uint8_t z_pulseCntRst; + uint32_t t_time; + + // Detect hysteresis + if (x->b_hysteresis) { + b_hyst = (u > MULTIPLE_TAP_LO); + } else { + b_hyst = (u > MULTIPLE_TAP_HI); + } + + // Detect pulse + b_pulse = (b_hyst != x->b_hysteresis); + + // Save time when first pulse is detected + if (b_hyst && b_pulse && (x->z_pulseCntPrev == 0)) { + t_time = timeNow; + } else { + t_time = x->t_timePrev; + } + + // Create timeout boolean + b_timeout = (timeNow - t_time > MULTIPLE_TAP_TIMEOUT); + + // Create pulse counter + if ((!b_hyst) && (x->z_pulseCntPrev == 0)) { + z_pulseCnt = 0U; + } else { + z_pulseCnt = b_pulse; + } + + // Reset counter if we detected complete tap presses OR there is a timeout + if ((x->z_pulseCntPrev >= MULTIPLE_TAP_NR) || b_timeout) { + z_pulseCntRst = 0U; + } else { + z_pulseCntRst = x->z_pulseCntPrev; + } + z_pulseCnt = z_pulseCnt + z_pulseCntRst; + + // Check if complete tap presses are detected AND no timeout + if ((z_pulseCnt >= MULTIPLE_TAP_NR) && (!b_timeout)) { + x->b_multipleTap = !x->b_multipleTap; // Toggle output + } + + // Update states + x->z_pulseCntPrev = z_pulseCnt; + x->b_hysteresis = b_hyst; + x->t_timePrev = t_time; +} + + diff --git a/docs/STM32F103xC_datasheet.pdf b/docs/STM32F103xC_datasheet.pdf new file mode 100644 index 0000000..46762ef Binary files /dev/null and b/docs/STM32F103xC_datasheet.pdf differ diff --git a/platformio.ini b/platformio.ini index 3a36b38..4570825 100644 --- a/platformio.ini +++ b/platformio.ini @@ -33,7 +33,6 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -57,7 +56,6 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -77,7 +75,6 @@ debug_tool = stlink upload_protocol = stlink build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -97,7 +94,6 @@ debug_tool = stlink upload_protocol = stlink build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -117,7 +113,6 @@ debug_tool = stlink upload_protocol = stlink build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -141,7 +136,6 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -165,7 +159,6 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -185,7 +178,6 @@ debug_tool = stlink upload_protocol = stlink build_flags = - -I${PROJECT_DIR}/inc/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld