diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..8a6de826 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "Src/bipropellantProtocol"] + path = Src/bipropellantProtocol + url = https://github.com/bipropellant/hbprotocol.git diff --git a/Inc/bipropellantProtocolMachine.h b/Inc/bipropellantProtocolMachine.h new file mode 100644 index 00000000..aa8bda3a --- /dev/null +++ b/Inc/bipropellantProtocolMachine.h @@ -0,0 +1,12 @@ +// Define to prevent recursive inclusion +#ifndef BIPROPELLANTPROTOCOLMACHINE_H +#define BIPROPELLANTPROTOCOLMACHINE_H + +#include "protocol.h" + +int setup_protocol(PROTOCOL_STAT *s); + +extern PROTOCOL_STAT sUSART2; +extern PROTOCOL_STAT sUSART3; + +#endif \ No newline at end of file diff --git a/Inc/bipropellantProtocolStructs.h b/Inc/bipropellantProtocolStructs.h new file mode 100644 index 00000000..39e8e97b --- /dev/null +++ b/Inc/bipropellantProtocolStructs.h @@ -0,0 +1,94 @@ +/* +* This file is part of the hoverboard-firmware-hack project. +* +* Copyright (C) 2018 Simon Hailes +* +* 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 . +*/ + +//////////////////////////////////////////////////////////////////// +// structures used in main repo which may mirror protocol structures +//////////////////////////////////////////////////////////////////// + +// Define to prevent recursive inclusion +#ifndef BIPROPELLANTPROTOCOLSTRUCTS_H +#define BIPROPELLANTPROTOCOLSTRUCTS_H + +#include "protocol.h" + +//// control structures used in firmware + +// used in main +extern PROTOCOL_SPEED_DATA SpeedData; + +extern PROTOCOL_PWM_DATA PWMData; + + +#pragma pack(push, 4) // int32_t and float are 4 byte each +typedef struct tag_HALL_DATA_STRUCT{ + int32_t HallPosn; // 90 per revolution + int32_t HallSpeed; // speed part calibrated to speed demand value + + float HallPosnMultiplier; // m per hall segment + + int32_t HallPosn_lastread; // posn offset set via protocol in raw value + int32_t HallPosn_mm; // posn in mm + int32_t HallPosn_mm_lastread; // posn offset set via protocol in mm + int32_t HallSpeed_mm_per_s; // speed in m/s + + uint32_t HallTimeDiff; + uint32_t HallSkipped; +} HALL_DATA_STRUCT; +#pragma pack(pop) + +#pragma pack(push, 4) // all used types (float and int) are 4 bytes + +typedef struct tag_MOTOR_ELECTRICAL{ + float dcAmps; + float dcAmpsAvgAcc; + float dcAmpsAvg; + int r1; + int r2; + int q; + + int dcAmpsx100; + + int pwm_limiter; + int pwm_requested; + int pwm_actual; + + unsigned int limiter_count; +} MOTOR_ELECTRICAL; +#pragma pack(pop) + +#pragma pack(push, 4) // all used types (float and int) are 4 bytes +typedef struct tag_ELECTRICAL_PARAMS{ + int bat_raw; + float batteryVoltage; + + int board_temp_raw; + float board_temp_filtered; + float board_temp_deg_c; + + int charging; + + int dcCurLim; // amps*100 + int dc_adc_limit; // limit expressed in terms of ADC units. + + MOTOR_ELECTRICAL motors[2]; + +} ELECTRICAL_PARAMS; +#pragma pack(pop) + +#endif \ No newline at end of file diff --git a/Inc/bldc.h b/Inc/bldc.h new file mode 100644 index 00000000..1fc0392c --- /dev/null +++ b/Inc/bldc.h @@ -0,0 +1,12 @@ +#ifndef BLDC_H +#define BLDC_H + +#include + +uint8_t bldc_getMotorsEnable(void); +void bldc_setMotorsEnable(uint8_t enable); + +extern int16_t batVoltage; +extern int32_t batVoltageFixdt; + +#endif \ No newline at end of file diff --git a/Inc/comms.h b/Inc/comms.h index 6bdb730d..5f0b4b2f 100644 --- a/Inc/comms.h +++ b/Inc/comms.h @@ -23,7 +23,6 @@ #ifndef COMMS_H #define COMMS_H -#include "stm32f1xx_hal.h" void setScopeChannel(uint8_t ch, int16_t val); void consoleScope(void); diff --git a/Inc/config.h b/Inc/config.h index 3435575d..659fb5a9 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -19,6 +19,7 @@ //#define VARIANT_HOVERCAR // Variant for HOVERCAR 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 + //#define VARIANT_BIPROPELLANT// Variant for Bipropellant Protocol #endif // ########################### END OF VARIANT SELECTION ############################ @@ -73,7 +74,7 @@ #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_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 @@ -252,6 +253,21 @@ #endif // ######################## END OF VARIANT_USART SETTINGS ######################### +// ############################ VARIANT_BIPROPELLANT SETTINGS ############################ +#ifdef VARIANT_BIPROPELLANT +// #define USART2_ENABLE + #define USART3_ENABLE + #define USART2_BAUD 115200 + #define USART3_BAUD 115200 + #undef TIMEOUT + #define TIMEOUT 30 // number of wrong / missing input commands before emergency off + #undef BEEPS_BACKWARD + #define BEEPS_BACKWARD 0 // 0 or 1 + #define SERIAL_BUFFER_SIZE 64 // [bytes] Size of Serial Rx buffer. Make sure it is always larger than the structure size + #define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B + #define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B +#endif +// ######################## END OF VARIANT_BIPROPELLANT SETTINGS ######################### // ################################# VARIANT_NUNCHUK SETTINGS ############################ @@ -318,7 +334,7 @@ #define PWM_CH1_MAX 1000 // (0 - 1000) #define PWM_CH1_MIN -1000 // (-1000 - 0) #define PWM_CH2_MAX 1000 // (0 - 1000) - #define PWM_CH2_MIN -1000 // (-1000 - 0) + #define PWM_CH2_MIN -1000 // (-1000 - 0) #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 #define STEER_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. @@ -333,7 +349,7 @@ // ################################# VARIANT_IBUS SETTINGS ############################## #ifdef VARIANT_IBUS -/* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL +/* CONTROL VIA RC REMOTE WITH FLYSKY IBUS PROTOCOL * Connected to Left sensor board cable. Channel 1: steering, Channel 2: speed. */ #define CONTROL_IBUS // use IBUS as input @@ -384,10 +400,10 @@ // Communication: [DONE] // Balancing controller: [TODO] #ifdef VARIANT_HOVERBOARD - #define SIDEBOARD_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM 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 + #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 ######################### @@ -429,6 +445,12 @@ #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(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) + #define USART2_ENABLE +#endif +#if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) + #define USART3_ENABLE +#endif // ########################### UART SETIINGS ############################ @@ -452,7 +474,7 @@ // ############################### VALIDATE SETTINGS ############################### #if !defined(VARIANT_ADC) && !defined(VARIANT_USART) && !defined(VARIANT_NUNCHUK) && !defined(VARIANT_PPM) && !defined(VARIANT_PWM) && \ - !defined(VARIANT_IBUS) && !defined(VARIANT_HOVERCAR) && !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) + !defined(VARIANT_IBUS) && !defined(VARIANT_HOVERCAR) && !defined(VARIANT_HOVERBOARD) && !defined(VARIANT_TRANSPOTTER) && !defined(VARIANT_BIPROPELLANT) #error Variant not defined! Please check platformio.ini or Inc/config.h for available variants. #endif diff --git a/Inc/setup.h b/Inc/setup.h index b7dd9d03..ce87e930 100644 --- a/Inc/setup.h +++ b/Inc/setup.h @@ -24,6 +24,7 @@ #define SETUP_H #include "stm32f1xx_hal.h" +#include "defines.h" void MX_GPIO_Init(void); void MX_TIM_Init(void); @@ -32,5 +33,7 @@ void MX_ADC2_Init(void); void UART2_Init(void); void UART3_Init(void); +extern volatile adc_buf_t adc_buffer; + #endif diff --git a/Inc/util.h b/Inc/util.h index a09136a1..39338dbb 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -26,20 +26,20 @@ // Rx Structures USART #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) - #ifdef CONTROL_IBUS + #ifdef CONTROL_IBUS typedef struct{ uint8_t start; - uint8_t type; + uint8_t type; uint8_t channels[IBUS_NUM_CHANNELS*2]; uint8_t checksuml; - uint8_t checksumh; + uint8_t checksumh; } SerialCommand; #else typedef struct{ - uint16_t start; + uint16_t start; int16_t steer; int16_t speed; - uint16_t checksum; + uint16_t checksum; } SerialCommand; #endif #endif @@ -79,15 +79,6 @@ void poweroffPressCheck(void); void readCommand(void); void usart2_rx_check(void); void usart3_rx_check(void); -#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) -void usart_process_debug(uint8_t *userCommand, uint32_t len); -#endif -#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) -void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx); -#endif -#if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) -void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sideboard_out, uint8_t usart_idx); -#endif // Sideboard functions void sideboardLeds(uint8_t *leds); @@ -107,5 +98,7 @@ typedef struct { } MultipleTap; void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x); +extern uint8_t ctrlModReq; + #endif diff --git a/Makefile b/Makefile index 6880e1e5..e34db835 100644 --- a/Makefile +++ b/Makefile @@ -46,6 +46,12 @@ Src/hd44780.c \ Src/pcf8574.c \ Src/comms.c \ Src/stm32f1xx_it.c \ +Src/bipropellantProtocol/protocol.c \ +Src/bipropellantProtocol/machine_protocol.c \ +Src/bipropellantProtocol/ascii_protocol.c \ +Src/bipropellantProtocol/cobsr.c \ +Src/bipropellantProtocolASCII.c \ +Src/bipropellantProtocolMachine.c \ Src/BLDC_controller_data.c \ Src/BLDC_controller.c @@ -97,6 +103,7 @@ AS_INCLUDES = # C includes C_INCLUDES = \ -IInc \ +-ISrc/bipropellantProtocol \ -IDrivers/STM32F1xx_HAL_Driver/Inc \ -IDrivers/STM32F1xx_HAL_Driver/Inc/Legacy \ -IDrivers/CMSIS/Device/ST/STM32F1xx/Include \ diff --git a/Src/bipropellantProtocol b/Src/bipropellantProtocol new file mode 160000 index 00000000..fa5e754b --- /dev/null +++ b/Src/bipropellantProtocol @@ -0,0 +1 @@ +Subproject commit fa5e754b9ac0cb160c94e9cb3fc0f688d597f40a diff --git a/Src/bipropellantProtocolASCII.c b/Src/bipropellantProtocolASCII.c new file mode 100644 index 00000000..336ac58b --- /dev/null +++ b/Src/bipropellantProtocolASCII.c @@ -0,0 +1,514 @@ +/* +* This file is part of the hoverboard-firmware-hack project. +* +* Copyright (C) 2018 Simon Hailes +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ + +#include "stm32f1xx_hal.h" +#include "defines.h" +#include "config.h" +#include "bldc.h" + +#include "bipropellantProtocolMachine.h" +#include "protocol.h" +#include "comms.h" + +#include +#include +#include +#include "bipropellantProtocolStructs.h" + + +////////////////////////////////////////////////////////// +// +// ASCII protocol: +// this accepts command sup to 10 bytes long terminated with CR. +// one of these commands (I) can enable an 'immediate' mode. +// In 'immediate' mode, keypresses cause immediate action; +// for example, controlling speed, or getting real-time feedback. +// +////////////////////////////////////////////////////////// + + +/////////////////////////////////////////////// +// extern variables you want to read/write here + +// from main.c +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 enablescope; // enable scope on values + +static int speedB = 0; +static int steerB = 0; + + +/////////////////////////////////////////////// + + +int immediate_dir(PROTOCOL_STAT *s, char byte, char *ascii_out) { + int dir = 1; + switch(byte){ + case 'S': + case 's': + dir = -1; + case 'W': + case 'w': + if ( !bldc_getMotorsEnable() ) + { + speedB = 0; steerB = 0; + } + bldc_setMotorsEnable(1); + speedB += 10*dir; + PWMData.pwm[1] = CLAMP(speedB - steerB, -1000, 1000); + PWMData.pwm[0] = CLAMP(speedB + steerB, -1000, 1000); + sprintf(ascii_out, "speed now %d, steer now %d, pwm %ld, pwm %ld\r\n", speedB, steerB, PWMData.pwm[0], PWMData.pwm[1]); + break; + + case 'A': + case 'a': + dir = -1; + case 'D': + case 'd': + if ( !bldc_getMotorsEnable() ) + { + speedB = 0; steerB = 0; + } + bldc_setMotorsEnable(1); + steerB += 10*dir; + PWMData.pwm[1] = CLAMP(speedB - steerB, -1000, 1000); + PWMData.pwm[0] = CLAMP(speedB + steerB, -1000, 1000); + sprintf(ascii_out, "speed now %d, steer now %d, pwm %ld, pwm %ld\r\n", speedB, steerB, PWMData.pwm[0], PWMData.pwm[1]); + break; + } + + return 1; +} + + +int immediate_stop(PROTOCOL_STAT *s, char byte, char *ascii_out) { + speedB = 0; + steerB = 0; + PWMData.pwm[1] = CLAMP(speedB - steerB, -1000, 1000); + PWMData.pwm[0] = CLAMP(speedB + steerB, -1000, 1000); + SpeedData.wanted_speed_mm_per_sec[0] = SpeedData.wanted_speed_mm_per_sec[1] = speedB; + bldc_setMotorsEnable(0); + sprintf(ascii_out, "Stop set\r\n"); + return 1; +} + +int immediate_quit(PROTOCOL_STAT *s, char byte, char *ascii_out) { + s->ascii.enable_immediate = 0; + speedB = 0; + steerB = 0; + PWMData.pwm[1] = CLAMP(speedB - steerB, -1000, 1000); + PWMData.pwm[0] = CLAMP(speedB + steerB, -1000, 1000); + SpeedData.wanted_speed_mm_per_sec[0] = SpeedData.wanted_speed_mm_per_sec[1] = speedB; + bldc_setMotorsEnable(0); + sprintf(ascii_out, "Immediate commands disabled\r\n"); + return 1; +} + +int immediate_hall(PROTOCOL_STAT *s, char byte, char *ascii_out) { + return 1; +} + +int immediate_sensors(PROTOCOL_STAT *s, char byte, char *ascii_out) { + sprintf(ascii_out, "Sensor Data not available\r\n"); + return 1; +} + +int immediate_electrical(PROTOCOL_STAT *s, char byte, char *ascii_out) { + return 1; +} + +int immediate_stm32(PROTOCOL_STAT *s, char byte, char *ascii_out) { +// case 'G': + sprintf(ascii_out, + "A:%04X B:%04X C:%04X D:%04X E:%04X\r\n"\ + "Button: %d Charge:%d\r\n", + (int)GPIOA->IDR, (int)GPIOB->IDR, (int)GPIOC->IDR, (int)GPIOD->IDR, (int)GPIOE->IDR, + (int)(BUTTON_PORT->IDR & BUTTON_PIN)?1:0, + (int)(CHARGER_PORT->IDR & CHARGER_PIN)?1:0 + ); + return 1; +} + +int immediate_toggle_control(PROTOCOL_STAT *s, char byte, char *ascii_out) { +//case 'O': + //stop all + immediate_stop(s, byte, ascii_out); + ascii_out += strlen(ascii_out); + return 1; +} + +int line_set_alarm(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'A': + int a = 0; + int b = 0; + int c = 0; + if (strlen(cmd) > 1){ + sscanf(cmd+1, "%d %d %d", &a, &b, &c); + } + if (a && (0==c)){ + c = 1000; + } + + buzzerFreq = a; + buzzerPattern = b; + sprintf(ascii_out, "Alarm set to %d %d %d\r\n", a, b, c); + return 1; +} + +int line_electrical(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'c': + immediate_electrical(s, *cmd, ascii_out); + return 1; +} + +int line_main_timing_stats(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 's': // display stats from main timing + // we don't have float printing + return 1; +} + + +// NOTE: needs params +int line_generic_var(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'f': // setting any parameter marked with uistr + int len = strlen(cmd); + if (len == 1){ + sprintf(ascii_out, "no flash var given\r\n"); + } else { + if ((cmd[1] | 0x20) == 'i'){ // initilaise + sprintf(ascii_out, "Flash initialised\r\n"); + } else { + if ((cmd[1] | 0x20) == 'a'){ + // read all + for (int i = 0; i < (sizeof(s->params)/sizeof(s->params[0])); i++){ + if(s->params[i] != NULL) { + if (s->params[i]->uistr){ + switch (s->params[i]->ui_type){ + case UI_SHORT: + { + // read it + + PROTOCOL_MSG3full newMsg; + memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG3full)); + + if (s->params[i]->fn) s->params[i]->fn( s, s->params[i], PROTOCOL_CMD_SILENTREAD, &newMsg); + + sprintf(ascii_out, "%s(%s): %d\r\n", + (s->params[i]->description)?s->params[i]->description:"", + s->params[i]->uistr, + (int)*(short *)newMsg.content); + s->send_serial_data_wait((unsigned char *)ascii_out, strlen(ascii_out)); + ascii_out[0] = 0; // don't print last one twice + break; + } + default: + break; + } + } + } + } + } else { + int i = 0; + int count = (sizeof(s->params)/sizeof(s->params[0])); + for (i = 0; i < count; i++){ + if(s->params[i] != NULL) { + if (s->params[i]->uistr){ + if (!strncmp(s->params[i]->uistr, &cmd[1], strlen(s->params[i]->uistr))){ + switch (s->params[i]->ui_type){ + case UI_SHORT: + // if number supplied, write + if ((cmd[1+strlen(s->params[i]->uistr)] >= '0') && (cmd[1+strlen(s->params[i]->uistr)] <= '9')){ + + PROTOCOL_MSG3full newMsg; + memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG3full)); + + *((short *)newMsg.content) = atoi(&cmd[1+strlen(s->params[i]->uistr)]); + newMsg.code = s->params[i]->code; + newMsg.cmd = PROTOCOL_CMD_READVALRESPONSE; + + if (s->params[i]->fn) s->params[i]->fn( s, s->params[i], PROTOCOL_CMD_READVALRESPONSE, &newMsg); + + + sprintf(ascii_out, "flash var %s(%s) now %d\r\n", + (s->params[i]->description)?s->params[i]->description:"", + s->params[i]->uistr, + (int)*(short *)s->params[i]->ptr); + } else { + // read it + PROTOCOL_MSG3full newMsg; + memset((void*)&newMsg,0x00,sizeof(PROTOCOL_MSG3full)); + + if (s->params[i]->fn) s->params[i]->fn( s, s->params[i], PROTOCOL_CMD_SILENTREAD, &newMsg); + + sprintf(ascii_out, "%s(%s): %d\r\n", + (s->params[i]->description)?s->params[i]->description:"", + s->params[i]->uistr, + (int)*(short *)s->params[i]->ptr + ); + s->send_serial_data_wait((unsigned char *)ascii_out, strlen(ascii_out)); + ascii_out[0] = 0; // don't print last one twice + } + break; + default: + sprintf(ascii_out, "flash var %s(%s) unsupported type\r\n", + (s->params[i]->description)?s->params[i]->description:"", + s->params[i]->uistr + ); + break; + } + break; // found our param, now leave + } + } + } + } + if (i == count){ + sprintf(ascii_out, "unknown flash data %s\r\n", cmd); + } + } + } + } + return 1; +} + + +// must be after all params added? +char *get_F_description(PROTOCOL_STAT *s) { + + char *p = NULL; + int len = 0; + + // first loop gets len, 2nd loop makes string + for (int l = 0; l < 2; l++) { + char *t = "print/set a flash constant (Fa to print all, Fi to default all):\r\n" + " Fss - print, Fss - set\r\n"; + + len = strlen(t); + + if (p) strcat(p, t); + + for (int i = 0; i < (sizeof(s->params)/sizeof(s->params[0])); i++){ + if(s->params[i] != NULL) { + if (s->params[i]->uistr){ + char tmp[256]; + snprintf(tmp, sizeof(tmp)-1, + " F%s - %s\r\n", + s->params[i]->uistr, + (s->params[i]->description)?s->params[i]->description:"" + ); + len += strlen(tmp); + if (p) strcat(p, tmp); + } + } + } + + if (NULL == p) { + p = malloc(len+1); + *p = 0; + } + + } + return p; +} + + +int line_stm32(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'G': + immediate_stm32(s, *cmd, ascii_out); + return 1; +} + +int line_hall(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'H': + immediate_hall(s, *cmd, ascii_out); + return 1; +} + +int line_immediate(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'I': + speedB = 0; + steerB = 0; + PWMData.pwm[1] = CLAMP(speedB - steerB, -1000, 1000); + PWMData.pwm[0] = CLAMP(speedB + steerB, -1000, 1000); + SpeedData.wanted_speed_mm_per_sec[0] = SpeedData.wanted_speed_mm_per_sec[1] = speedB; + if (strlen(cmd) == 1){ + s->ascii.enable_immediate = 1; + sprintf(ascii_out, "Immediate commands enabled - WASDXHCGQ\r\n>"); + } else { + switch (cmd[1] | 0x20){ + case 's': + s->ascii.enable_immediate = 1; + sprintf(ascii_out, "Immediate commands enabled - WASDXHCGQ - Speed control\r\n>"); + break; + case 'p': + s->ascii.enable_immediate = 1; + sprintf(ascii_out, "Immediate commands enabled - WASDXHCGQ - Position control\r\n>"); + break; + case 'w': + s->ascii.enable_immediate = 1; + sprintf(ascii_out, "Immediate commands enabled - WASDXHCGQ - Power (pWm) control\r\n>"); + break; + } + } + return 1; +} + +int line_sensors(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'N': + immediate_sensors(s, *cmd, ascii_out); + return 1; +} + +int line_poweroff_control(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'P': + if (strlen(cmd) == 1){ + + } else { + if ((cmd[1] | 0x20) == 'r'){ + sprintf(ascii_out, "Reset in 500ms\r\n"); + s->send_serial_data_wait((unsigned char *)ascii_out, strlen(ascii_out)); + HAL_Delay(500); + HAL_NVIC_SystemReset(); + } + + if ((cmd[1] | 0x20) == 'e'){ + + } else { + int s = -1; + sscanf(cmd+1, "%d", &s); + if (s >= 0){ + + } + } + } + + return 1; +} + +int line_test_message(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'T': + if (strlen(cmd) < 2){ + sprintf(ascii_out, "Test command needs A N or T qualifier\r\n"); + } else { + // send a test message in machine protocol + switch (cmd[1]){ + case 'A': + case 'a': + //protocol_send_ack(); + break; + case 'N': + case 'n': + //protocol_send_nack(); + break; + case 'T': + case 't':{ + char tmp[] = { PROTOCOL_SOM_ACK, 0, 5, PROTOCOL_CMD_TEST, 'T', 'e', 's', 't' }; + protocol_post(s, (PROTOCOL_MSG3full*)tmp); + } + break; + } + // CR before prompt.... after message + sprintf(ascii_out, "\r\n"); + } + return 1; +} + + +int line_reset_firmware(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +//case 'R': + if (cmd[1] == '!'){ + sprintf(ascii_out, "\r\n!!!!!Resetting!!!!!\r\n"); + s->send_serial_data_wait((unsigned char *)ascii_out, strlen(ascii_out)); + ascii_out[0] = 0; + HAL_Delay(500); + HAL_NVIC_SystemReset(); + } + return 1; +} + +int line_read_memory(PROTOCOL_STAT *s, char *cmd, char *ascii_out) { +// memory read hex address +//case 'M': + unsigned char *addr = 0; + unsigned int len = 4; + if (cmd[1] == 'f') { + + } else { + sscanf(&cmd[1], "%lx,%x", (uint32_t *)&addr, &len); + } + strcat( ascii_out, "\r\n" ); + for (int a = 0; a < len; a++) { + char t[5]; + sprintf(t, "%2.2X ", *(addr+a)); + strcat( ascii_out, t ); + if (!((a+1)&0xf)){ + strcat( ascii_out, "\r\n" ); + s->send_serial_data_wait((unsigned char *)ascii_out, strlen(ascii_out)); + ascii_out[0] = 0; + } + } + strcat( ascii_out, "\r\n" ); + return 1; +} + + + +///////////////////////////////////////////// +// single byte commands at start of command +// - i.e. only after CR of LF and ascii buffer empty +int main_ascii_init(PROTOCOL_STAT *s){ + + ascii_add_immediate( 'W', immediate_dir, "Faster" ); + ascii_add_immediate( 'S', immediate_dir, "Slower"); + ascii_add_immediate( 'D', immediate_dir, "Lefter"); + ascii_add_immediate( 'A', immediate_dir, "Righter"); + ascii_add_immediate( 'X', immediate_stop, "DisableDrive"); + ascii_add_immediate( 'Q', immediate_quit, "Quit Immediate"); + ascii_add_immediate( 'H', immediate_hall, "display hall data"); + ascii_add_immediate( 'N', immediate_sensors, "display sensor data"); + ascii_add_immediate( 'C', immediate_electrical, "display electrical measurements"); + ascii_add_immediate( 'G', immediate_stm32, "display stm32 specific"); + ascii_add_immediate( 'O', immediate_toggle_control, "toggle control mode"); + + + ascii_add_line_fn( 'A', line_set_alarm, "set alarm"); + ascii_add_line_fn( 'C', line_electrical, "show electrical measurements"); + ascii_add_line_fn( 'S', line_main_timing_stats, "show main loop timing stats"); + + ascii_add_line_fn( 'R', line_reset_firmware, " - R! -> Reset Firmware"); + ascii_add_line_fn( 'T', line_test_message, "tt - send a test protocol message "); + ascii_add_line_fn( 'P', line_poweroff_control, " P -power control\r\n" + " P -disablepoweroff\r\n" + " PE enable poweroff\r\n" + " Pn power off in n seconds\r\n" + " Pr software reset\r\n" ); + + ascii_add_line_fn( 'M', line_read_memory, " M - dump memory\r\n" + " Mf - dump flash\r\n" + " M, - dump mem\r\n"); + ascii_add_line_fn( 'N', line_sensors, "display sensor data"); + ascii_add_line_fn( 'I', line_immediate, "enable immediate commands - Ip/Is/Iw - direct to posn/speed/pwm control\r\n"); + ascii_add_line_fn( 'G', line_stm32, "display stm32 specific"); + + ascii_add_line_fn( 'F', line_generic_var, get_F_description(s)); + + return 1; +} +///////////////////////////////////////////// + diff --git a/Src/bipropellantProtocolMachine.c b/Src/bipropellantProtocolMachine.c new file mode 100644 index 00000000..142b1ae0 --- /dev/null +++ b/Src/bipropellantProtocolMachine.c @@ -0,0 +1,256 @@ +#include "defines.h" +#include "config.h" +#include "bipropellantProtocolMachine.h" + +#include "comms.h" +#include "util.h" +#include "stm32f1xx_hal.h" +#include "bldc.h" +#include "setup.h" + +#include +#include + +#include "bipropellantProtocolStructs.h" + +#if defined(USART2_ENABLE) + PROTOCOL_STAT sUSART2; +#endif +#if defined(USART3_ENABLE) + PROTOCOL_STAT sUSART3; +#endif + +extern uint32_t timeoutCnt; + +extern int main_ascii_init(PROTOCOL_STAT *s); // from ascii_proto_funcs.c + + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x09 enable + +static char protocol_enable = 0; + +void fn_enable ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { + switch (cmd) { + case PROTOCOL_CMD_READVAL: + case PROTOCOL_CMD_SILENTREAD: + protocol_enable = bldc_getMotorsEnable(); + break; + + case PROTOCOL_CMD_WRITEVAL: + case PROTOCOL_CMD_READVALRESPONSE: + if ( bldc_getMotorsEnable() != protocol_enable) { + // clear speeds to zero every time it is changed + PWMData.pwm[0] = 0; + PWMData.pwm[1] = 0; + } + bldc_setMotorsEnable(protocol_enable); + break; + } + fn_defaultProcessing(s, param, cmd, msg); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x02 HallData + +/* see hallinterrupts.h */ + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x03 SpeedData + +PROTOCOL_SPEED_DATA SpeedData = { + {0, 0}, + + 600, // max power (PWM) + -600, // min power + 40 // minimum mm/s which we can ask for +}; + + +void fn_SpeedData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { + switch (cmd) { + case PROTOCOL_CMD_WRITEVAL: + case PROTOCOL_CMD_READVALRESPONSE: + break; + } + fn_defaultProcessing(s, param, cmd, msg); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x0D PWMData and 0x0E PWMData.pwm + +PROTOCOL_PWM_DATA PWMData = { + .pwm[0] = 0, + .pwm[1] = 0, + .speed_max_power = 600, + .speed_min_power = -600, + .speed_minimum_pwm = 40 // guard value, below this set to zero +}; + +extern int pwms[2]; + +void fn_PWMData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { + switch (cmd) { + case PROTOCOL_CMD_WRITEVAL: + case PROTOCOL_CMD_READVALRESPONSE: + ctrlModReq = 1; // 1 = VOLTAGE mode (default), 2 = SPEED mode, 3 = TORQUE mode. + timeoutCnt = 0; + break; + } + + fn_defaultProcessing(s, param, cmd, msg); + + switch (cmd) { + case PROTOCOL_CMD_WRITEVAL: + case PROTOCOL_CMD_READVALRESPONSE: + for (int i = 0; i < 2; i++) { + if (((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] > ((PROTOCOL_PWM_DATA*) (param->ptr))->speed_max_power) { + ((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] = ((PROTOCOL_PWM_DATA*) (param->ptr))->speed_max_power; + } + if (((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] < ((PROTOCOL_PWM_DATA*) (param->ptr))->speed_min_power) { + ((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] = ((PROTOCOL_PWM_DATA*) (param->ptr))->speed_min_power; + } + if ((((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] > 0) && (((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] < ((PROTOCOL_PWM_DATA*) (param->ptr))->speed_minimum_pwm)) { + ((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] = 0; + } + if ((((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] < 0) && (((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] > -((PROTOCOL_PWM_DATA*) (param->ptr))->speed_minimum_pwm)) { + ((PROTOCOL_PWM_DATA*) (param->ptr))->pwm[i] = 0; + } + } + break; + } +} + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x21 BuzzerData + +PROTOCOL_BUZZER_DATA BuzzerData = { + .buzzerFreq = 0, + .buzzerPattern = 0, + .buzzerLen = 0, +}; + +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... + +void fn_BuzzerData ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { + switch (cmd) { + case PROTOCOL_CMD_READVAL: + case PROTOCOL_CMD_SILENTREAD: + /* + ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerFreq = buzzerFreq; + ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerLen = buzzerLen; + ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerPattern = buzzerPattern; + */ + break; + } + + fn_defaultProcessing(s, param, cmd, msg); + + switch (cmd) { + case PROTOCOL_CMD_WRITEVAL: + case PROTOCOL_CMD_READVALRESPONSE: + /* + buzzerFreq = ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerFreq; + buzzerLen = ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerLen; + buzzerPattern = ((PROTOCOL_BUZZER_DATA*) (param->ptr))->buzzerPattern; + */ + break; + } +} + + +//////////////////////////////////////////////////////////////////////////////////////////// +// Variable & Functions for 0x08 Electrical Params + +extern int16_t curDC_max; +extern int16_t curL_DC; +extern int16_t curR_DC; + +void fn_electrictalParams ( PROTOCOL_STAT *s, PARAMSTAT *param, unsigned char cmd, PROTOCOL_MSG3full *msg ) { + switch (cmd) { + case PROTOCOL_CMD_READVAL: + case PROTOCOL_CMD_SILENTREAD: + if(param != NULL && param->ptr != NULL && param->len == sizeof(ELECTRICAL_PARAMS)) + { + ELECTRICAL_PARAMS *measured = (ELECTRICAL_PARAMS*) param->ptr; + memset(measured, 0, sizeof(ELECTRICAL_PARAMS)); + measured->bat_raw = adc_buffer.batt1; + measured->batteryVoltage = (float)batVoltageFixdt / (float)0xFFFF; + measured->board_temp_raw = adc_buffer.temp; + measured->dcCurLim = curDC_max; + measured->motors[0].dcAmps = curL_DC; + measured->motors[1].dcAmps = curR_DC; + } + break; + } + fn_defaultProcessingReadOnly(s, param, cmd, msg); +} + + +//////////////////////////////////////////////////////////////////////////////////////////// +// initialize protocol and register functions +int setup_protocol(PROTOCOL_STAT *s) { + + protocol_GetTick = HAL_GetTick; + protocol_Delay = HAL_Delay; + protocol_SystemReset =HAL_NVIC_SystemReset; + + + int errors = 0; + + #if defined(USART2_ENABLE) + + extern int USART2_DMA_send(unsigned char *data, int len); + + errors += protocol_init(&sUSART2); + + sUSART2.send_serial_data=USART2_DMA_send; + sUSART2.send_serial_data_wait=USART2_DMA_send; + sUSART2.timeout1 = 500; + sUSART2.timeout2 = 100; + sUSART2.allow_ascii = 1; + + #endif + + #if defined(USART3_ENABLE) + + extern int USART3_DMA_send(unsigned char *data, int len); + + errors += protocol_init(&sUSART3); + + sUSART3.send_serial_data=USART3_DMA_send; + sUSART3.send_serial_data_wait=USART3_DMA_send; + sUSART3.timeout1 = 500; + sUSART3.timeout2 = 100; + sUSART3.allow_ascii = 1; + + #endif + + // initialise ascii protocol functions + main_ascii_init(s); + + + +// errors += setParamVariable( s, 0x02, UI_NONE, (void *)&HallData, sizeof(HallData) ); + + errors += setParamVariable( s, 0x03, UI_NONE, &SpeedData, sizeof(SpeedData) ); + setParamHandler( s, 0x03, fn_SpeedData ); + + setParamHandler( s, 0x08, fn_electrictalParams ); + + errors += setParamVariable( s, 0x09, UI_CHAR, &protocol_enable, sizeof(protocol_enable) ); + setParamHandler( s, 0x09, fn_enable ); + + errors += setParamVariable( s, 0x0D, UI_NONE, &PWMData, sizeof(PWMData) ); + setParamHandler( s, 0x0D, fn_PWMData ); + + errors += setParamVariable( s, 0x0E, UI_2LONG, &(PWMData.pwm), sizeof(PWMData.pwm) ); + setParamHandler( s, 0x0E, fn_PWMData ); + + errors += setParamVariable( s, 0x21, UI_NONE, &BuzzerData, sizeof(BuzzerData) ); + setParamHandler( s, 0x21, fn_BuzzerData ); + + return errors; + +} diff --git a/Src/bldc.c b/Src/bldc.c index cf0c17c7..bb604f42 100644 --- a/Src/bldc.c +++ b/Src/bldc.c @@ -47,8 +47,7 @@ extern ExtY rtY_Right; /* External outputs */ static int16_t pwm_margin = 110; /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */ -extern uint8_t ctrlModReq; -static int16_t curDC_max = (I_DC_MAX * A2BIT_CONV); +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; @@ -61,7 +60,7 @@ uint8_t buzzerFreq = 0; uint8_t buzzerPattern = 0; static uint32_t buzzerTimer = 0; -uint8_t enable = 0; // initially motors are disabled for SAFETY +static volatile uint8_t m_motorsEnable = 0; // initially motors are disabled for SAFETY static uint8_t enableFin = 0; static const uint16_t pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000 @@ -74,8 +73,19 @@ static int16_t offsetrrC = 2000; static int16_t offsetdcl = 2000; static int16_t offsetdcr = 2000; -int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; -static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 16; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point +int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; +int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 16; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point + + +uint8_t bldc_getMotorsEnable(void) +{ + return m_motorsEnable; +} + +void bldc_setMotorsEnable(uint8_t enable) +{ + m_motorsEnable = enable; +} // ================================= // DMA interrupt frequency =~ 16 kHz @@ -106,7 +116,7 @@ void DMA1_Channel1_IRQHandler(void) { 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)(offsetrrB - adc_buffer.rrB); curR_phaC = (int16_t)(offsetrrC - adc_buffer.rrC); @@ -114,13 +124,13 @@ void DMA1_Channel1_IRQHandler(void) { // Disable PWM when current limit is reached (current chopping) // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX - if(ABS(curL_DC) > curDC_max || enable == 0) { + if(ABS(curL_DC) > curDC_max || m_motorsEnable == 0) { LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; } else { LEFT_TIM->BDTR |= TIM_BDTR_MOE; } - if(ABS(curR_DC) > curDC_max || enable == 0) { + if(ABS(curR_DC) > curDC_max || m_motorsEnable == 0) { RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; } else { RIGHT_TIM->BDTR |= TIM_BDTR_MOE; @@ -149,9 +159,9 @@ void DMA1_Channel1_IRQHandler(void) { OverrunFlag = true; /* Make sure to stop BOTH motors in case of an error */ - enableFin = enable && !rtY_Left.z_errCode && !rtY_Right.z_errCode; - - // ========================= LEFT MOTOR ============================ + enableFin = m_motorsEnable && !rtY_Left.z_errCode && !rtY_Right.z_errCode; + + // ========================= LEFT MOTOR ============================ // Get hall sensors values uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN); uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN); @@ -159,17 +169,17 @@ void DMA1_Channel1_IRQHandler(void) { /* Set motor inputs here */ rtU_Left.b_motEna = enableFin; - rtU_Left.z_ctrlModReq = ctrlModReq; + rtU_Left.z_ctrlModReq = ctrlModReq; rtU_Left.r_inpTgt = pwml; rtU_Left.b_hallA = hall_ul; rtU_Left.b_hallB = hall_vl; rtU_Left.b_hallC = hall_wl; rtU_Left.i_phaAB = curL_phaA; rtU_Left.i_phaBC = curL_phaB; - rtU_Left.i_DCLink = curL_DC; - + rtU_Left.i_DCLink = curL_DC; + /* Step the controller */ - #ifdef MOTOR_LEFT_ENA + #ifdef MOTOR_LEFT_ENA BLDC_controller_step(rtM_Left); #endif @@ -186,9 +196,9 @@ void DMA1_Channel1_IRQHandler(void) { LEFT_TIM->LEFT_TIM_V = (uint16_t)CLAMP(vl + pwm_res / 2, pwm_margin, pwm_res-pwm_margin); LEFT_TIM->LEFT_TIM_W = (uint16_t)CLAMP(wl + pwm_res / 2, pwm_margin, pwm_res-pwm_margin); // ================================================================= - - // ========================= RIGHT MOTOR =========================== + + // ========================= RIGHT MOTOR =========================== // Get hall sensors values uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN); uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN); @@ -226,7 +236,7 @@ void DMA1_Channel1_IRQHandler(void) { /* Indicate task complete */ OverrunFlag = false; - + // ############################################################################### } diff --git a/Src/comms.c b/Src/comms.c index 61ee47db..155be2bf 100644 --- a/Src/comms.c +++ b/Src/comms.c @@ -5,19 +5,24 @@ #include "setup.h" #include "config.h" #include "comms.h" +#include "bipropellantProtocolMachine.h" extern UART_HandleTypeDef huart2; extern UART_HandleTypeDef huart3; static volatile uint8_t uart_buf[100]; static volatile int16_t ch_buf[8]; -//volatile char char_buf[300]; + +uint8_t enablescope = 1; void setScopeChannel(uint8_t ch, int16_t val) { ch_buf[ch] = val; } void consoleScope(void) { + if (!enablescope) + return; + #if defined DEBUG_SERIAL_SERVOTERM && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3) uart_buf[0] = 0xff; uart_buf[1] = CLAMP(ch_buf[0]+127, 0, 255); @@ -32,12 +37,12 @@ void consoleScope(void) { #ifdef DEBUG_SERIAL_USART2 if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength); + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength); } #endif #ifdef DEBUG_SERIAL_USART3 if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength); + HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength); } #endif #endif @@ -48,15 +53,15 @@ void consoleScope(void) { strLength = sprintf((char *)(uintptr_t)uart_buf, "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]); - + #ifdef DEBUG_SERIAL_USART2 if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength); + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)uart_buf, strLength); } - #endif + #endif #ifdef DEBUG_SERIAL_USART3 if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength); + HAL_UART_Transmit_DMA(&huart3, (uint8_t *)uart_buf, strLength); } #endif #endif @@ -69,13 +74,39 @@ void consoleLog(char *message) #if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3) #ifdef DEBUG_SERIAL_USART2 if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart2, (uint8_t *)message, strlen((char *)(uintptr_t)message)); + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)message, strlen((char *)(uintptr_t)message)); } #endif #ifdef DEBUG_SERIAL_USART3 if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 0) { - HAL_UART_Transmit_DMA(&huart3, (uint8_t *)message, strlen((char *)(uintptr_t)message)); + HAL_UART_Transmit_DMA(&huart3, (uint8_t *)message, strlen((char *)(uintptr_t)message)); } #endif #endif + + #if defined(VARIANT_BIPROPELLANT) + #if defined(USART2_ENABLE) + protocol_send_text(&sUSART2, message, PROTOCOL_SOM_NOACK); + #endif + + #if defined(USART3_ENABLE) + protocol_send_text(&sUSART3, message, PROTOCOL_SOM_NOACK); + #endif + #endif +} + + +#if defined(USART2_ENABLE) +int USART2_DMA_send(unsigned char *data, int len) { + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)data, len); + return 1; +} +#endif + +#if defined(USART3_ENABLE) +int USART3_DMA_send(unsigned char *data, int len) { + HAL_UART_Transmit_DMA(&huart3, (uint8_t *)data, len); + return 1; } +#endif + diff --git a/Src/main.c b/Src/main.c index 3bef8384..41b0f9a1 100644 --- a/Src/main.c +++ b/Src/main.c @@ -27,8 +27,11 @@ #include "config.h" #include "util.h" #include "comms.h" +#include "bldc.h" #include "BLDC_controller.h" /* BLDC's header file */ #include "rtwtypes.h" +#include "bipropellantProtocolMachine.h" +#include "bipropellantProtocolStructs.h" #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) #include "hd44780.h" @@ -43,7 +46,6 @@ 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) extern LCD_PCF8574_HandleTypeDef lcd; extern uint8_t LCDerrorFlag; @@ -75,8 +77,6 @@ extern volatile int pwmr; // global variable for pwm right. -1000 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 int16_t batVoltage; // global variable for battery voltage #if defined(SIDEBOARD_SERIAL_USART2) @@ -126,12 +126,12 @@ static uint8_t sideboard_leds_R; #ifdef VARIANT_TRANSPOTTER extern uint8_t nunchuk_connected; - extern float setDistance; + extern float setDistance; static uint8_t checkRemote = 0; static uint16_t distance; static float steering; - static int distanceErr; + static int distanceErr; static int lastDistance = 0; static uint16_t transpotter_counter = 0; #endif @@ -184,7 +184,7 @@ int main(void) { HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET); HAL_ADC_Start(&hadc1); - HAL_ADC_Start(&hadc2); + HAL_ADC_Start(&hadc2); poweronMelody(); HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET); @@ -203,19 +203,39 @@ int main(void) { readCommand(); // Read Command: cmd1, cmd2 calcAvgSpeed(); // Calculate average measured speed: speedAvg, speedAvgAbs + #if defined(VARIANT_BIPROPELLANT) + #if defined(USART2_ENABLE) + protocol_tick( &sUSART2 ); + if(sUSART2.ascii.enable_immediate) + { + timeout = 0; + } + #endif + #if defined(USART3_ENABLE) + protocol_tick( &sUSART3 ); + if(sUSART3.ascii.enable_immediate) + { + timeoutCnt = 0; + } + #endif + cmd2 = (PWMData.pwm[0] + PWMData.pwm[1]) /2; // Speed + cmd1 = PWMData.pwm[0] - cmd2; // Steer + + #endif + #ifndef VARIANT_TRANSPOTTER // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) ####### - if (enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (cmd1 > -50 && cmd1 < 50) && (cmd2 > -50 && cmd2 < 50)){ + if (bldc_getMotorsEnable() == 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 + bldc_setMotorsEnable(1); // enable motors consoleLog("-- Motors enabled --\r\n"); } - // ####### VARIANT_HOVERCAR ####### + // ####### VARIANT_HOVERCAR ####### #ifdef VARIANT_HOVERCAR // Calculate speed Blend, a number between [0, 1] in fixdt(0,16,15) - uint16_t speedBlend; + uint16_t speedBlend; speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm] // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality @@ -223,16 +243,16 @@ int main(void) { multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBreak); // Break pedal in this case is "cmd1" variable } - // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving + // If Brake pedal (cmd1) is pressed, bring to 0 also the Throttle pedal (cmd2) to avoid "Double pedal" driving if (cmd1 > 20) { cmd2 = (int16_t)((cmd2 * speedBlend) >> 15); } - // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal) + // Make sure the Brake pedal is opposite to the direction of motion AND it goes to 0 as we reach standstill (to avoid Reverse driving by Brake pedal) if (speedAvg > 0) { cmd1 = (int16_t)((-cmd1 * speedBlend) >> 15); } else { - cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); + cmd1 = (int16_t)(( cmd1 * speedBlend) >> 15); } #endif @@ -248,12 +268,12 @@ int main(void) { filtLowPass32(steerRateFixdt >> 4, FILTER, &steerFixdt); filtLowPass32(speedRateFixdt >> 4, FILTER, &speedFixdt); steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer - speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer + speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to integer // ####### VARIANT_HOVERCAR ####### - #ifdef VARIANT_HOVERCAR + #ifdef VARIANT_HOVERCAR if (!MultipleTapBreak.b_multipleTap) { // Check driving direction - speed = steer + speed; // Forward driving + speed = steer + speed; // Forward driving } else { speed = steer - speed; // Reverse driving } @@ -289,7 +309,7 @@ int main(void) { 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; + bldc_setMotorsEnable(1); } if (distanceErr > -300) { #ifdef INVERT_R_DIRECTION @@ -307,11 +327,11 @@ int main(void) { if (!HAL_GPIO_ReadPin(LED_PORT, LED_PIN)) { //enable = 1; } else { - enable = 0; + bldc_setMotorsEnable(0); } } } else { - enable = 0; + bldc_setMotorsEnable(0); } } timeoutCnt = 0; @@ -320,7 +340,7 @@ int main(void) { if (timeoutCnt > TIMEOUT) { pwml = 0; pwmr = 0; - enable = 0; + bldc_setMotorsEnable(0); #ifdef SUPPORT_LCD LCD_SetLocation(&lcd, 0, 0); LCD_WriteString(&lcd, "Len:"); LCD_SetLocation(&lcd, 8, 0); LCD_WriteString(&lcd, "m("); @@ -331,7 +351,7 @@ int main(void) { } if ((distance / 1345.0) - setDistance > 0.5 && (lastDistance / 1345.0) - setDistance > 0.5) { // Error, robot too far away! - enable = 0; + bldc_setMotorsEnable(0); longBeep(5); #ifdef SUPPORT_LCD LCD_ClearDisplay(&lcd); @@ -356,12 +376,12 @@ int main(void) { nunchuk_connected = 1; } } - } + } #endif #ifdef SUPPORT_LCD if (transpotter_counter % 100 == 0) { - if (LCDerrorFlag == 1 && enable == 0) { + if (LCDerrorFlag == 1 && bldc_getMotorsEnable() == 0) { } else { if (nunchuk_connected == 0) { @@ -430,7 +450,7 @@ int main(void) { #if defined(FEEDBACK_SERIAL_USART2) if(__HAL_DMA_GET_COUNTER(huart2.hdmatx) == 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.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Feedback, sizeof(Feedback)); @@ -439,12 +459,12 @@ int main(void) { #if defined(FEEDBACK_SERIAL_USART3) if(__HAL_DMA_GET_COUNTER(huart3.hdmatx) == 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.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.cmdLed); HAL_UART_Transmit_DMA(&huart3, (uint8_t *)&Feedback, sizeof(Feedback)); } - #endif + #endif } #endif @@ -455,7 +475,7 @@ int main(void) { 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; + bldc_setMotorsEnable(0); buzzerFreq = 8; buzzerPattern = 1; } else if (timeoutFlagADC || timeoutFlagSerial || timeoutCnt > TIMEOUT) { // beep in case of ADC timeout, Serial timeout or General timeout - fast beep diff --git a/Src/setup.c b/Src/setup.c index d859c9e6..3d703361 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -54,20 +54,20 @@ DMA_HandleTypeDef hdma_usart3_tx; volatile adc_buf_t adc_buffer; -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) +#if defined(USART2_ENABLE) /* USART2 init function */ void UART2_Init(void) { /* DMA controller clock enable */ __HAL_RCC_DMA1_CLK_ENABLE(); - + /* DMA1_Channel6_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn); /* DMA1_Channel7_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn); - + huart2.Instance = USART2; huart2.Init.BaudRate = USART2_BAUD; huart2.Init.WordLength = USART2_WORDLENGTH; @@ -80,7 +80,7 @@ volatile adc_buf_t adc_buffer; } #endif -#if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART3_ENABLE) || defined(FEEDBACK_SERIAL_USART3) /* USART3 init function */ void UART3_Init(void) { @@ -94,7 +94,7 @@ void UART3_Init(void) /* DMA1_Channel3_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn); - + huart3.Instance = USART3; huart3.Init.BaudRate = USART3_BAUD; huart3.Init.WordLength = USART3_WORDLENGTH; @@ -107,8 +107,7 @@ void UART3_Init(void) } #endif -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || \ - defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART2_ENABLE) || defined(USART3_ENABLE) void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) { GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -119,11 +118,11 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART2_MspInit 0 */ /* USART2 clock enable */ __HAL_RCC_USART2_CLK_ENABLE(); - + __HAL_RCC_GPIOA_CLK_ENABLE(); - /**USART2 GPIO Configuration + /**USART2 GPIO Configuration PA2 ------> USART2_TX - PA3 ------> USART2_RX + PA3 ------> USART2_RX */ GPIO_InitStruct.Pin = GPIO_PIN_2; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -174,11 +173,11 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART3_MspInit 0 */ /* USART3 clock enable */ __HAL_RCC_USART3_CLK_ENABLE(); - + __HAL_RCC_GPIOB_CLK_ENABLE(); - /**USART3 GPIO Configuration + /**USART3 GPIO Configuration PB10 ------> USART3_TX - PB11 ------> USART3_RX + PB11 ------> USART3_RX */ GPIO_InitStruct.Pin = GPIO_PIN_10; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -234,10 +233,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART2_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_USART2_CLK_DISABLE(); - - /**USART2 GPIO Configuration + + /**USART2 GPIO Configuration PA2 ------> USART2_TX - PA3 ------> USART2_RX + PA3 ------> USART2_RX */ HAL_GPIO_DeInit(GPIOA, GPIO_PIN_2|GPIO_PIN_3); @@ -258,10 +257,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART3_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_USART3_CLK_DISABLE(); - - /**USART3 GPIO Configuration + + /**USART3 GPIO Configuration PB10 ------> USART3_TX - PB11 ------> USART3_RX + PB11 ------> USART3_RX */ HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10|GPIO_PIN_11); @@ -275,7 +274,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART3_MspDeInit 1 */ } -} +} #endif DMA_HandleTypeDef hdma_i2c2_rx; @@ -397,7 +396,7 @@ void MX_GPIO_Init(void) { GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Pin = CHARGER_PIN; HAL_GPIO_Init(CHARGER_PORT, &GPIO_InitStruct); - + GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Pin = BUTTON_PIN; @@ -575,7 +574,7 @@ void MX_TIM_Init(void) { HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_3); HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_1); HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2); @@ -674,7 +673,7 @@ void MX_ADC2_Init(void) { hadc2.Init.NbrOfConversion = 5; HAL_ADC_Init(&hadc2); - + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; sConfig.Channel = ADC_CHANNEL_10; // pc0 right cur -> left sConfig.Rank = 1; diff --git a/Src/stm32f1xx_it.c b/Src/stm32f1xx_it.c index 278fcca7..eb4936b4 100644 --- a/Src/stm32f1xx_it.c +++ b/Src/stm32f1xx_it.c @@ -236,7 +236,7 @@ void DMA1_Channel5_IRQHandler(void) void EXTI3_IRQHandler(void) { __HAL_GPIO_EXTI_CLEAR_IT(PPM_PIN); - PPM_ISR_Callback(); + PPM_ISR_Callback(); } #endif #ifdef CONTROL_PPM_RIGHT @@ -251,7 +251,7 @@ void EXTI15_10_IRQHandler(void) #ifdef CONTROL_PWM_LEFT void EXTI2_IRQHandler(void) -{ +{ __HAL_GPIO_EXTI_CLEAR_IT(PWM_PIN_CH1); PWM_ISR_CH1_Callback(); } @@ -259,7 +259,7 @@ void EXTI2_IRQHandler(void) void EXTI3_IRQHandler(void) { __HAL_GPIO_EXTI_CLEAR_IT(PWM_PIN_CH2); - PWM_ISR_CH2_Callback(); + PWM_ISR_CH2_Callback(); } #endif #ifdef CONTROL_PWM_RIGHT @@ -276,7 +276,7 @@ void EXTI15_10_IRQHandler(void) } #endif -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) +#if defined(USART2_ENABLE) void DMA1_Channel6_IRQHandler(void) { /* USER CODE BEGIN DMA1_Channel4_IRQn 0 */ @@ -303,7 +303,7 @@ void DMA1_Channel7_IRQHandler(void) } #endif -#if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART3_ENABLE) || defined(FEEDBACK_SERIAL_USART3) /** * @brief This function handles DMA1 channel2 global interrupt. */ @@ -333,7 +333,7 @@ void DMA1_Channel3_IRQHandler(void) } #endif -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) +#if defined(USART2_ENABLE) /** * @brief This function handles USART2 global interrupt. */ @@ -352,7 +352,7 @@ void USART2_IRQHandler(void) } #endif -#if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART3_ENABLE) || defined(FEEDBACK_SERIAL_USART3) /** * @brief This function handles USART3 global interrupt. */ @@ -363,7 +363,7 @@ void USART3_IRQHandler(void) /* USER CODE END USART2_IRQn 0 */ HAL_UART_IRQHandler(&huart3); /* USER CODE BEGIN USART2_IRQn 1 */ - if(RESET != __HAL_UART_GET_IT_SOURCE(&huart3, UART_IT_IDLE)) { // Check for IDLE line interrupt + if(RESET != __HAL_UART_GET_IT_SOURCE(&huart3, UART_IT_IDLE)) { // Check for IDLE line interrupt __HAL_UART_CLEAR_IDLEFLAG(&huart3); // Clear IDLE line flag (otherwise it will continue to enter interrupt) usart3_rx_check(); // Check for data to process } diff --git a/Src/util.c b/Src/util.c index b299e102..709fa8b3 100644 --- a/Src/util.c +++ b/Src/util.c @@ -27,8 +27,10 @@ #include "comms.h" #include "eeprom.h" #include "util.h" +#include "bldc.h" #include "BLDC_controller.h" #include "rtwtypes.h" +#include "bipropellantProtocolMachine.h" #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) #include "hd44780.h" @@ -39,18 +41,14 @@ //------------------------------------------------------------------------ // Global variables set externally //------------------------------------------------------------------------ -extern volatile adc_buf_t adc_buffer; extern I2C_HandleTypeDef hi2c2; extern UART_HandleTypeDef huart2; extern UART_HandleTypeDef huart3; -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 timeoutCnt; // global variable for general timeout counter extern volatile uint32_t main_loop_counter; @@ -100,7 +98,7 @@ uint8_t timeoutFlagADC = 0; // Timeout Flag for ADC Protection: 0 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 +uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request #if defined(DEBUG_I2C_LCD) || defined(SUPPORT_LCD) LCD_PCF8574_HandleTypeDef lcd; @@ -153,7 +151,7 @@ static int16_t INPUT_MIN; // [-] Input target minimum limitation static int16_t timeoutCntADC = 0; // Timeout counter for ADC Protection #endif -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) +#if defined(USART2_ENABLE) static uint8_t rx_buffer_L[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer static uint32_t rx_buffer_L_len = ARRAY_LEN(rx_buffer_L); #endif @@ -167,7 +165,7 @@ SerialSideboard Sideboard_L_raw; static uint32_t Sideboard_L_len = sizeof(Sideboard_L); #endif -#if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART3_ENABLE) static uint8_t rx_buffer_R[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer static uint32_t rx_buffer_R_len = ARRAY_LEN(rx_buffer_R); #endif @@ -210,13 +208,13 @@ static uint8_t brakePressed; /* =========================== Initialization Functions =========================== */ void BLDC_Init(void) { - /* Set BLDC controller parameters */ + /* 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.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.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) @@ -242,7 +240,7 @@ void BLDC_Init(void) { BLDC_controller_initialize(rtM_Right); } -void Input_Lim_Init(void) { // Input Limitations - ! Do NOT touch ! +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); @@ -266,26 +264,31 @@ void Input_Init(void) { Nunchuk_Init(); #endif - #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) - UART2_Init(); - #endif - #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) - UART3_Init(); + #if defined(VARIANT_BIPROPELLANT) + #if defined(USART2_ENABLE) + setup_protocol(&sUSART2); + #endif + #if defined(USART3_ENABLE) + setup_protocol(&sUSART3); + #endif #endif - #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) + + #if defined(USART2_ENABLE) + UART2_Init(); HAL_UART_Receive_DMA(&huart2, (uint8_t *)rx_buffer_L, sizeof(rx_buffer_L)); UART_DisableRxErrors(&huart2); #endif - #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) + #if defined(USART3_ENABLE) + UART3_Init(); HAL_UART_Receive_DMA(&huart3, (uint8_t *)rx_buffer_R, sizeof(rx_buffer_R)); UART_DisableRxErrors(&huart3); #endif - #ifdef CONTROL_ADC + #ifdef CONTROL_ADC uint16_t writeCheck, i_max, n_max; - HAL_FLASH_Unlock(); - EE_Init(); /* EEPROM Init */ + HAL_FLASH_Unlock(); + EE_Init(); /* EEPROM Init */ EE_ReadVariable(VirtAddVarTab[0], &writeCheck); if (writeCheck == FLASH_WRITE_KEY) { EE_ReadVariable(VirtAddVarTab[1], &ADC1_MIN_CAL); @@ -305,10 +308,10 @@ void Input_Init(void) { #endif - #ifdef VARIANT_TRANSPOTTER - enable = 1; - - HAL_FLASH_Unlock(); + #ifdef VARIANT_TRANSPOTTER + bldc_setMotorsEnable(1); + + HAL_FLASH_Unlock(); EE_Init(); /* EEPROM Init */ EE_ReadVariable(VirtAddVarTab[0], &saveValue); HAL_FLASH_Lock(); @@ -362,8 +365,7 @@ void Input_Init(void) { * @param huart: UART handle. * @retval None */ -#if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) || \ - defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART2_ENABLE) || defined(USART3_ENABLE) void UART_DisableRxErrors(UART_HandleTypeDef *huart) { /* Disable PE (Parity Error) interrupts */ @@ -374,6 +376,121 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart) } #endif +/* ====================== Private (static) Functions ======================= */ + +#if defined(VARIANT_BIPROPELLANT) + #if defined(USART2_ENABLE) + static void usart_process_bipropellantProtocolUSART2(uint8_t *data, uint32_t len) + { + for (; len > 0; len--, data++) { + protocol_byte( &sUSART2, (unsigned char) *data ); + } + } + #endif + + #if defined(USART3_ENABLE) + static void usart_process_bipropellantProtocolUSART3(uint8_t *data, uint32_t len) + { + for (; len > 0; len--, data++) { + protocol_byte( &sUSART3, (unsigned char) *data ); + } + } + #endif +#endif //VARIANT_BIPROPELLANT + +/* + * Process Rx debug user command input + */ +#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) +static void usart_process_debug(uint8_t *userCommand, uint32_t len) +{ + for (; len > 0; len--, userCommand++) { + if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands + consoleLog("-- Command received --\r\n"); + // handle_input(*userCommand); // -> Create this function to handle the user commands + } + } +} +#endif + +/* + * Process command Rx data + * - if the command_in data is valid (correct START_FRAME and checksum) copy the command_in to command_out + */ +#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) +static void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx) +{ + #ifdef CONTROL_IBUS + if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) { + ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; + for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { + ibus_chksum -= command_in->channels[i]; + } + if (ibus_chksum == (uint16_t)((command_in->checksumh << 8) + command_in->checksuml)) { + *command_out = *command_in; + if (usart_idx == 2) { // Sideboard USART2 + #ifdef CONTROL_SERIAL_USART2 + timeoutCntSerial_L = 0; // Reset timeout counter + timeoutFlagSerial_L = 0; // Clear timeout flag + #endif + } else if (usart_idx == 3) { // Sideboard USART3 + #ifdef CONTROL_SERIAL_USART3 + timeoutCntSerial_R = 0; // Reset timeout counter + timeoutFlagSerial_R = 0; // Clear timeout flag + #endif + } + } + } + #else + uint16_t checksum; + if (command_in->start == SERIAL_START_FRAME) { + checksum = (uint16_t)(command_in->start ^ command_in->steer ^ command_in->speed); + if (command_in->checksum == checksum) { + *command_out = *command_in; + if (usart_idx == 2) { // Sideboard USART2 + #ifdef CONTROL_SERIAL_USART2 + timeoutCntSerial_L = 0; // Reset timeout counter + timeoutFlagSerial_L = 0; // Clear timeout flag + #endif + } else if (usart_idx == 3) { // Sideboard USART3 + #ifdef CONTROL_SERIAL_USART3 + timeoutCntSerial_R = 0; // Reset timeout counter + timeoutFlagSerial_R = 0; // Clear timeout flag + #endif + } + } + } + #endif +} +#endif + +/* + * Process Sideboard Rx data + * - if the Sideboard_in data is valid (correct START_FRAME and checksum) copy the Sideboard_in to Sideboard_out + */ +#if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) +static void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sideboard_out, uint8_t usart_idx) +{ + uint16_t checksum; + if (Sideboard_in->start == SERIAL_START_FRAME) { + checksum = (uint16_t)(Sideboard_in->start ^ Sideboard_in->roll ^ Sideboard_in->pitch ^ Sideboard_in->yaw ^ Sideboard_in->sensors); + if (Sideboard_in->checksum == checksum) { + *Sideboard_out = *Sideboard_in; + if (usart_idx == 2) { // Sideboard USART2 + #ifdef SIDEBOARD_SERIAL_USART2 + timeoutCntSerial_L = 0; // Reset timeout counter + timeoutFlagSerial_L = 0; // Clear timeout flag + #endif + } else if (usart_idx == 3) { // Sideboard USART3 + #ifdef SIDEBOARD_SERIAL_USART3 + timeoutCntSerial_R = 0; // Reset timeout counter + timeoutFlagSerial_R = 0; // Clear timeout flag + #endif + } + } + } +} +#endif /* =========================== General Functions =========================== */ @@ -418,7 +535,7 @@ void calcAvgSpeed(void) { // 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); } @@ -437,7 +554,7 @@ void adcCalibLim(void) { } #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; @@ -448,7 +565,7 @@ void adcCalibLim(void) { 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 @@ -458,16 +575,16 @@ void adcCalibLim(void) { 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); + 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); + ADC2_MAX_temp = MAX(ADC2_MAX_temp, ADC2_MID_temp); adc_cal_timeout++; HAL_Delay(5); } - // ADC calibration checks + // ADC calibration checks #ifdef ADC_PROTECT_ENA - if ((ADC1_MIN_temp + 150 - ADC_PROTECT_THRESH) > 0 && (ADC1_MAX_temp - 150 + ADC_PROTECT_THRESH) < 4095 && + 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 { @@ -480,10 +597,10 @@ void adcCalibLim(void) { 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; + 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; + ADC2_MAX_CAL = ADC2_MAX_temp - 150; consoleLog("OK\n"); } else { adc_cal_valid = 0; @@ -519,11 +636,11 @@ void updateCurSpdLim(void) { filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt); filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt); cur_spd_timeout++; - HAL_Delay(5); + 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 + 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 @@ -564,7 +681,7 @@ void saveConfig() { EE_WriteVariable(VirtAddVarTab[8], rtP_Left.n_max); HAL_FLASH_Lock(); } - #endif + #endif } /* @@ -593,7 +710,7 @@ int addDeadBand(int16_t u, int16_t deadBand, int16_t min, int16_t max) { void poweroff(void) { buzzerPattern = 0; - enable = 0; + bldc_setMotorsEnable(0); consoleLog("-- Motors disabled --\r\n"); for (int i = 0; i < 8; i++) { buzzerFreq = (uint8_t)i; @@ -608,21 +725,21 @@ void poweroff(void) { void poweroffPressCheck(void) { #if defined(CONTROL_ADC) if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; + bldc_setMotorsEnable(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) { shortBeep(5); } } if (cnt_press >= 5 * 100) { // Check if press is more than 5 sec - HAL_Delay(300); + 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); } + while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } longBeep(8); updateCurSpdLim(); shortBeep(5); } else { // Long press: Calibrate ADC Limits - longBeep(16); + longBeep(16); adcCalibLim(); shortBeep(5); } @@ -632,7 +749,7 @@ void poweroffPressCheck(void) { } #elif defined(VARIANT_TRANSPOTTER) if(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; + bldc_setMotorsEnable(0); while(HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { HAL_Delay(10); } shortBeep(5); HAL_Delay(300); @@ -653,7 +770,7 @@ void poweroffPressCheck(void) { } #else if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - enable = 0; // disable motors + bldc_setMotorsEnable(0); while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released poweroff(); // release power-latch } @@ -671,7 +788,7 @@ void readCommand(void) { 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; @@ -704,27 +821,27 @@ void readCommand(void) { #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_MIN / (ADC1_MID_CAL - ADC1_MIN_CAL), INPUT_MIN, 0); // ADC1 + 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_MIN / (ADC1_MID_CAL - ADC1_MIN_CAL), INPUT_MIN, 0); // 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_MIN / (ADC2_MID_CAL - ADC2_MIN_CAL), INPUT_MIN, 0); // ADC2 + 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_MIN / (ADC2_MID_CAL - ADC2_MIN_CAL), INPUT_MIN, 0); // 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) && + 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 (timeoutFlagADC) { // Check for previous timeout flag if (timeoutCntADC-- <= 0) // Timeout de-qualification - timeoutFlagADC = 0; // Timeout flag cleared + timeoutFlagADC = 0; // Timeout flag cleared } else { - timeoutCntADC = 0; // Reset the timeout counter + timeoutCntADC = 0; // Reset the timeout counter } } else { if (timeoutCntADC++ >= ADC_PROTECT_TIMEOUT) { // Timeout qualification @@ -739,7 +856,7 @@ void readCommand(void) { cmd2 = 0; } else { ctrlModReq = ctrlModReqRaw; // Follow the Mode request - } + } #endif #if defined(SUPPORT_BUTTONS_LEFT) || defined(SUPPORT_BUTTONS_RIGHT) @@ -751,12 +868,12 @@ void readCommand(void) { #if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) // Handle received data validity, timeout and fix out-of-sync if necessary - #ifdef CONTROL_IBUS + #ifdef CONTROL_IBUS 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] - 500) * 2, INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP((ibus_captured_value[1] - 500) * 2, INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP((ibus_captured_value[1] - 500) * 2, INPUT_MIN, INPUT_MAX); #else if (IN_RANGE(command.steer, INPUT_MIN, INPUT_MAX) && IN_RANGE(command.speed, INPUT_MIN, INPUT_MAX)) { cmd1 = command.steer; @@ -797,7 +914,7 @@ void readCommand(void) { timeoutFlagSerial = timeoutFlagSerial_L || timeoutFlagSerial_R; #endif - #ifdef VARIANT_HOVERCAR + #ifdef VARIANT_HOVERCAR brakePressed = (uint8_t)(cmd1 > 50); #endif @@ -821,27 +938,39 @@ void readCommand(void) { */ void usart2_rx_check(void) { - #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) +#if defined(USART2_ENABLE) static uint32_t old_pos; uint32_t pos; pos = rx_buffer_L_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer - #endif #if defined(DEBUG_SERIAL_USART2) - if (pos != old_pos) { // Check change in received data + if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_L[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer - if (pos > 0) { // Check and continue with beginning of buffer - usart_process_debug(&rx_buffer_L[0], pos); // Process remaining data + usart_process_debug(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_debug(&rx_buffer_L[0], pos); // Process remaining data + } + } + } + #endif + + #if defined(VARIANT_BIPROPELLANT) + if (pos != old_pos) { // Check change in received data + if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one + usart_process_bipropellantProtocolUSART2(&rx_buffer_L[old_pos], pos - old_pos); // Process data + } else { // "Overflow" buffer mode + usart_process_bipropellantProtocolUSART2(&rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_bipropellantProtocolUSART2(&rx_buffer_L[0], pos); // Process remaining data } } } - #endif // DEBUG_SERIAL_USART2 + #endif #ifdef CONTROL_SERIAL_USART2 - uint8_t *ptr; + uint8_t *ptr; if (pos != old_pos) { // Check change in received data ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length @@ -850,7 +979,7 @@ void usart2_rx_check(void) } else if ((rx_buffer_L_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer - ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw + ptr += rx_buffer_L_len - old_pos; // Move to correct position in command_raw memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data } usart_process_command(&command_raw, &command, 2); // Process data @@ -859,7 +988,7 @@ void usart2_rx_check(void) #endif // CONTROL_SERIAL_USART2 #ifdef SIDEBOARD_SERIAL_USART2 - uint8_t *ptr; + uint8_t *ptr; if (pos != old_pos) { // Check change in received data ptr = (uint8_t *)&Sideboard_L_raw; // Initialize the pointer with Sideboard_raw address if (pos > old_pos && (pos - old_pos) == Sideboard_L_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length @@ -868,7 +997,7 @@ void usart2_rx_check(void) } else if ((rx_buffer_L_len - old_pos + pos) == Sideboard_L_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_L[old_pos], rx_buffer_L_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer - ptr += rx_buffer_L_len - old_pos; // Move to correct position in Sideboard_raw + ptr += rx_buffer_L_len - old_pos; // Move to correct position in Sideboard_raw memcpy(ptr, &rx_buffer_L[0], pos); // Copy remaining data } usart_process_sideboard(&Sideboard_L_raw, &Sideboard_L, 2); // Process data @@ -876,12 +1005,11 @@ void usart2_rx_check(void) } #endif // SIDEBOARD_SERIAL_USART2 - #if defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART2) old_pos = pos; // Update old position if (old_pos == rx_buffer_L_len) { // Check and manually update if we reached end of buffer old_pos = 0; } - #endif +#endif // USART2_ENABLE } @@ -891,27 +1019,39 @@ void usart2_rx_check(void) */ void usart3_rx_check(void) { - #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) +#if defined(USART3_ENABLE) static uint32_t old_pos; - uint32_t pos; + uint32_t pos; pos = rx_buffer_R_len - __HAL_DMA_GET_COUNTER(huart3.hdmarx); // Calculate current position in buffer - #endif #if defined(DEBUG_SERIAL_USART3) - if (pos != old_pos) { // Check change in received data + if (pos != old_pos) { // Check change in received data if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one usart_process_debug(&rx_buffer_R[old_pos], pos - old_pos); // Process data } else { // "Overflow" buffer mode - usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer - if (pos > 0) { // Check and continue with beginning of buffer - usart_process_debug(&rx_buffer_R[0], pos); // Process remaining data + usart_process_debug(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_debug(&rx_buffer_R[0], pos); // Process remaining data } } } - #endif // DEBUG_SERIAL_USART3 + #endif + + #if defined(VARIANT_BIPROPELLANT) + if (pos != old_pos) { // Check change in received data + if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one + usart_process_bipropellantProtocolUSART3(&rx_buffer_R[old_pos], pos - old_pos); // Process data + } else { // "Overflow" buffer mode + usart_process_bipropellantProtocolUSART3(&rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First Process data from the end of buffer + if (pos > 0) { // Check and continue with beginning of buffer + usart_process_bipropellantProtocolUSART3(&rx_buffer_R[0], pos); // Process remaining data + } + } + } + #endif #ifdef CONTROL_SERIAL_USART3 - uint8_t *ptr; + uint8_t *ptr; if (pos != old_pos) { // Check change in received data ptr = (uint8_t *)&command_raw; // Initialize the pointer with command_raw address if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length @@ -920,7 +1060,7 @@ void usart3_rx_check(void) } else if ((rx_buffer_R_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer - ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw + ptr += rx_buffer_R_len - old_pos; // Move to correct position in command_raw memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data } usart_process_command(&command_raw, &command, 3); // Process data @@ -938,7 +1078,7 @@ void usart3_rx_check(void) } else if ((rx_buffer_R_len - old_pos + pos) == Sideboard_R_len) { // "Overflow" buffer mode: check if data length equals expected length memcpy(ptr, &rx_buffer_R[old_pos], rx_buffer_R_len - old_pos); // First copy data from the end of buffer if (pos > 0) { // Check and continue with beginning of buffer - ptr += rx_buffer_R_len - old_pos; // Move to correct position in Sideboard_raw + ptr += rx_buffer_R_len - old_pos; // Move to correct position in Sideboard_raw memcpy(ptr, &rx_buffer_R[0], pos); // Copy remaining data } usart_process_sideboard(&Sideboard_R_raw, &Sideboard_R, 3); // Process data @@ -946,108 +1086,12 @@ void usart3_rx_check(void) } #endif // SIDEBOARD_SERIAL_USART3 - #if defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3) || defined(SIDEBOARD_SERIAL_USART3) old_pos = pos; // Update old position if (old_pos == rx_buffer_R_len) { // Check and manually update if we reached end of buffer old_pos = 0; } - #endif -} - -/* - * Process Rx debug user command input - */ -#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3) -void usart_process_debug(uint8_t *userCommand, uint32_t len) -{ - for (; len > 0; len--, userCommand++) { - if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands - consoleLog("-- Command received --\r\n"); - // handle_input(*userCommand); // -> Create this function to handle the user commands - } - } -} -#endif // SERIAL_DEBUG - -/* - * Process command Rx data - * - if the command_in data is valid (correct START_FRAME and checksum) copy the command_in to command_out - */ -#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3) -void usart_process_command(SerialCommand *command_in, SerialCommand *command_out, uint8_t usart_idx) -{ - #ifdef CONTROL_IBUS - if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) { - ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND; - for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) { - ibus_chksum -= command_in->channels[i]; - } - if (ibus_chksum == (uint16_t)((command_in->checksumh << 8) + command_in->checksuml)) { - *command_out = *command_in; - if (usart_idx == 2) { // Sideboard USART2 - #ifdef CONTROL_SERIAL_USART2 - timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag - #endif - } else if (usart_idx == 3) { // Sideboard USART3 - #ifdef CONTROL_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag - #endif - } - } - } - #else - uint16_t checksum; - if (command_in->start == SERIAL_START_FRAME) { - checksum = (uint16_t)(command_in->start ^ command_in->steer ^ command_in->speed); - if (command_in->checksum == checksum) { - *command_out = *command_in; - if (usart_idx == 2) { // Sideboard USART2 - #ifdef CONTROL_SERIAL_USART2 - timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag - #endif - } else if (usart_idx == 3) { // Sideboard USART3 - #ifdef CONTROL_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag - #endif - } - } - } - #endif -} -#endif - -/* - * Process Sideboard Rx data - * - if the Sideboard_in data is valid (correct START_FRAME and checksum) copy the Sideboard_in to Sideboard_out - */ -#if defined(SIDEBOARD_SERIAL_USART2) || defined(SIDEBOARD_SERIAL_USART3) -void usart_process_sideboard(SerialSideboard *Sideboard_in, SerialSideboard *Sideboard_out, uint8_t usart_idx) -{ - uint16_t checksum; - if (Sideboard_in->start == SERIAL_START_FRAME) { - checksum = (uint16_t)(Sideboard_in->start ^ Sideboard_in->roll ^ Sideboard_in->pitch ^ Sideboard_in->yaw ^ Sideboard_in->sensors); - if (Sideboard_in->checksum == checksum) { - *Sideboard_out = *Sideboard_in; - if (usart_idx == 2) { // Sideboard USART2 - #ifdef SIDEBOARD_SERIAL_USART2 - timeoutCntSerial_L = 0; // Reset timeout counter - timeoutFlagSerial_L = 0; // Clear timeout flag - #endif - } else if (usart_idx == 3) { // Sideboard USART3 - #ifdef SIDEBOARD_SERIAL_USART3 - timeoutCntSerial_R = 0; // Reset timeout counter - timeoutFlagSerial_R = 0; // Clear timeout flag - #endif - } - } - } +#endif // USART3_ENABLE } -#endif - /* =========================== Sideboard Functions =========================== */ @@ -1060,9 +1104,9 @@ void sideboardLeds(uint8_t *leds) { // Enable flag: use LED4 (bottom Blue) // enable == 1, turn on led // enable == 0, blink led - if (enable) { + if (bldc_getMotorsEnable()) { *leds |= LED4_SET; - } else if (!enable && (main_loop_counter % 20 == 0)) { + } else if (!bldc_getMotorsEnable() && (main_loop_counter % 20 == 0)) { *leds ^= LED4_SET; } @@ -1087,7 +1131,7 @@ void sideboardLeds(uint8_t *leds) { // 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; + *leds &= ~LED1_SET & ~LED3_SET & ~LED2_SET; } else if (batVoltage < BAT_LVL1) { // | B | 0 | 0 | *leds ^= LED1_SET; *leds &= ~LED3_SET & ~LED2_SET; @@ -1159,7 +1203,7 @@ void sideboardSensors(uint8_t sensors) { case 4: // COMMUTATION rtP_Left.z_ctrlTypSel = 0; rtP_Right.z_ctrlTypSel = 0; - break; + break; } shortBeepMany(sensor1_index + 1); } @@ -1170,17 +1214,17 @@ void sideboardSensors(uint8_t sensors) { if (sensor2_index > 1) { sensor2_index = 0; } switch (sensor2_index) { case 0: // FW Disabled - rtP_Left.b_fieldWeakEna = 0; + rtP_Left.b_fieldWeakEna = 0; rtP_Right.b_fieldWeakEna = 0; Input_Lim_Init(); break; case 1: // FW Enabled - rtP_Left.b_fieldWeakEna = 1; + rtP_Left.b_fieldWeakEna = 1; rtP_Right.b_fieldWeakEna = 1; Input_Lim_Init(); - break; + break; } - shortBeepMany(sensor2_index + 1); + shortBeepMany(sensor2_index + 1); } #endif } @@ -1193,18 +1237,18 @@ void sideboardSensors(uint8_t sensors) { * Max: 32767.99998474121 * Min: -32768 * Res: 1.52587890625e-05 - * + * * Inputs: u = int16 or int32 * Outputs: y = fixdt(1,32,16) * Parameters: coef = fixdt(0,16,16) = [0,65535U] - * - * Example: + * + * 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; + 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); @@ -1215,9 +1259,9 @@ void sideboardSensors(uint8_t sensors) { // 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 + // int32_t tmp; + // tmp = (int16_t)(u << 4) - (*y >> 16); + // tmp = CLAMP(tmp, -32768, 32767); // Overflow protection // *y = coef * tmp + (*y); // } @@ -1246,7 +1290,7 @@ void rateLimiter16(int16_t u, int16_t rate, int16_t *y) { } - /* mixerFcn(rtu_speed, rtu_steer, &rty_speedR, &rty_speedL); + /* 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) @@ -1259,9 +1303,9 @@ void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t prodSpeed = (int16_t)((rtu_speed * (int16_t)SPEED_COEFFICIENT) >> 14); prodSteer = (int16_t)((rtu_steer * (int16_t)STEER_COEFFICIENT) >> 14); - tmp = prodSpeed - prodSteer; + tmp = prodSpeed - prodSteer; tmp = CLAMP(tmp, -32768, 32767); // Overflow protection - *rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int + *rty_speedR = (int16_t)(tmp >> 4); // Convert from fixed-point to int *rty_speedR = CLAMP(*rty_speedR, INPUT_MIN, INPUT_MAX); tmp = prodSpeed + prodSteer; @@ -1276,7 +1320,7 @@ void mixerFcn(int16_t rtu_speed, int16_t rtu_steer, int16_t *rty_speedR, int16_t /* 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) + * 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) { @@ -1285,7 +1329,7 @@ void multipleTapDet(int16_t u, uint32_t timeNow, MultipleTap *x) { uint8_t b_pulse; uint8_t z_pulseCnt; uint8_t z_pulseCntRst; - uint32_t t_time; + uint32_t t_time; // Detect hysteresis if (x->b_hysteresis) { diff --git a/platformio.ini b/platformio.ini index 2de82aef..ba3f0318 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,6 +18,7 @@ src_dir = Src ;default_envs = VARIANT_HOVERCAR ; Variant for HOVERCAR build ;default_envs = VARIANT_HOVERBOARD ; Variant for HOVERBOARD ;default_envs = VARIANT_TRANSPOTTER ; Variant for TRANSPOTTER build https://github.com/NiklasFauth/hoverboard-firmware-hack/wiki/Build-Instruction:-TranspOtter https://hackaday.io/project/161891-transpotter-ng +;default_envs = VARIANT_BIPROPELLANT ; Variant for Bipropellant Protocol ;================================================================ ;================================================================ @@ -34,6 +35,7 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -57,6 +59,7 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -76,6 +79,7 @@ debug_tool = stlink upload_protocol = stlink build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -95,6 +99,7 @@ debug_tool = stlink upload_protocol = stlink build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -114,6 +119,7 @@ debug_tool = stlink upload_protocol = stlink build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -133,6 +139,7 @@ debug_tool = stlink upload_protocol = stlink build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -156,6 +163,7 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -179,6 +187,7 @@ monitor_port = COM5 monitor_speed = 38400 build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -198,6 +207,7 @@ debug_tool = stlink upload_protocol = stlink build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ -DUSE_HAL_DRIVER -DSTM32F103xE -Wl,-T./STM32F103RCTx_FLASH.ld @@ -206,5 +216,29 @@ build_flags = -g -ggdb ; to generate correctly the 'firmware.elf' for STM STUDIO vizualization # -Wl,-lnosys -D VARIANT_TRANSPOTTER - + ;================================================================ + +[env:VARIANT_BIPROPELLANT] +platform = ststm32 +framework = stm32cube +board = genericSTM32F103RC +debug_tool = stlink +upload_protocol = stlink + +; Serial Port settings (make sure the COM port is correct) +monitor_port = /dev/ttyUSB0 +monitor_speed = 115200 + +build_flags = + -I${PROJECT_DIR}/Src/bipropellantProtocol/ + -DUSE_HAL_DRIVER + -DSTM32F103xE + -Wl,-T./STM32F103RCTx_FLASH.ld + -Wl,-lc + -Wl,-lm + -g -ggdb ; to generate correctly the 'firmware.elf' for STM STUDIO vizualization +# -Wl,-lnosys + -D VARIANT_BIPROPELLANT + +;================================================================ \ No newline at end of file