diff --git a/.gitignore b/.gitignore index 7fa02c92..d623bdce 100644 --- a/.gitignore +++ b/.gitignore @@ -1,13 +1,23 @@ -*.asm -*.bin -*.elf -*.ihx -*.cdb -*.lk -*.map -*.adb -*.lst -*.rel -*.rst -*.sym -Debug +main.hex +**/*~ +*.asm +*.bin +*.elf +*.ihx +*.cdb +*.lk +*.map +*.adb +*.lst +*.rel +*.rst +*.sym +!*.dll +!*.exe +Debug +/tools/nbproject/private/ +/tools/JavaConfigurator/nbproject/private/ +/tools/JavaConfigurator/build/ +/tools/JavaConfigurator/dist/ +/nbproject/private/ +/Result.log diff --git a/ACAcommons.c b/ACAcommons.c new file mode 100644 index 00000000..c2919ac5 --- /dev/null +++ b/ACAcommons.c @@ -0,0 +1,381 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAcommons.c + * Author: Björn Schmidt + * + * Created on September 7, 2018, 7:31 PM + */ + +#include +#include +#include "stm8s.h" +#include "stm8s_itc.h" +#include "stm8s_gpio.h" +#include "config.h" +#include "gpio.h" +#include "adc.h" +#include "ACAcommons.h" +#include "ACAcontrollerState.h" + +static uint8_t ui8_temp; + +uint8_t float2int(float in, float maxRange) { + uint16_t result; + if (in < 0.0) + return 0; + result = (uint16_t) (in * (float) ((float) 256 / (float) maxRange)); + if (result > 255) + result = 255; + return (result); +} + +float int2float(uint8_t in, float maxRange) { + return ((float) in / (float) ((float) 256 / (float) maxRange)); +} + +int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) { + // if input is smaller/bigger than expected return the min/max out ranges value + if (x < in_min) + return out_min; + else if (x > in_max) + return out_max; + + // map the input to the output range. + // round up if mapping bigger ranges to smaller ranges + else if ((in_max - in_min) > (out_max - out_min)) + return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min; + // round down if mapping smaller ranges to bigger ranges + else + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +uint32_t PI_control(uint16_t pv, uint16_t setpoint, uint8_t uint_PWM_Enable) { + float float_p; + static float float_i; + static float float_dc = 0; + float_p = ((float) setpoint - (float) pv) * flt_s_pid_gain_p; + float_i += ((float) setpoint - (float) pv) * flt_s_pid_gain_i; + + if (float_i > 255)float_i = 255; + if (float_i < 0)float_i = 0; + + if (!uint_PWM_Enable && ((ui16_aca_experimental_flags & PWM_AUTO_OFF) == PWM_AUTO_OFF)) { + float_i = ui32_erps_filtered * flt_s_motor_constant - float_p; + } + + if (float_p + float_i > float_dc + 5) { + float_dc += 5; + } else if (float_p + float_i < float_dc - 5) { + float_dc -= 5; + } else { + float_dc = float_p + float_i; + } + + if (float_dc > 255)float_dc = 255; + if (float_dc < 0)float_dc = 0; + + return ((uint32_t) (float_dc)); +} + +void updateSpeeds(void) { + // Update speed after speed interrupt occurrence + if (ui8_SPEED_Flag) { + if (ui16_time_ticks_for_speed_calculation > 1500) { + //ignore spikes speed information, Do nothing if derived speed would be greater ca. 82km/h with 28" wheel + ui16_time_ticks_between_speed_interrupt = ui16_time_ticks_for_speed_calculation; //save recent speed + ui16_time_ticks_for_speed_calculation = 0; //reset speed counter + + ui32_speed_sensor_rpks_accumulated -= ui32_speed_sensor_rpks_accumulated >> 2; + ui32_speed_sensor_rpks_accumulated += (((uint32_t)ui16_pwm_cycles_second)*1000) / ((uint32_t) ui16_time_ticks_between_speed_interrupt); // speed in rounds per 1000 seconds + ui32_speed_sensor_rpks = ui32_speed_sensor_rpks_accumulated >> 2; //tic frequency 15625 Hz + } + + ui8_SPEED_Flag = 0; //reset interrupt flag + + } + //if wheel isn't turning, reset speed + // FIXME, the following is gathered from two places that were executed just in that order + // distinction 40000/65529 doesn't really make much sense + if (ui16_time_ticks_for_speed_calculation > 40000) { + ui32_speed_sensor_rpks = 0; + } + if (ui16_time_ticks_for_speed_calculation > 65529 && ui16_time_ticks_between_speed_interrupt != 65530) { + ui16_time_ticks_between_speed_interrupt = 65530; //Set Display to 0 km/h + PAS_act = 0; //Set PAS indicator to 0 to avoid motor startig, if pushing backwards from standstill + } +} + +uint32_t CheckSpeed(uint16_t current_target, uint16_t speed, uint16_t softLimit, uint16_t hardLimit) { + //ramp down motor power if you are riding too fast and speed liming is active + if (speed > softLimit && ui8_offroad_state != 255) { // FIXME the && part is only in here for old setpoint, aca already defines a higher limit + + if (speed > hardLimit) { //if you are riding much too fast, stop motor immediately + current_target = ui16_current_cal_b; + + } else { + uint32_t ui32_ct_normalized = ((current_target - ui16_current_cal_b)); + + current_target = (uint16_t) + ((ui32_ct_normalized * (hardLimit - speed)) / + (hardLimit - softLimit) + + ui16_current_cal_b); + + + } + } + return ((uint32_t) current_target); +} + +// internal signals so that components con communicate simple things without code cohesion + +void setSignal(uint8_t signal) { + uint32_icc_signals |= 1 << signal; +} + +uint8_t readAndClearSignal(uint8_t signal) { + if ((uint32_icc_signals & (1 << signal)) > 0) { + uint32_icc_signals &= ~(1 << signal); + return 1; + } + return 0; +} + +void initErpsRatio(void) { + //if (readAndClearSignal(SIGNAL_SPEEDLIMIT_CHANGED) == 1) + ui16_speed_kph_to_erps_ratio = (uint16_t) ((float) ui8_gear_ratio * 1000000.0 / ((float) wheel_circumference * 36.0)); +} + +void updateHallOrder(uint8_t hall_sensors) { + + if (++ui8_hall_order_counter > 5) { + ui8_hall_order_counter = 0; + } + uint8_t_hall_order[ui8_hall_order_counter] = hall_sensors; +} + +void updatePasDir(void) { + if (((ui16_aca_flags & TQ_SENSOR_MODE) == TQ_SENSOR_MODE)&&(ui16_time_ticks_between_pas_interrupt < timeout)) { + //only for Torquesensor Mode. + PAS_is_active = 1; + } else if (((ui16_aca_flags & TQ_SENSOR_MODE) != TQ_SENSOR_MODE) && (PAS_act > 3)) { + //set direction only if enough pulses in the right direction are detected. + PAS_is_active = 1; + } else { + PAS_is_active = 0; + } +} + +void updateX4(void) { + ui16_x4_value = ui16_adc_read_x4_value(); +} + +//added by DerBastler - Light +void updateLight(void) { + if (((light_stat &1) != 1)&& ((light_stat &128) == 128)) { + light_pin_set(); + light_stat = (light_stat&~1) | 1; + }else if (((light_stat &1) == 1)&& ((light_stat &128)!= 128)) { + light_pin_reset(); + light_stat = (light_stat&~1) | 0; + } +} + + +void updateRequestedTorque(void) { + + ui16_momentary_throttle = (uint16_t) map(ui8_adc_read_throttle(), ADC_THROTTLE_MIN_VALUE, ADC_THROTTLE_MAX_VALUE, 0, SETPOINT_MAX_VALUE); //read in recent throttle value for throttle override + + if (((ui16_aca_flags & TQ_SENSOR_MODE) != TQ_SENSOR_MODE)) { + ui16_throttle_accumulated -= ui16_throttle_accumulated >> 4; + ui16_throttle_accumulated += ui8_adc_read_throttle(); + ui8_temp = ui16_throttle_accumulated >> 4; //read in value from adc + ui16_sum_throttle = (uint8_t) map(ui8_temp, ui8_throttle_min_range, ui8_throttle_max_range, 0, SETPOINT_MAX_VALUE); //map throttle to limits + } else { + + ui16_sum_torque = 0; + for (ui8_temp = 0; ui8_temp < NUMBER_OF_PAS_MAGS; ui8_temp++) { // sum up array content + ui16_sum_torque += ui16_torque[ui8_temp]; + } + + ui16_sum_torque /= NUMBER_OF_PAS_MAGS; + + } +} + +void checkPasInActivity(void) { + ui8_PAS_update_call_when_inactive_counter++; + if (ui16_time_ticks_for_pas_calculation > timeout) { + // updatePasStatus does not fire if pas inactive, so set interval to reasonably high value here + ui16_time_ticks_between_pas_interrupt = 64000L; + // also ensure torque array slowly resets + ui16_torque[ui8_torque_index] = (uint8_t) map(ui8_throttle_min_range, ui8_throttle_min_range, ui8_throttle_max_range, 0, SETPOINT_MAX_VALUE); //map throttle to limits + ui8_torque_index++; + if (ui8_torque_index > NUMBER_OF_PAS_MAGS - 1) { + ui8_torque_index = 0; + } + + } + // we are called at 50 Hz, if there has been no interrupt for more than ~1s, ramp down PAS automatically + if (ui8_PAS_Flag == 0 && ui8_PAS_update_call_when_inactive_counter > (uint8_t) (timeout >> 6)) { + + ui8_PAS_update_call_when_inactive_counter = 0; + + if (PAS_act > 0) { + PAS_act--; + } + + updatePasDir(); + + } +} + +void updatePasStatus(void) { + + if (ui8_PAS_Flag == 1) { + ui8_PAS_Flag = 0; //reset interrupt flag + + ui16_time_ticks_between_pas_interrupt = ui16_time_ticks_for_pas_calculation; //save recent cadence + ui16_PAS_High = ui16_PAS_High_Counter; + + if ((0 == (ui16_aca_flags & PAS_INVERTED)) && ((float) ui16_time_ticks_between_pas_interrupt / (float) ui16_PAS_High > flt_s_pas_threshold)) { + if (PAS_act < 7) { + PAS_act++; + } + } else if ((PAS_INVERTED == (ui16_aca_flags & PAS_INVERTED)) && ((float) ui16_time_ticks_between_pas_interrupt / (float) ui16_PAS_High < flt_s_pas_threshold)) { + if (PAS_act < 7) { + PAS_act++; + } + } else { + if (PAS_act > 0) { + PAS_act--; + } + } + + + if (((ui16_aca_flags & TQ_SENSOR_MODE) == TQ_SENSOR_MODE)) { + ui8_temp = ui8_adc_read_throttle(); //read in recent torque value + ui16_torque[ui8_torque_index] = (uint8_t) map(ui8_temp, ui8_throttle_min_range, ui8_throttle_max_range, 0, SETPOINT_MAX_VALUE); //map throttle to limits + + ui8_torque_index++; + if (ui8_torque_index > NUMBER_OF_PAS_MAGS - 1) { + ui8_torque_index = 0; + } //reset index counter + + } + + updatePasDir(); + ui16_time_ticks_for_pas_calculation = 1; + ui16_PAS_High_Counter = 1; //reset PAS Counter + ui8_PAS_update_call_when_inactive_counter = 0; + } +} + +void updateSlowLoopStates(void) { + + if (ui16_no_pass_counter < 64000){ + ui16_no_pass_counter++; + } + + if (ui16_motor_speed_erps == 0) { + if (ui16_idle_counter < 64000){ + ui16_idle_counter++; + } + } else { + ui16_idle_counter = 0; + } + + // debug only + //ui8_variableDebugC = ui16_no_pass_counter>>8; + + //disable lock if passcode is not at least 4 digits + if (ui16_passcode < 1001){ + ui8_lockstatus = 16; + ui16_no_pass_counter =0; + }else if (ui16_no_pass_counter > 3000) { + //lock after 60 seconds idle + ui8_lockstatus = 255; + } + + if (((ui16_aca_flags & IDLE_DISABLES_OFFROAD) == IDLE_DISABLES_OFFROAD) && (ui8_offroad_state > 4) && (ui16_idle_counter > 3000)) { + //disable after 60 seconds idle + ui8_offroad_state = 0; + } + + if (((ui16_aca_flags & BRAKE_DISABLES_OFFROAD) == BRAKE_DISABLES_OFFROAD) && (ui8_offroad_state > 4)) { + // if disabling is enabled :) + if (!GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN)) { + ui8_offroad_counter++; + if (ui8_offroad_counter == 255) {//disable on pressing brake for 5 seconds + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else { + ui8_offroad_counter = 0; + } + } + + // check if offroad mode is enabled + if (0 == (ui16_aca_flags & OFFROAD_ENABLED)) { + return; + } + + if (ui8_offroad_state == 0 && !GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN)) {//first step, brake on. + ui8_offroad_state = 1; + } else if (ui8_offroad_state == 1) {//second step, make sure the brake is hold according to definded time + ui8_offroad_counter++; + if (GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter < MORSE_TIME_1) {//brake is released too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_1 + MORSE_TOLERANCE) {//brake is released according to cheatcode + ui8_offroad_state = 2; + ui8_offroad_counter = 0; + } else if (!GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_1 + MORSE_TOLERANCE) {//brake is released too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 2) {//third step, make sure the brake is released according to definded time + ui8_offroad_counter++; + if (!GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter < MORSE_TIME_2) { //brake is hold too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (!GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_2 + MORSE_TOLERANCE) {//brake is hold according to cheatcode + ui8_offroad_state = 3; + ui8_offroad_counter = 0; + + } else if (GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_2 + MORSE_TOLERANCE) {//brake is hold too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 3) {//second step, make sure the brake is hold according to definded time + ui8_offroad_counter++; + if (GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter < MORSE_TIME_3) {//brake is released too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_3 + MORSE_TOLERANCE) {//brake is released according to cheatcode + ui8_offroad_state = 4; + ui8_offroad_counter = 0; + + } else if (!GPIO_ReadInputPin(BRAKE__PORT, BRAKE__PIN) && ui8_offroad_counter > MORSE_TIME_3 + MORSE_TOLERANCE) {//brake is released too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 4) { + // wait 3 seconds in state 4 for display feedback + ui8_offroad_counter++; + if (ui8_offroad_counter > 150) { + ui8_offroad_state = 255; + ui8_offroad_counter = 0; + } + } + + +} diff --git a/ACAcommons.h b/ACAcommons.h new file mode 100644 index 00000000..e2ad4b75 --- /dev/null +++ b/ACAcommons.h @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAcommons.h + * Author: Björn Schmidt + * + * Created on September 7, 2018, 7:31 PM + */ + +#ifndef ACACOMMONS_H +#define ACACOMMONS_H + +#include "config.h" + +uint32_t PI_control(uint16_t pv, uint16_t setpoint, uint8_t uint_PWM_Enable); +uint32_t CheckSpeed(uint16_t current_target, uint16_t speed, uint16_t softLimit, uint16_t hardLimit); +int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); + +uint8_t float2int(float in, float maxRange); +float int2float(uint8_t in, float maxRange); + +void setSignal(uint8_t signal); +uint8_t readAndClearSignal(uint8_t signal); +void updateHallOrder(uint8_t hall_sensors); +void updateSlowLoopStates(void); +void initErpsRatio(void); +void updateSpeeds(void); +void updateX4(void); +void updateLight(void); +void updatePasStatus(void); +void checkPasInActivity(void); +void updateRequestedTorque(void); + +#endif /* ACACOMMONS_H */ + diff --git a/ACAcontrollerState.c b/ACAcontrollerState.c new file mode 100644 index 00000000..68789168 --- /dev/null +++ b/ACAcontrollerState.c @@ -0,0 +1,296 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAcontrollerState.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + +#include +#include "config.h" +#include "stm8s.h" +#include "stm8s_itc.h" +#include "stm8s_gpio.h" +#include "gpio.h" +#include "ACAeeprom.h" +#include "ACAcontrollerState.h" +#include "ACAcommons.h" + +// user controllable settings +uint8_t ui8_throttle_min_range = 32; +uint8_t ui8_throttle_max_range = 192; + + + + +uint8_t ui8_speedlimit_kph = 25; // normal limit +uint8_t ui8_speedlimit_without_pas_kph = 6; // limit without pas activity +uint8_t ui8_speedlimit_with_throttle_override_kph = 35; // limit with pas and throttle both active +uint8_t ui8_speedlimit_actual_kph; // dynamic speedlimit based on current state +float flt_s_pas_threshold = 1.7; +float flt_s_pid_gain_p = 0.5; +float flt_s_pid_gain_i = 0.2; +float flt_s_motor_constant = 1.5; +float flt_torquesensorCalibration = 0.0; +uint32_t uint32_torquesensorCalibration = TQS_CALIB; +uint16_t ui16_s_ramp_end = 1500; +uint16_t ui16_s_ramp_start = 7000; +uint8_t ui8_s_motor_angle = 214; +uint8_t ui8_s_hall_angle4_0 = 0; +uint8_t ui8_s_hall_angle6_60 = 42; +uint8_t ui8_s_hall_angle2_120 = 85; +uint8_t ui8_s_hall_angle3_180 = 127; +uint8_t ui8_s_hall_angle1_240 = 170; +uint8_t ui8_s_hall_angle5_300 = 212; + +uint8_t ui8_s_battery_voltage_calibration; +uint8_t ui8_s_battery_voltage_min; +uint8_t ui8_s_battery_voltage_max; + +// internal +uint32_t uint32_icc_signals = 0; + +uint16_t ui16_erps_max = 0; +uint16_t ui16_pwm_cycles_second = 0; + +uint8_t ui8_gear_ratio = 1; + +uint8_t ui8_a_s_assistlevels[6]; +uint8_t ui8_assist_dynamic_percent_addon = 0; +uint8_t ui8_assistlevel_global = 66; // 2 + regen 4 +uint8_t ui8_assist_percent_actual = 20; +uint8_t ui8_assist_percent_wanted = 20; +uint8_t ui8_walk_assist = 0; +uint8_t PAS_act = 3; //recent PAS direction reading +uint8_t PAS_is_active = 0; +uint16_t ui16_sum_torque = 0; +uint16_t ui16_sum_throttle = 0; +uint16_t ui16_momentary_throttle = 0; +uint8_t ui8_offroad_state = 0; //state of offroad switching procedure +uint32_t uint32_current_target = 0; //target for PI-Control +uint16_t ui16_setpoint = 0; +uint16_t ui16_throttle_accumulated = 0; +uint8_t ui8_current_cal_a = 0; +uint16_t ui16_current_cal_b = 0; +uint16_t ui16_x4_cal_b = 0; +uint16_t ui16_x4_value = 0; +uint16_t ui16_throttle_cal_b = 0; +uint16_t ui16_battery_current_max_value = 0; +uint16_t ui16_regen_current_max_value = 0; +uint8_t ui8_possible_motor_state = 0; +uint8_t ui8_dynamic_motor_state = 0; +uint8_t ui8_BatteryVoltage = 0; //Battery Voltage read from ADC +uint8_t ui8_battery_voltage_nominal =0; +uint16_t ui16_motor_speed_erps = 0; +uint32_t ui32_erps_filtered = 0; //filtered value of erps +uint16_t ui16_virtual_erps_speed = 0; +uint16_t ui16_BatteryCurrent = 0; //Battery Current read from ADC8 +uint8_t ui8_position_correction_value = 127; // in 360/256 degrees +uint8_t ui8_correction_at_angle = 127; +uint16_t ui16_ADC_iq_current = 0; +uint16_t ui16_ADC_iq_current_filtered = 0; +uint16_t ui16_control_state = 0; +uint8_t ui8_uptime = 0; + +uint8_t ui8_variableDebugA = 0; +uint8_t ui8_variableDebugB = 0; +uint8_t ui8_variableDebugC = 0; + +int8_t i8_motor_temperature = 0; + +uint8_t uint8_t_60deg_pwm_cycles[6]; +uint8_t uint8_t_hall_case[7]; +uint8_t uint8_t_hall_order[6]; +int8_t int8_t_hall_counter = 0; +uint8_t ui8_hall_order_counter = 5; + +uint16_t ui16_speed_kph_to_erps_ratio = 0; + +uint32_t ui32_speed_sensor_rpks; //speed sensor rounds per 1000 sec +uint32_t ui32_speed_sensor_rpks_accumulated = 0; +uint16_t ui16_time_ticks_between_speed_interrupt = 64000L; //speed in timetics +uint16_t ui16_time_ticks_for_speed_calculation = 0; //time tics for speed measurement +uint16_t ui16_time_ticks_for_uart_timeout = 0; +uint8_t ui8_SPEED_Flag = 0; //flag for SPEED interrupt +uint8_t ui8_offroad_counter = 0; //counter for offroad switching procedure +uint16_t ui16_idle_counter = 0; +uint16_t ui16_no_pass_counter = 3000; +uint16_t ui16_passcode = 0; +uint8_t ui8_lockstatus = 255; + +uint16_t ui16_aca_flags = 0; +uint16_t ui16_aca_experimental_flags = 0; + +uint16_t ui16_torque[NUMBER_OF_PAS_MAGS]; //array for torque values of one crank revolution +uint8_t ui8_torque_index = 0; //counter for torque array + +uint16_t ui16_time_ticks_between_pas_interrupt_smoothed = 0; +uint16_t ui16_time_ticks_for_pas_calculation = 0; //time tics for cadence measurement +uint16_t ui16_PAS_High_Counter = 1; //time tics for direction detection +uint16_t ui16_time_ticks_between_pas_interrupt = 64000L; //cadence in timetics +uint16_t ui16_PAS_High = 1; //number of High readings on PAS +uint8_t ui8_PAS_update_call_when_inactive_counter = 50; //increased when no pas change is detected (50Hz) +uint8_t ui8_PAS_Flag = 0; + +uint8_t light_stat = 0; +uint8_t walk_stat = 0; + +void controllerstate_init(void) { + uint8_t di; + uint8_t eepromVal; + uint8_t eepromHighVal; + + // convert static defines to volatile vars + ui16_pwm_cycles_second = PWM_CPS_NORMAL_SPEED; + ui16_erps_max = ui16_pwm_cycles_second / 30; //limit erps to have minimum 30 points on the sine curve for proper commutation + ui8_a_s_assistlevels[0] =0; + ui8_a_s_assistlevels[1] =LEVEL_1; + ui8_a_s_assistlevels[2] =LEVEL_2; + ui8_a_s_assistlevels[3] =LEVEL_3; + ui8_a_s_assistlevels[4] =LEVEL_4; + ui8_a_s_assistlevels[5] =LEVEL_5; + ui16_aca_flags = ACA; + ui16_aca_experimental_flags = ACA_EXPERIMENTAL; + ui8_s_battery_voltage_calibration = ADC_BATTERY_VOLTAGE_K; + ui8_s_battery_voltage_min = BATTERY_VOLTAGE_MIN_VALUE; + ui8_s_battery_voltage_max = BATTERY_VOLTAGE_MAX_VALUE; + ui8_speedlimit_kph = limit; + ui8_speedlimit_without_pas_kph = limit_without_pas; + ui8_speedlimit_with_throttle_override_kph = limit_with_throttle_override; + ui8_speedlimit_actual_kph = limit; + ui8_throttle_min_range = ADC_THROTTLE_MIN_VALUE; + ui8_throttle_max_range = ADC_THROTTLE_MAX_VALUE; + flt_s_pas_threshold = PAS_THRESHOLD; + flt_s_pid_gain_p = P_FACTOR; + flt_s_pid_gain_i = I_FACTOR; + ui16_s_ramp_start = RAMP_START; + ui16_s_ramp_end = RAMP_END; + ui8_s_motor_angle = MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT; + ui8_s_hall_angle4_0 = ANGLE_4_0; + ui8_s_hall_angle6_60 = ANGLE_6_60; + ui8_s_hall_angle2_120 = ANGLE_2_120; + ui8_s_hall_angle3_180 = ANGLE_3_180; + ui8_s_hall_angle1_240 = ANGLE_1_240; + ui8_s_hall_angle5_300 = ANGLE_5_300; + ui16_battery_current_max_value = BATTERY_CURRENT_MAX_VALUE; + ui16_regen_current_max_value = REGEN_CURRENT_MAX_VALUE; + ui8_current_cal_a = current_cal_a; + ui8_correction_at_angle = CORRECTION_AT_ANGLE; + flt_torquesensorCalibration = TQS_CALIB; + uint32_torquesensorCalibration = (uint32_t)flt_torquesensorCalibration; + ui8_gear_ratio = GEAR_RATIO; + + // read in overrides from eeprom if they are > 0, assuming 0s are uninitialized + eepromHighVal = eeprom_read(OFFSET_BATTERY_CURRENT_MAX_VALUE_HIGH_BYTE); + eepromVal = eeprom_read(OFFSET_BATTERY_CURRENT_MAX_VALUE); + if (eepromVal > 0 || eepromHighVal > 0) ui16_battery_current_max_value = ((uint16_t) eepromHighVal << 8) + (uint16_t) eepromVal; + + eepromHighVal = eeprom_read(OFFSET_PASSCODE_HIGH_BYTE); + eepromVal = eeprom_read(OFFSET_PASSCODE); + if (eepromVal > 0 || eepromHighVal > 0) ui16_passcode = ((uint16_t) eepromHighVal << 8) + (uint16_t) eepromVal; + + eepromHighVal = eeprom_read(OFFSET_ACA_FLAGS_HIGH_BYTE); + eepromVal = eeprom_read(OFFSET_ACA_FLAGS); + if (eepromVal > 0 || eepromHighVal > 0) ui16_aca_flags = ((uint16_t) eepromHighVal << 8) + (uint16_t) eepromVal; + eepromHighVal = eeprom_read(OFFSET_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE); + eepromVal = eeprom_read(OFFSET_ACA_EXPERIMENTAL_FLAGS); + if (eepromVal > 0 || eepromHighVal > 0) ui16_aca_experimental_flags = ((uint16_t) eepromHighVal << 8) + (uint16_t) eepromVal; + + if ((ui16_aca_experimental_flags & HIGH_SPEED_MOTOR) == HIGH_SPEED_MOTOR) { + // pwm_init is run right after this functions call in main.c, so it's ok to change it here + // we do this only here at startup, so controller has to be restarted for motor speed changes to take effect + ui16_pwm_cycles_second = PWM_CPS_HIGH_SPEED; + ui16_erps_max = ui16_pwm_cycles_second / 30; + } + + eepromVal = eeprom_read(OFFSET_GEAR_RATIO); + if (eepromVal > 0) ui8_gear_ratio = eepromVal; + + eepromVal = eeprom_read(OFFSET_REGEN_CURRENT_MAX_VALUE); + if (eepromVal > 0) ui16_regen_current_max_value = eepromVal; + eepromVal = eeprom_read(OFFSET_MAX_SPEED_DEFAULT); + if (eepromVal > 0) ui8_speedlimit_kph = eepromVal; + eepromVal = eeprom_read(OFFSET_MAX_SPEED_WITHOUT_PAS); + if (eepromVal > 0) ui8_speedlimit_without_pas_kph = eepromVal; + eepromVal = eeprom_read(OFFSET_MAX_SPEED_WITH_THROTTLE_OVERRIDE); + if (eepromVal > 0) ui8_speedlimit_with_throttle_override_kph = eepromVal; + eepromVal = eeprom_read(OFFSET_CURRENT_CAL_A); + if (eepromVal > 0) ui8_current_cal_a = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_LEVEL); + if (eepromVal > 0) ui8_assistlevel_global = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_WANTED); + if (eepromVal > 0) ui8_assist_percent_wanted = eepromVal; + eepromVal = eeprom_read(OFFSET_THROTTLE_MIN_RANGE); + if (eepromVal > 0) ui8_throttle_min_range = eepromVal; + eepromVal = eeprom_read(OFFSET_THROTTLE_MAX_RANGE); + if (eepromVal > 0) ui8_throttle_max_range = eepromVal; + eepromVal = eeprom_read(OFFSET_PAS_TRESHOLD); + if (eepromVal > 0) flt_s_pas_threshold = int2float(eepromVal, 4.0); + eepromVal = eeprom_read(OFFSET_TQ_CALIB); + if (eepromVal > 0){ flt_torquesensorCalibration = int2float(eepromVal, 8000.0); + uint32_torquesensorCalibration = (uint32_t)flt_torquesensorCalibration; + } + eepromVal = eeprom_read(OFFSET_PID_GAIN_P); + if (eepromVal > 0) flt_s_pid_gain_p = int2float(eepromVal, 2.0); + eepromVal = eeprom_read(OFFSET_PID_GAIN_I); + if (eepromVal > 0) flt_s_pid_gain_i = int2float(eepromVal, 2.0); + eepromVal = eeprom_read(OFFSET_RAMP_END); + if (eepromVal > 0) ui16_s_ramp_end = eepromVal << 5; + eepromVal = eeprom_read(OFFSET_RAMP_START); + if (eepromVal > 0) ui16_s_ramp_start = eepromVal << 6; + eepromVal = eeprom_read(OFFSET_MOTOR_ANGLE); + if (eepromVal > 0) ui8_s_motor_angle = eepromVal; + eepromVal = eeprom_read(OFFSET_CORRECTION_AT_ANGLE); + if (eepromVal > 0) ui8_correction_at_angle = eepromVal; + + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_4_0); + if (eepromVal > 0) ui8_s_hall_angle4_0 = eepromVal; + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_6_60); + if (eepromVal > 0) ui8_s_hall_angle6_60 = eepromVal; + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_2_120); + if (eepromVal > 0) ui8_s_hall_angle2_120 = eepromVal; + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_3_180); + if (eepromVal > 0) ui8_s_hall_angle3_180 = eepromVal; + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_1_240); + if (eepromVal > 0) ui8_s_hall_angle1_240 = eepromVal; + eepromVal = eeprom_read(OFFSET_HALL_ANGLE_5_300); + if (eepromVal > 0) ui8_s_hall_angle5_300 = eepromVal; + + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_LEVEL_1); + if (eepromVal > 0) ui8_a_s_assistlevels[1] = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_LEVEL_2); + if (eepromVal > 0) ui8_a_s_assistlevels[2] = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_LEVEL_3); + if (eepromVal > 0) ui8_a_s_assistlevels[3] = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_LEVEL_4); + if (eepromVal > 0) ui8_a_s_assistlevels[4] = eepromVal; + eepromVal = eeprom_read(OFFSET_ASSIST_PERCENT_LEVEL_5); + if (eepromVal > 0) ui8_a_s_assistlevels[5] = eepromVal; + + eepromVal = eeprom_read(OFFSET_BATTERY_VOLTAGE_CALIB); + if (eepromVal > 0) ui8_s_battery_voltage_calibration = eepromVal; + eepromVal = eeprom_read(OFFSET_BATTERY_VOLTAGE_MIN); + if (eepromVal > 0) ui8_s_battery_voltage_min = eepromVal; + eepromVal = eeprom_read(OFFSET_BATTERY_VOLTAGE_MAX); + if (eepromVal > 0) ui8_s_battery_voltage_max = eepromVal; + + ui8_battery_voltage_nominal = (((uint16_t)(ui8_s_battery_voltage_max-(ui8_s_battery_voltage_max-ui8_s_battery_voltage_min)/2))*ui8_s_battery_voltage_calibration)/256L; + + eepromVal = eeprom_read(OFFSET_MOTOR_CONSTANT); + if (eepromVal > 0) flt_s_motor_constant = int2float(eepromVal, 4.0); + + for (di = 0; di < 6; di++) { + uint8_t_hall_order[di] = 0; + } + +} + diff --git a/ACAcontrollerState.h b/ACAcontrollerState.h new file mode 100644 index 00000000..9fc0b082 --- /dev/null +++ b/ACAcontrollerState.h @@ -0,0 +1,194 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAcontrollerState.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + + +#ifndef ACACONTROLLERSTATE_H +#define ACACONTROLLERSTATE_H + +#include "config.h" + +extern uint32_t uint32_icc_signals; // inter component communication, very simplistig way of signalling stuff via a shared var + +extern uint16_t ui16_erps_max; +extern uint16_t ui16_pwm_cycles_second; + +extern uint8_t ui8_throttle_min_range; +extern uint8_t ui8_throttle_max_range; + + + + +extern uint16_t ui16_control_state; +extern uint8_t ui8_a_s_assistlevels[6]; +extern uint8_t ui8_assist_percent_actual; +extern uint8_t ui8_assist_percent_wanted; +extern uint8_t ui8_assistlevel_global; //assist level for regen (high 4 bits) and torque (low 4 bits) +extern uint8_t ui8_assist_dynamic_percent_addon; +extern uint8_t PAS_act; +extern uint8_t PAS_is_active; +extern uint16_t ui16_sum_torque; +extern uint16_t ui16_sum_throttle; +extern uint16_t ui16_momentary_throttle; +extern uint8_t ui8_offroad_state; +extern uint32_t uint32_current_target; +extern uint16_t ui16_setpoint; +extern uint16_t ui16_throttle_accumulated; +extern uint16_t ui16_current_cal_b; +extern uint8_t ui8_current_cal_a; +extern uint16_t ui16_x4_cal_b; +extern uint16_t ui16_x4_value; +extern uint16_t ui16_throttle_cal_b; +extern uint16_t ui16_battery_current_max_value; +extern uint16_t ui16_regen_current_max_value; +extern uint8_t ui8_possible_motor_state; +extern uint8_t ui8_dynamic_motor_state; +extern uint8_t ui8_BatteryVoltage; +extern uint8_t ui8_battery_voltage_nominal; +extern uint16_t ui16_motor_speed_erps; +extern uint16_t ui16_virtual_erps_speed; +extern uint32_t ui32_erps_filtered; //filtered value of erps +extern uint16_t ui16_BatteryCurrent; +extern uint8_t ui8_position_correction_value; +extern uint8_t ui8_correction_at_angle; +extern uint16_t ui16_ADC_iq_current; +extern uint16_t ui16_ADC_iq_current_filtered; +extern uint8_t ui8_speedlimit_kph; +extern uint8_t ui8_speedlimit_without_pas_kph; +extern uint8_t ui8_speedlimit_actual_kph; +extern uint8_t ui8_speedlimit_with_throttle_override_kph; +extern uint8_t ui8_uptime; +extern uint8_t ui8_walk_assist; + +extern uint8_t ui8_variableDebugA; +extern uint8_t ui8_variableDebugB; +extern uint8_t ui8_variableDebugC; + +extern int8_t i8_motor_temperature; + +extern uint8_t uint8_t_60deg_pwm_cycles[6]; +extern uint8_t uint8_t_hall_case[7]; +extern uint8_t uint8_t_hall_order[6]; +extern int8_t int8_t_hall_counter; +extern uint8_t ui8_hall_debug_counter; +extern uint8_t ui8_hall_order_counter; + +extern uint8_t ui8_gear_ratio; +extern uint16_t ui16_speed_kph_to_erps_ratio; + +extern uint32_t ui32_speed_sensor_rpks; //speed sensor rounds per 1000 sec +extern uint32_t ui32_speed_sensor_rpks_accumulated; + +extern uint16_t ui16_time_ticks_for_uart_timeout; +extern uint16_t ui16_time_ticks_for_speed_calculation; +extern uint16_t ui16_time_ticks_between_speed_interrupt; //Counter for bike speed +extern uint8_t ui8_SPEED_Flag; //Flag for PAS Interrupt detected +extern uint16_t ui16_time_ticks_between_speed_interrupt; //Speed duration of one wheel revolution (tics * 64us) +extern uint8_t ui8_offroad_counter; +extern uint16_t ui16_idle_counter; +extern uint16_t ui16_no_pass_counter; +extern uint16_t ui16_passcode; +extern uint8_t ui8_lockstatus; + +extern uint16_t ui16_aca_flags; +extern uint16_t ui16_aca_experimental_flags; + +extern uint16_t ui16_torque[NUMBER_OF_PAS_MAGS]; +extern uint8_t ui8_torque_index; + +extern uint16_t ui16_time_ticks_between_pas_interrupt_smoothed; // for filtering of PAS value +extern float flt_current_PAS_fraction; +extern uint16_t ui16_time_ticks_between_pas_interrupt; +extern uint16_t ui16_PAS_High; +extern uint16_t ui16_time_ticks_for_pas_calculation; //Counter for cadence +extern uint16_t ui16_PAS_High_Counter; +extern uint8_t ui8_PAS_Flag; //flag for PAS interrupt +extern uint8_t ui8_PAS_update_call_when_inactive_counter; + + +extern float flt_torquesensorCalibration; +extern uint32_t uint32_torquesensorCalibration; +extern float flt_s_pas_threshold; +extern float flt_s_pid_gain_p; +extern float flt_s_pid_gain_i; +extern float flt_s_motor_constant; +extern uint16_t ui16_s_ramp_end; +extern uint16_t ui16_s_ramp_start; +extern uint8_t ui8_s_motor_angle; +extern uint8_t ui8_s_hall_angle4_0; +extern uint8_t ui8_s_hall_angle6_60; +extern uint8_t ui8_s_hall_angle2_120; +extern uint8_t ui8_s_hall_angle3_180; +extern uint8_t ui8_s_hall_angle1_240; +extern uint8_t ui8_s_hall_angle5_300; + +extern uint8_t ui8_s_battery_voltage_calibration; +extern uint8_t ui8_s_battery_voltage_min; +extern uint8_t ui8_s_battery_voltage_max; + +extern uint8_t light_stat; +extern uint8_t walk_stat; + +void controllerstate_init(void); + +typedef enum { + // values from 0-31 are allowed as signals are stored in a single uint32_t + SIGNAL_SPEEDLIMIT_CHANGED = ((uint8_t) 0x00), + +} ICC_SIGNALS; + +typedef enum { + ASSIST_LVL_AFFECTS_THROTTLE = ((uint16_t) 1), + OFFROAD_ENABLED = ((uint16_t) 2), + BRAKE_DISABLES_OFFROAD = ((uint16_t) 4), + + DIGITAL_REGEN = ((uint16_t) 8), + SPEED_INFLUENCES_REGEN = ((uint16_t) 16), + SPEED_INFLUENCES_TORQUESENSOR = ((uint16_t) 32), + PAS_INVERTED = ((uint16_t) 64), + + DUMMY_ALWAYS_ON = ((uint16_t) 128), + + BYPASS_LOW_SPEED_REGEN_PI_CONTROL = ((uint16_t) 256), + + DYNAMIC_ASSIST_LEVEL = ((uint16_t) 512), + + POWER_BASED_CONTROL= ((uint16_t) 1024), + TQ_SENSOR_MODE = ((uint16_t) 2048), + ANGLE_CORRECTION_ENABLED = ((uint16_t) 4096), + EXTERNAL_SPEED_SENSOR = ((uint16_t) 8192), + IDLE_DISABLES_OFFROAD = ((uint16_t) 16384) + + +} ACA_FLAGS; + +typedef enum { + + DC_STATIC_ZERO = ((uint16_t) 1), + AVOID_MOTOR_CYCLES_JITTER = ((uint16_t) 2), + DISABLE_INTERPOLATION = ((uint16_t) 4), + DISABLE_60_DEG_INTERPOLATION = ((uint16_t) 8), + SWITCH_360_DEG_INTERPOLATION = ((uint16_t) 16), + USE_ALTERNATE_WAVETABLE = ((uint16_t) 32), + USE_ALTERNATE_WAVETABLE_B = ((uint16_t) 64), + DUMMY_EXP_ALWAYS_ON = ((uint16_t) 128), + HIGH_SPEED_MOTOR = ((uint16_t) 256), + PWM_AUTO_OFF = ((uint16_t) 1024), + +} ACA_EXPERIMENTAL_FLAGS; + +#endif /* BOCONTROLLERSTATE_H */ + diff --git a/ACAeeprom.c b/ACAeeprom.c new file mode 100644 index 00000000..cb4eb85f --- /dev/null +++ b/ACAeeprom.c @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAeeprom.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + +#include +#include +#include "stm8s.h" +#include "stm8s_itc.h" +#include "stm8s_flash.h" +#include "config.h" +#include "ACAeeprom.h" + +uint8_t eeprom_magic_byte = 0; + +void eeprom_init(void) { +#ifndef EEPROM_NOINIT + eeprom_magic_byte = (FLASH_ReadByte(EEPROM_BASE_ADDRESS + EEPROM_MAX_INIT_RANGE)); + if (eeprom_magic_byte != EEPROM_INIT_MAGIC_BYTE) { + // eeprom needs to be reset after flashing + uint8_t di; + FLASH_SetProgrammingTime(FLASH_PROGRAMTIME_STANDARD); + FLASH_Unlock(FLASH_MEMTYPE_DATA); + + for (di = 1; di < EEPROM_MAX_INIT_RANGE; di++) { + while (!FLASH_GetFlagStatus(FLASH_FLAG_DUL)); + FLASH_ProgramByte(EEPROM_BASE_ADDRESS + di, 0x00); + while (!FLASH_GetFlagStatus(FLASH_FLAG_EOP)); + } + + while (!FLASH_GetFlagStatus(FLASH_FLAG_DUL)); + FLASH_ProgramByte(EEPROM_BASE_ADDRESS + EEPROM_MAX_INIT_RANGE, EEPROM_INIT_MAGIC_BYTE); + while (!FLASH_GetFlagStatus(FLASH_FLAG_EOP)); + + FLASH_Lock(FLASH_MEMTYPE_DATA); + + // reread to check everything went well + eeprom_magic_byte = (FLASH_ReadByte(EEPROM_BASE_ADDRESS + EEPROM_MAX_INIT_RANGE)); + } +#endif +} + +uint8_t eeprom_read(uint8_t address_offset) { + // only values between 1 and EEPROM_MAX_INIT_RANGE allowed + if ((address_offset < 1) || (address_offset > EEPROM_MAX_INIT_RANGE - 1)) { + return 0; + } + + return (FLASH_ReadByte(EEPROM_BASE_ADDRESS + address_offset)); +} + +uint8_t eeprom_write(uint8_t address_offset, uint8_t value) { + // magic byte at EEPROM_MAX_INIT_RANGE, only values between 1 and EEPROM_MAX_INIT_RANGE allowed + if ((address_offset < 1) || (address_offset > EEPROM_MAX_INIT_RANGE - 1)) { + return 1; + } + + if (eeprom_read(address_offset) == value) { + return 0; + } + + FLASH_SetProgrammingTime(FLASH_PROGRAMTIME_STANDARD); + FLASH_Unlock(FLASH_MEMTYPE_DATA); + while (!FLASH_GetFlagStatus(FLASH_FLAG_DUL)); + FLASH_ProgramByte(EEPROM_BASE_ADDRESS + address_offset, value); + while (!FLASH_GetFlagStatus(FLASH_FLAG_EOP)); + FLASH_Lock(FLASH_MEMTYPE_DATA); + + return 0; +} \ No newline at end of file diff --git a/ACAeeprom.h b/ACAeeprom.h new file mode 100644 index 00000000..f2335e0c --- /dev/null +++ b/ACAeeprom.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAeeprom.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + +#ifndef BOEEPROM_H +#define BOEEPROM_H + +#include "config.h" + +extern uint8_t eeprom_magic_byte; + +#define EEPROM_BASE_ADDRESS 0x4000 +#define EEPROM_MAX_INIT_RANGE 0x2F // we should at least have 640 avail / 48 in use for now + +typedef enum { + OFFSET_MOTOR_ANGLE = ((uint8_t) 0x01), + OFFSET_ASSIST_PERCENT_WANTED = ((uint8_t) 0x02), + OFFSET_ASSIST_LEVEL = ((uint8_t) 0x03), + OFFSET_THROTTLE_MIN_RANGE = ((uint8_t) 0x04), + OFFSET_THROTTLE_MAX_RANGE = ((uint8_t) 0x05), + OFFSET_RAMP_START = ((uint8_t) 0x06), + OFFSET_PAS_TRESHOLD = ((uint8_t) 0x07), + OFFSET_PID_GAIN_P = ((uint8_t) 0x08), + OFFSET_PID_GAIN_I = ((uint8_t) 0x09), + OFFSET_RAMP_END = ((uint8_t) 0x0A), + + OFFSET_MAX_SPEED_DEFAULT = ((uint8_t) 0x0B), + OFFSET_MAX_SPEED_WITHOUT_PAS = ((uint8_t) 0x0C), + OFFSET_MAX_SPEED_WITH_THROTTLE_OVERRIDE = ((uint8_t) 0x0D), + + OFFSET_BATTERY_CURRENT_MAX_VALUE_HIGH_BYTE = ((uint8_t) 0x0E), + OFFSET_BATTERY_CURRENT_MAX_VALUE = ((uint8_t) 0x0F), + OFFSET_REGEN_CURRENT_MAX_VALUE = ((uint8_t) 0x10), + OFFSET_CURRENT_CAL_A = ((uint8_t) 0x11), + + OFFSET_ACA_FLAGS_HIGH_BYTE = ((uint8_t) 0x12), + OFFSET_ACA_FLAGS = ((uint8_t) 0x13), + + OFFSET_TQ_CALIB = ((uint8_t) 0x14), + OFFSET_CORRECTION_AT_ANGLE = ((uint8_t) 0x15), + + OFFSET_HALL_ANGLE_4_0 = ((uint8_t) 0x16), + OFFSET_HALL_ANGLE_6_60 = ((uint8_t) 0x17), + OFFSET_HALL_ANGLE_2_120 = ((uint8_t) 0x18), + OFFSET_HALL_ANGLE_3_180 = ((uint8_t) 0x19), + OFFSET_HALL_ANGLE_1_240 = ((uint8_t) 0x1A), + OFFSET_HALL_ANGLE_5_300 = ((uint8_t) 0x1B), + + OFFSET_BATTERY_VOLTAGE_CALIB = ((uint8_t) 0x1C), + OFFSET_MOTOR_CONSTANT = ((uint8_t) 0x1D), + + OFFSET_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE = ((uint8_t) 0x1E), + OFFSET_ACA_EXPERIMENTAL_FLAGS = ((uint8_t) 0x1F), + + OFFSET_ASSIST_PERCENT_LEVEL_1 = ((uint8_t) 0x20), + OFFSET_ASSIST_PERCENT_LEVEL_2 = ((uint8_t) 0x21), + OFFSET_ASSIST_PERCENT_LEVEL_3 = ((uint8_t) 0x22), + OFFSET_ASSIST_PERCENT_LEVEL_4 = ((uint8_t) 0x23), + OFFSET_ASSIST_PERCENT_LEVEL_5 = ((uint8_t) 0x24), + + OFFSET_BATTERY_VOLTAGE_MIN = ((uint8_t) 0x25), + OFFSET_BATTERY_VOLTAGE_MAX = ((uint8_t) 0x26), + + OFFSET_PASSCODE_HIGH_BYTE = ((uint8_t) 0x27), + OFFSET_PASSCODE = ((uint8_t) 0x28), + OFFSET_GEAR_RATIO = ((uint8_t) 0x29) + +} BO_EEPROM_OFFSETS; + +void eeprom_init(void); +uint8_t eeprom_write(uint8_t adress_offset, uint8_t value); +uint8_t eeprom_read(uint8_t address_offset); + +#endif /* BOEEPROM_H */ \ No newline at end of file diff --git a/ACAsetPoint.c b/ACAsetPoint.c new file mode 100644 index 00000000..055f9a85 --- /dev/null +++ b/ACAsetPoint.c @@ -0,0 +1,343 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAsetPoint.c + * Author: Björn Schmidt + * + * Created on September 7, 2018, 6:47 PM + */ + +#include +#include +#include "config.h" +#include "stm8s.h" +#include "stm8s_it.h" +#include "ACAsetPoint.h" +#include "ACAcontrollerState.h" +#include "ACAcommons.h" +// FIXME ugly cross references +// why? cause this blackbox is just for calculating, +// it's not supposed to read values on its own +// all values should be read by a slowloop_"readall/updatesensors, whatever" before calling it +#include "brake.h" +#include "adc.h" // FIXME ugly cross reference + +static uint32_t ui32_dutycycle; // local version of setpoint + +static int8_t uint_PWM_Enable = 0; //flag for PWM state +static uint16_t ui16_BatteryCurrent_accumulated = 2496L; //8x current offset, for filtering or Battery Current +static uint16_t ui16_BatteryVoltage_accumulated; +static uint16_t ui16_assist_percent_smoothed; +static uint32_t ui32_time_ticks_between_pas_interrupt_accumulated = 0; // for filtering of PAS value +static uint32_t ui32_erps_accumulated; //for filtering of erps +//static uint32_t ui32_speedlimit_actual_accumulated; +static uint32_t ui32_sumthrottle_accumulated; //it is already smoothed b4 we get it, we want to smooth it even more though for dynamic assist levels + +static float float_temp = 0; //for float calculations + +static uint32_t uint32_temp = 0; +static uint16_t uint16_temp = 0; +static uint16_t controll_state_temp = 0; +static uint8_t ui8_temp = 0; + +uint16_t cutoffSetpoint(uint32_t ui32_dutycycle) { + if (ui32_dutycycle < 5) { + ui32_dutycycle = 0; + } + if (ui32_dutycycle > 255) { + ui32_dutycycle = 255; + } + return ui32_dutycycle; +} + +BitStatus checkMaxErpsOverride(){ + if (ui32_erps_filtered > ui16_erps_max) { + ui32_dutycycle = PI_control(ui32_erps_filtered, ui16_erps_max,uint_PWM_Enable); //limit the erps to maximum value to have minimum 30 points of sine table for proper commutation + controll_state_temp +=1024; + return 1; + } + return 0; +} + +BitStatus checkUnderVoltageOverride(){ + //check for undervoltage --> ramp down power starting 6.25% above min + ui8_temp = ui8_s_battery_voltage_min + (ui8_s_battery_voltage_min>>4); + if (ui8_BatteryVoltage < ui8_temp) { + + uint32_current_target = map(ui8_BatteryVoltage, ui8_s_battery_voltage_min, ui8_temp, ui16_current_cal_b, uint32_current_target ); + ui32_dutycycle = PI_control(ui16_BatteryCurrent, uint32_current_target,uint_PWM_Enable); + controll_state_temp +=2048; + return 1; + } + return 0; +} + +BitStatus checkOverVoltageOverride(){ + //check for overvoltage --> ramp down regen starting 3.125% below max + ui8_temp = ui8_s_battery_voltage_max - (ui8_s_battery_voltage_max>>5); + if (ui8_BatteryVoltage > ui8_temp) { + + uint32_current_target = map(ui8_BatteryVoltage, ui8_temp, ui8_s_battery_voltage_max, uint32_current_target, ui16_current_cal_b ); + ui32_dutycycle = PI_control(ui16_BatteryCurrent, uint32_current_target,uint_PWM_Enable); + controll_state_temp +=4096; + return 1; + } + return 0; +} + +void aca_setpoint_init(void) { + ui32_time_ticks_between_pas_interrupt_accumulated = ((uint32_t)ui16_s_ramp_start)<<3; +} + +uint16_t aca_setpoint(uint16_t ui16_time_ticks_between_pas_interrupt, uint16_t setpoint_old) { + // select virtual erps speed based on speedsensor type + if (((ui16_aca_flags & EXTERNAL_SPEED_SENSOR) == EXTERNAL_SPEED_SENSOR)) { + ui16_virtual_erps_speed = (uint16_t) ((((uint32_t)ui8_gear_ratio) * ui32_speed_sensor_rpks) /1000); + }else{ + ui16_virtual_erps_speed = (uint16_t) ui32_erps_filtered; + } + + // first select current speed limit + if (ui8_offroad_state == 255) { + ui8_speedlimit_actual_kph = 80; + } else if (ui8_offroad_state > 15 && ui16_sum_throttle <= 2) { // allow a slight increase based on ui8_offroad_state + ui8_speedlimit_actual_kph = ui8_speedlimit_kph + (ui8_offroad_state - 16); + } else if (ui8_offroad_state > 15 && ui16_sum_throttle > 2) { + ui8_speedlimit_actual_kph = ui8_speedlimit_with_throttle_override_kph + (ui8_offroad_state - 16); + } else if (ui16_time_ticks_for_pas_calculation > timeout || !PAS_is_active) { + ui8_speedlimit_actual_kph = ui8_speedlimit_without_pas_kph; + } else { + ui8_speedlimit_actual_kph = ui8_speedlimit_kph; + } + + // >=8 means levels are switched of, use wanted percentage directly instead + ui16_assist_percent_smoothed -= ui16_assist_percent_smoothed >> 4; + if ((ui8_assistlevel_global & 15) < 8) { + ui16_assist_percent_smoothed += ui8_a_s_assistlevels[ui8_assistlevel_global & 15]; + } else { + ui16_assist_percent_smoothed += ui8_assist_percent_wanted; + } + ui8_assist_percent_actual = ui16_assist_percent_smoothed >> 4; + + + // average throttle over a longer time period (for dynamic assist level) + ui32_sumthrottle_accumulated -= ui32_sumthrottle_accumulated >> 10; + ui32_sumthrottle_accumulated += ui16_sum_throttle; + ui8_assist_dynamic_percent_addon = ui32_sumthrottle_accumulated >> 10; + if ((ui8_assist_dynamic_percent_addon + ui8_assist_percent_actual) > 100) { + ui8_assist_dynamic_percent_addon = 100 - ui8_assist_percent_actual; + } + + ui16_BatteryCurrent_accumulated -= ui16_BatteryCurrent_accumulated >> 3; + ui16_BatteryCurrent_accumulated += ui16_adc_read_motor_total_current(); + ui16_BatteryCurrent = ui16_BatteryCurrent_accumulated >> 3; + + ui16_BatteryVoltage_accumulated -= ui16_BatteryVoltage_accumulated >> 3; + ui16_BatteryVoltage_accumulated += ui8_adc_read_battery_voltage(); + ui8_BatteryVoltage = ui16_BatteryVoltage_accumulated >> 3; + + ui32_erps_accumulated -= ui32_erps_accumulated >> 3; + ui32_erps_accumulated += ui16_motor_speed_erps; + ui32_erps_filtered = ui32_erps_accumulated >> 3; + + ui32_time_ticks_between_pas_interrupt_accumulated -= ui32_time_ticks_between_pas_interrupt_accumulated >> 3; + // do not allow values > ramp_start into smoothing cause it makes startup sluggish + // also do not allow values < ramp_start when pedalling backwards + if ((!PAS_is_active||(ui16_time_ticks_between_pas_interrupt > ui16_s_ramp_start)) && ((ui16_aca_flags & TQ_SENSOR_MODE) != TQ_SENSOR_MODE)) { + ui32_time_ticks_between_pas_interrupt_accumulated += ui16_s_ramp_start; + } else { + ui32_time_ticks_between_pas_interrupt_accumulated += ui16_time_ticks_between_pas_interrupt; + } + ui16_time_ticks_between_pas_interrupt_smoothed = ui32_time_ticks_between_pas_interrupt_accumulated >> 3; + + // check for brake --> set regen current + if (brake_is_set()) { + + controll_state_temp = 255; + //Current target based on regen assist level + if ((ui16_aca_flags & DIGITAL_REGEN) == DIGITAL_REGEN) { + + ui8_temp = ui8_a_s_assistlevels[ui8_assistlevel_global >> 4]; + controll_state_temp -= 1; + + //Current target based on linear input on pad X4 + } else { + ui8_temp = map(ui16_x4_value >> 2, ui8_throttle_min_range, ui8_throttle_max_range, 0, 100); //map regen throttle to limits + controll_state_temp -= 2; + } + float_temp = (float) ui8_temp * (float) (ui16_regen_current_max_value) / 100.0; + + //Current target gets ramped down with speed + if (((ui16_aca_flags & SPEED_INFLUENCES_REGEN) == SPEED_INFLUENCES_REGEN) && (ui16_virtual_erps_speed < ((ui16_speed_kph_to_erps_ratio * ((uint16_t) ui8_speedlimit_kph)) / 100))) { + + if (ui16_virtual_erps_speed < 15) { + // turn of regen at low speeds + // based on erps in order to avoid an additional calculation + float_temp = 0.0; + } else { + + float_temp *= ((float) ui16_virtual_erps_speed / ((float) (ui16_speed_kph_to_erps_ratio * ((float) ui8_speedlimit_kph)) / 100.0)); // influence of current speed based on base speed limit + controll_state_temp -= 4; + } + } + + uint32_current_target = (uint32_t) ui16_current_cal_b - float_temp; + + if (!checkOverVoltageOverride()){ + ui32_dutycycle = PI_control(ui16_BatteryCurrent, uint32_current_target,uint_PWM_Enable); + } + + if (((ui16_aca_flags & BYPASS_LOW_SPEED_REGEN_PI_CONTROL) == BYPASS_LOW_SPEED_REGEN_PI_CONTROL) && (ui32_dutycycle == 0)) { + //try to get best regen at Low Speeds for BionX IGH + ui32_dutycycle = ui16_virtual_erps_speed * 2; + controll_state_temp -= 8; + } + + } else { + + uint32_current_target = ui16_current_cal_b; // reset target to zero + controll_state_temp = 0; + //if none of the overruling boundaries are concerned, calculate new setpoint + + // if torque sim is requested. We could check if we could solve this function with just one line with map function... + if (((ui16_aca_flags & TQ_SENSOR_MODE) != TQ_SENSOR_MODE)) { + + // add dynamic assist level based on past throttle input + ui8_temp = ui8_assist_percent_actual; + + if ((ui16_aca_flags & DYNAMIC_ASSIST_LEVEL) == DYNAMIC_ASSIST_LEVEL) { + ui8_temp += ui8_assist_dynamic_percent_addon; + } + + if (ui16_time_ticks_between_pas_interrupt_smoothed > ui16_s_ramp_end) { + //if you are pedaling slower than defined ramp end + //or not pedalling at all + //current is proportional to cadence + uint32_current_target = (ui8_temp * (ui16_battery_current_max_value) / 100); + float_temp = 1.0 - (((float) (ui16_time_ticks_between_pas_interrupt_smoothed - ui16_s_ramp_end)) / ((float) (ui16_s_ramp_start - ui16_s_ramp_end))); + uint32_current_target = ((uint16_t) (uint32_current_target)*(uint16_t) (float_temp * 100.0)) / 100 + ui16_current_cal_b; + controll_state_temp += 1; + + //in you are pedaling faster than in ramp end defined, desired battery current level is set, + } else { + uint32_current_target = (ui8_temp * (ui16_battery_current_max_value) / 100 + ui16_current_cal_b); + controll_state_temp += 2; + } + } else { // torque sensor mode + + + //erst mal alles aufmultiplizieren, damit beim Teilen was über 1 übrig bleibt. Bitte mal überschlagen, ob die int32 da nicht überlaufen kann... + uint32_temp = ui16_sum_torque; + uint32_temp *= ui8_assist_percent_actual; + uint32_temp *= ui16_battery_current_max_value; + uint32_temp *= uint32_torquesensorCalibration; + + uint32_temp /= ui16_time_ticks_between_pas_interrupt_smoothed; // hier lässt sich die geteilt-Operation nicht vermeiden :-( + + if(PAS_is_active) + uint32_current_target = (uint32_temp >>8) + (uint32_t) ui16_current_cal_b; //right shift 15 fasst die Operationen /100 (annähernd >>7) aus der assist_percent und /255 ( >>8) aus dem battery_current max zusammen, ist nicht ganz korrekt, ggf. nur >>14 nehmen -->/(256*128) vs. /(256*64) + else uint32_current_target =(uint32_t) ui16_current_cal_b; + + + controll_state_temp += 4; + + } + + + float_temp = 0.0; + // throttle / torquesensor override following + if (((ui16_aca_flags & TQ_SENSOR_MODE) != TQ_SENSOR_MODE)) { + if (ui8_speedlimit_kph > 1){ + // do not apply throttle at very low speed limits (technical restriction, speelimit can and should never be lover than 1) + float_temp = (float) ui16_sum_throttle; + } + } else { + float_temp = (float) ui16_momentary_throttle; // or ui16_sum_throttle + //float_temp *= (1 - (float) ui16_virtual_erps_speed / 2 / (float) (ui16_speed_kph_to_erps_ratio * ((float) ui8_speedlimit_kph))); //ramp down linear with speed. Risk: Value is getting negative if speed>2*speedlimit + + } + + // map curret target to assist level, not to maximum value + if ((ui16_aca_flags & ASSIST_LVL_AFFECTS_THROTTLE) == ASSIST_LVL_AFFECTS_THROTTLE) { + float_temp *= ((float) ui8_assist_percent_actual / 100.0); + controll_state_temp += 8; + } + + float_temp = float_temp * (float) (ui16_battery_current_max_value) / 255.0 + (float) ui16_current_cal_b; //calculate current target + + + if ((uint32_t) float_temp > uint32_current_target) { + + //override current target with throttle + uint32_current_target = (uint32_t) float_temp; + + controll_state_temp += 16; + } + + // check for overspeed + uint32_temp = uint32_current_target; + uint32_current_target = CheckSpeed((uint16_t) uint32_current_target, (uint16_t) ui16_virtual_erps_speed, (ui16_speed_kph_to_erps_ratio * ((uint16_t) ui8_speedlimit_actual_kph)) / 100, (ui16_speed_kph_to_erps_ratio * ((uint16_t) (ui8_speedlimit_actual_kph + 2))) / 100); //limit speed + if (uint32_temp != uint32_current_target) { + controll_state_temp += 32; + } + + if (uint32_current_target > ui16_battery_current_max_value + ui16_current_cal_b) { + uint32_current_target = ui16_battery_current_max_value + ui16_current_cal_b; + controll_state_temp += 64; + } + //phase current limiting + if (setpoint_old > 0 && (uint32_current_target - ui16_current_cal_b)*255 / setpoint_old > PHASE_CURRENT_MAX_VALUE) { // limit phase current according to Phase Current = battery current/duty cycle + uint32_current_target = (PHASE_CURRENT_MAX_VALUE) * setpoint_old / 255 + ui16_current_cal_b; + controll_state_temp += 128; + } + + // control power instead of current + if ((ui16_aca_flags & POWER_BASED_CONTROL) == POWER_BASED_CONTROL) { + // nominal voltage based on limits + ui8_temp = ((ui8_s_battery_voltage_max - ui8_s_battery_voltage_min)>>1)+ui8_s_battery_voltage_min; + uint32_current_target*=ui8_temp/ui8_BatteryVoltage; + } + + if ((ui16_aca_experimental_flags & DC_STATIC_ZERO) == DC_STATIC_ZERO) { + ui32_dutycycle = 0; + controll_state_temp += 256; + }else if (!checkUnderVoltageOverride() && !checkMaxErpsOverride()){ + + if (ui8_walk_assist) uint32_current_target = 10 + ui16_current_cal_b; + //send current target to PI-controller + ui32_dutycycle = PI_control(ui16_BatteryCurrent, uint32_current_target,uint_PWM_Enable); + } + + if ((ui16_aca_experimental_flags & PWM_AUTO_OFF) == PWM_AUTO_OFF) { + controll_state_temp += 512; + //disable PWM if enabled and no power is wanted + if (uint_PWM_Enable && ui32_erps_filtered == 0 && uint32_current_target == ui16_current_cal_b) { + TIM1_CtrlPWMOutputs(DISABLE); + uint_PWM_Enable = 0; + } + //enable PWM if disabled and voltage is 6.25% higher than min, some hysteresis and power is wanted + if (!uint_PWM_Enable && ui8_BatteryVoltage > (ui8_s_battery_voltage_min + (ui8_s_battery_voltage_min >>4)) && (uint32_current_target != ui16_current_cal_b)){ + TIM1_CtrlPWMOutputs(ENABLE); + uint_PWM_Enable = 1; + } + }else{ + + //enable PWM if disabled and voltage is 6.25% higher than min, some hysteresis + if (!uint_PWM_Enable && ui8_BatteryVoltage > (ui8_s_battery_voltage_min + (ui8_s_battery_voltage_min >>4))) { + TIM1_CtrlPWMOutputs(ENABLE); + uint_PWM_Enable = 1; + } + } + } + ui16_control_state = controll_state_temp; + return cutoffSetpoint(ui32_dutycycle); + +} diff --git a/ACAsetPoint.h b/ACAsetPoint.h new file mode 100644 index 00000000..8c710d4e --- /dev/null +++ b/ACAsetPoint.h @@ -0,0 +1,28 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: ACAsetPoint.h + * Author: Björn Schmidt + * + * Created on September 7, 2018, 6:47 PM + */ + + +#ifndef ACASETPOINT_H +#define ACASETPOINT_H + +#include "config.h" + +uint16_t aca_setpoint (uint16_t ui16_time_ticks_between_pas_interrupt, uint16_t setpoint_old ); +void aca_setpoint_init(void); + +#endif /* ACASETPOINT_H */ + diff --git a/BOdisplay.c b/BOdisplay.c new file mode 100644 index 00000000..c84917c7 --- /dev/null +++ b/BOdisplay.c @@ -0,0 +1,683 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: BOdisplay.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + + +#include +#include "uart.h" +#include "config.h" +#include "stm8s.h" +#include "stm8s_itc.h" +#include "BOdisplay.h" +#include "interrupts.h" +#include "ACAeeprom.h" +#include "brake.h" // ugly crossrefernce for brake_is_set(), FIXME +#include "ACAcontrollerState.h" +#include "ACAcommons.h" + +#ifdef BLUOSEC + +// example +//:304100305F\r\n +// 0 A 0 (as chars) +uint8_t ui8_rx_buffer[17]; // modbus ascii with max 8 bytes payload (array including padding) // modbus rtu uses only 11 bytes +uint8_t ui8_tx_buffer[65]; // (max 30*8bit key + 30*8bit data points + bounced checksum(+ key) + address + function + checksum) (array excluding padding) +uint8_t ui8_rx_converted_buffer[7]; // for decoded ascii values + +uint8_t ui8_rx_buffer_counter = 0; +uint8_t ui8_tx_buffer_counter = 0; + +uint8_t int2hex(uint8_t in) { + if (in <= 9) + return in + 0x30; + return 0x41 - 10 + in; +} + +uint8_t hex2int(uint8_t ch) { + if (ch >= 0x30 && ch <= 0x39) + return ch - 0x30; + if (ch >= 0x41 && ch <= 0x46) + return ch - 0x41 + 10; + if (ch >= 0x61 && ch <= 0x66) + return ch - 0x61 + 10; + return 0; +} + +uint8_t calcLRC(uint8_t ints[], uint8_t start, uint8_t end) { + uint8_t i; + uint8_t LRC = 0; + for (i = start; i < end; i++) { + LRC = LRC + ints[i]; + //printf("LI %u\r\n",LRC); + } + //printf("LIX %u\r\n",LRC); + return (~LRC) + 1; +} + +void sendPreparedAsciiPackage(void) { + uint8_t j; + uart_put_buffered(0x3A); + for (j = 0; j < ui8_tx_buffer_counter; j++) { + uart_put_buffered(int2hex(ui8_tx_buffer[j] >> 4)); + uart_put_buffered(int2hex(ui8_tx_buffer[j]&15)); + } + uart_put_buffered(0x0D); + uart_put_buffered(0x0A); +} + +void sendPreparedRtuPackage(void) { + uint8_t j; + for (j = 0; j < ui8_tx_buffer_counter; j++) { + uart_put_buffered(ui8_tx_buffer[j]); + } + uart_put_buffered(0x00); + uart_put_buffered(0x00); + uart_put_buffered(0x00); + uart_put_buffered(0x00); +} + +void sendPreparedPackage(void) { + //sendPreparedAsciiPackage(); + sendPreparedRtuPackage(); +} + +void addPayload(uint8_t id, uint8_t value) { + ui8_tx_buffer[ui8_tx_buffer_counter++] = id; + ui8_tx_buffer[ui8_tx_buffer_counter++] = value; +} + +void prepareBasePackage(uint8_t address, uint8_t function) { + ui8_tx_buffer_counter = 0; + ui8_tx_buffer[ui8_tx_buffer_counter++] = address; + ui8_tx_buffer[ui8_tx_buffer_counter++] = function; + +} + +void signPackage(void) { + ui8_tx_buffer[ui8_tx_buffer_counter] = calcLRC(ui8_tx_buffer, 0, ui8_tx_buffer_counter); + ui8_tx_buffer_counter++; +} + +void addConfigStateInfosA(void) { + + // float casts might be costly but they are only requested once every 10 seconds + addPayload(CODE_ERPS_FACTOR, ui8_gear_ratio); + addPayload(CODE_WHEEL_CIRCUMFENCE_HIGH_BYTE, wheel_circumference >>8); + addPayload(CODE_WHEEL_CIRCUMFENCE, wheel_circumference); + addPayload(CODE_CURRENT_CAL_A, ui8_current_cal_a); + addPayload(CODE_CURRENT_CAL_B_HIGH_BYTE, ui16_current_cal_b >> 8); + addPayload(CODE_CURRENT_CAL_B, ui16_current_cal_b); + addPayload(CODE_EEPROM_MAGIC_BYTE, eeprom_magic_byte); + addPayload(CODE_MAX_SPEED_DEFAULT, ui8_speedlimit_kph); + addPayload(CODE_MAX_SPEED_WITHOUT_PAS, ui8_speedlimit_without_pas_kph); + addPayload(CODE_MAX_SPEED_WITH_THROTTLE_OVERRIDE, ui8_speedlimit_with_throttle_override_kph); + addPayload(CODE_ACA_FLAGS_HIGH_BYTE, ui16_aca_flags >> 8); + addPayload(CODE_ACA_FLAGS, ui16_aca_flags); + addPayload(CODE_THROTTLE_MIN_RANGE, ui8_throttle_min_range); + addPayload(CODE_THROTTLE_MAX_RANGE, ui8_throttle_max_range); + addPayload(CODE_MOTOR_SPECIFIC_ANGLE, ui8_s_motor_angle); + addPayload(CODE_PAS_TRESHOLD, float2int(flt_s_pas_threshold, 4.0)); + addPayload(CODE_PID_GAIN_P, float2int(flt_s_pid_gain_p, 2.0)); + addPayload(CODE_PID_GAIN_I, float2int(flt_s_pid_gain_i, 2.0)); + addPayload(CODE_TQ_CALIB, float2int(flt_torquesensorCalibration, 8000.0)); + addPayload(CODE_RAMP_END, ui16_s_ramp_end >> 5); + addPayload(CODE_RAMP_START, ui16_s_ramp_start >> 6); + addPayload(CODE_MAX_BAT_CURRENT_HIGH_BYTE, ui16_battery_current_max_value >> 8); + addPayload(CODE_MAX_BAT_CURRENT, ui16_battery_current_max_value); + addPayload(CODE_CORRECTION_AT_ANGLE, ui8_correction_at_angle); + + // 5 more elements left/avail (max30) + +} + +void addConfigStateInfosB(void) { + + addPayload(CODE_HALL_ANGLE_4_0, ui8_s_hall_angle4_0); + addPayload(CODE_HALL_ANGLE_6_60, ui8_s_hall_angle6_60); + addPayload(CODE_HALL_ANGLE_2_120, ui8_s_hall_angle2_120); + addPayload(CODE_HALL_ANGLE_3_180, ui8_s_hall_angle3_180); + addPayload(CODE_HALL_ANGLE_1_240, ui8_s_hall_angle1_240); + addPayload(CODE_HALL_ANGLE_5_300, ui8_s_hall_angle5_300); + + addPayload(CODE_ASSIST_PERCENT_LEVEL_1, ui8_a_s_assistlevels[1]); + addPayload(CODE_ASSIST_PERCENT_LEVEL_2, ui8_a_s_assistlevels[2]); + addPayload(CODE_ASSIST_PERCENT_LEVEL_3, ui8_a_s_assistlevels[3]); + addPayload(CODE_ASSIST_PERCENT_LEVEL_4, ui8_a_s_assistlevels[4]); + addPayload(CODE_ASSIST_PERCENT_LEVEL_5, ui8_a_s_assistlevels[5]); + + addPayload(CODE_MAX_REGEN_CURRENT, ui16_regen_current_max_value); + addPayload(CODE_ADC_BATTERY_VOLTAGE_CALIB, ui8_s_battery_voltage_calibration); + addPayload(CODE_ADC_BATTERY_VOLTAGE_MIN, ui8_s_battery_voltage_min); + addPayload(CODE_ADC_BATTERY_VOLTAGE_MAX, ui8_s_battery_voltage_max); + + addPayload(CODE_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE, ui16_aca_experimental_flags >> 8); + addPayload(CODE_ACA_EXPERIMENTAL_FLAGS, ui16_aca_experimental_flags); + + addPayload(CODE_MOTOR_CONSTANT, float2int(flt_s_motor_constant, 4.0)); + // 12 more elements left/avail (max30) +} + +void addHallStateInfos(void) { + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x00, uint8_t_hall_case[0]); + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x01, uint8_t_hall_case[1]); + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x02, uint8_t_hall_case[2]); + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x03, uint8_t_hall_case[3]); + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x04, uint8_t_hall_case[4]); + addPayload(CODE_CURRENT_AT_HALL_POSITION_BASE + 0x05, uint8_t_hall_case[5]); + addPayload(CODE_HALL_ORDER_BASE + 0x00, uint8_t_hall_order[0]); + addPayload(CODE_HALL_ORDER_BASE + 0x01, uint8_t_hall_order[1]); + addPayload(CODE_HALL_ORDER_BASE + 0x02, uint8_t_hall_order[2]); + addPayload(CODE_HALL_ORDER_BASE + 0x03, uint8_t_hall_order[3]); + addPayload(CODE_HALL_ORDER_BASE + 0x04, uint8_t_hall_order[4]); + addPayload(CODE_HALL_ORDER_BASE + 0x05, uint8_t_hall_order[5]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x00, uint8_t_60deg_pwm_cycles[0]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x01, uint8_t_60deg_pwm_cycles[1]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x02, uint8_t_60deg_pwm_cycles[2]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x03, uint8_t_60deg_pwm_cycles[3]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x04, uint8_t_60deg_pwm_cycles[4]); + addPayload(CODE_60_DEG_PWM_CYCLES + 0x05, uint8_t_60deg_pwm_cycles[5]); + + addPayload(CODE_VAR_DEBUG_A, ui8_variableDebugA); + addPayload(CODE_VAR_DEBUG_B, ui8_variableDebugB); + addPayload(CODE_VAR_DEBUG_C, ui8_variableDebugC); + + // 9 more elements left/avail (max30) +} + +void addDetailStateInfos(void) { + addPayload(CODE_OFFROAD, ui8_offroad_state); + addPayload(CODE_PAS_ACTIVE, PAS_act); + addPayload(CODE_PAS_DIR, PAS_is_active); + addPayload(CODE_CORRECTION_VALUE, ui8_position_correction_value); + addPayload(CODE_PHASE_CURRENT, ui16_ADC_iq_current >> 2); + addPayload(CODE_THROTTLE_HIGH_BYTE, ui16_momentary_throttle >> 8); + addPayload(CODE_THROTTLE, ui16_momentary_throttle); + addPayload(CODE_CURRENT_TARGET_HIGH_BYTE, uint32_current_target >> 8); + addPayload(CODE_CURRENT_TARGET, uint32_current_target); + addPayload(CODE_CURRENT_RAMP_HIGH_BYTE, ui16_time_ticks_between_pas_interrupt_smoothed >> 8); + addPayload(CODE_CURRENT_RAMP, ui16_time_ticks_between_pas_interrupt_smoothed); + addPayload(CODE_PAS_HIGH_COUNTER_HIGH_BYTE, ui16_PAS_High >> 8); + addPayload(CODE_PAS_HIGH_COUNTER, ui16_PAS_High); + addPayload(CODE_PAS_COUNTER_HIGH_BYTE, ui16_time_ticks_between_pas_interrupt >> 8); + addPayload(CODE_PAS_COUNTER, ui16_time_ticks_between_pas_interrupt); + addPayload(CODE_LOCKSTATUS, ui8_lockstatus); + // 10 more elements left/avail (max30) +} + +void addBasicStateInfos(void) { + addPayload(CODE_ACTUAL_MAX_SPEED, ui8_speedlimit_actual_kph); + addPayload(CODE_ASSIST_LEVEL, ui8_assistlevel_global); + addPayload(CODE_ASSIST_LEVEL_SMOOTHED_PERCENT, ui8_assist_percent_actual); + addPayload(CODE_ASSIST_PERCENT_WANTED, ui8_assist_percent_wanted); + addPayload(CODE_ASSIST_LEVEL_DYNAMIC_ADDON, ui8_assist_dynamic_percent_addon); + addPayload(CODE_BRAKE_STATUS, (int) brake_is_set()); + addPayload(CODE_MOTOR_STATE, ui8_possible_motor_state | ui8_dynamic_motor_state<<4); + addPayload(CODE_BATTERY_VOLTAGE, ui8_BatteryVoltage); + addPayload(CODE_ER_SPEED_HIGH_BYTE, ui16_motor_speed_erps >> 8); + addPayload(CODE_ER_SPEED, ui16_motor_speed_erps); + addPayload(CODE_SENSOR_RPKS_HIGH_BYTE, ui32_speed_sensor_rpks >> 8); + addPayload(CODE_SENSOR_RPKS, ui32_speed_sensor_rpks); + addPayload(CODE_BATTERY_CURRENT_HIGH_BYTE, ui16_BatteryCurrent >> 8); + addPayload(CODE_BATTERY_CURRENT, ui16_BatteryCurrent); + addPayload(CODE_SUM_TORQUE, ui16_sum_torque); + addPayload(CODE_SUM_THROTTLE, ui16_sum_throttle); + addPayload(CODE_SETPOINT, ui16_setpoint); + addPayload(CODE_SETPOINT_STATE_HIGH_BYTE, ui16_control_state >> 8); + addPayload(CODE_SETPOINT_STATE, ui16_control_state); + addPayload(CODE_UPTIME, ui8_uptime); + addPayload(CODE_X4VALUE_HIGH_BYTE, ui16_x4_value >> 8); + addPayload(CODE_X4VALUE, ui16_x4_value); + addPayload(CODE_VER_SPEED_HIGH_BYTE, ui16_virtual_erps_speed >> 8); + addPayload(CODE_VER_SPEED, ui16_virtual_erps_speed); + // 4 more elements left/avail (max30) +} + +void gatherDynamicPayload(uint8_t function) { + switch (function) { + case FUN_BASIC_INFOS: + addBasicStateInfos(); + break; + case FUN_DETAIL_INFOS: + addDetailStateInfos(); + break; + case FUN_HALL_INFOS: + addHallStateInfos(); + break; + default: + addPayload(CODE_ERROR, CODE_ERROR); + } +} + +void gatherStaticPayload(uint8_t function) { + switch (function) { + case FUN_CONFIG_INFOS_A: + addConfigStateInfosA(); + break; + case FUN_CONFIG_INFOS_B: + addConfigStateInfosB(); + break; + default: + addPayload(CODE_ERROR, CODE_ERROR); + } +} + +void digestConfigRequest(uint8_t configAddress, uint8_t requestedCodeLowByte, uint8_t requestedValueHighByte, uint8_t requestedValue) { + + + switch (requestedCodeLowByte) { + case CODE_ERPS_FACTOR: + ui8_gear_ratio = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_GEAR_RATIO, requestedValue); + } + // special processing, reinit ratio + initErpsRatio(); + addPayload(requestedCodeLowByte, ui8_gear_ratio); + break; + case CODE_OFFROAD: + ui8_offroad_state = requestedValue; + addPayload(requestedCodeLowByte, ui8_offroad_state); + break; + case CODE_PASSCODE: + if ((ui8_lockstatus == 16) && (configAddress == EEPROM_ADDRESS)){ + // write new passcode only if unlocked + ui16_passcode = ((uint16_t) requestedValueHighByte << 8)+(uint16_t) requestedValue; + eeprom_write(OFFSET_PASSCODE_HIGH_BYTE, requestedValueHighByte); + eeprom_write(OFFSET_PASSCODE, requestedValue); + addPayload(CODE_PASSCODE_HIGH_BYTE, ui16_passcode >> 8); + addPayload(requestedCodeLowByte, ui16_passcode); + }else if ((configAddress != EEPROM_ADDRESS) && (ui16_passcode == (((uint16_t) requestedValueHighByte << 8)+(uint16_t) requestedValue))){ + // unlock if correct code was sent + ui16_no_pass_counter = 0; + ui8_lockstatus = 16; + addPayload(CODE_PASSCODE_HIGH_BYTE, ui16_passcode >> 8); + addPayload(requestedCodeLowByte, ui16_passcode); + }else{ + addPayload(CODE_ERROR, CODE_ERROR); + } + break; + case CODE_ACA_FLAGS: + ui16_aca_flags = ((uint16_t) requestedValueHighByte << 8)+(uint16_t) requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ACA_FLAGS_HIGH_BYTE, requestedValueHighByte); + eeprom_write(OFFSET_ACA_FLAGS, requestedValue); + } + addPayload(CODE_ACA_FLAGS_HIGH_BYTE, ui16_aca_flags >> 8); + addPayload(requestedCodeLowByte, ui16_aca_flags); + break; + case CODE_ACA_EXPERIMENTAL_FLAGS: + ui16_aca_experimental_flags = ((uint16_t) requestedValueHighByte << 8)+(uint16_t) requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE, requestedValueHighByte); + eeprom_write(OFFSET_ACA_EXPERIMENTAL_FLAGS, requestedValue); + } + addPayload(CODE_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE, ui16_aca_experimental_flags >> 8); + addPayload(requestedCodeLowByte, ui16_aca_experimental_flags); + break; + case CODE_MAX_BAT_CURRENT: + ui16_battery_current_max_value = ((uint16_t) requestedValueHighByte << 8)+(uint16_t) requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_BATTERY_CURRENT_MAX_VALUE_HIGH_BYTE, requestedValueHighByte); + eeprom_write(OFFSET_BATTERY_CURRENT_MAX_VALUE, requestedValue); + } + addPayload(CODE_MAX_BAT_CURRENT_HIGH_BYTE, ui16_battery_current_max_value >> 8); + addPayload(requestedCodeLowByte, ui16_battery_current_max_value); + break; + case CODE_MAX_REGEN_CURRENT: + ui16_regen_current_max_value = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_REGEN_CURRENT_MAX_VALUE, requestedValue); + } + addPayload(requestedCodeLowByte, ui16_regen_current_max_value); + break; + case CODE_CURRENT_CAL_A: + ui8_current_cal_a = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_CURRENT_CAL_A, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_current_cal_a); + break; + case CODE_ADC_BATTERY_VOLTAGE_CALIB: + ui8_s_battery_voltage_calibration = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_BATTERY_VOLTAGE_CALIB, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_battery_voltage_calibration); + break; + case CODE_ADC_BATTERY_VOLTAGE_MIN: + ui8_s_battery_voltage_min = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_BATTERY_VOLTAGE_MIN, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_battery_voltage_min); + break; + case CODE_ADC_BATTERY_VOLTAGE_MAX: + ui8_s_battery_voltage_max = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_BATTERY_VOLTAGE_MAX, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_battery_voltage_max); + break; + case CODE_ASSIST_PERCENT_LEVEL_1: + ui8_a_s_assistlevels[1] = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_LEVEL_1, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_a_s_assistlevels[1]); + break; + case CODE_ASSIST_PERCENT_LEVEL_2: + ui8_a_s_assistlevels[2] = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_LEVEL_2, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_a_s_assistlevels[2]); + break; + case CODE_ASSIST_PERCENT_LEVEL_3: + ui8_a_s_assistlevels[3] = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_LEVEL_3, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_a_s_assistlevels[3]); + break; + case CODE_ASSIST_PERCENT_LEVEL_4: + ui8_a_s_assistlevels[4] = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_LEVEL_4, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_a_s_assistlevels[4]); + break; + case CODE_ASSIST_PERCENT_LEVEL_5: + ui8_a_s_assistlevels[5] = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_LEVEL_5, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_a_s_assistlevels[5]); + break; + case CODE_ASSIST_LEVEL: + ui8_assistlevel_global = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_LEVEL, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_assistlevel_global); + break; + case CODE_ASSIST_PERCENT_WANTED: + // also sends new (fake) level in high byte + ui8_assist_percent_wanted = requestedValue; + ui8_assistlevel_global = requestedValueHighByte; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_ASSIST_PERCENT_WANTED, requestedValue); + eeprom_write(OFFSET_ASSIST_LEVEL, requestedValueHighByte); + } + addPayload(requestedCodeLowByte, ui8_assist_percent_wanted); + addPayload(CODE_ASSIST_LEVEL, ui8_assistlevel_global); + break; + case CODE_THROTTLE_MIN_RANGE: + ui8_throttle_min_range = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_THROTTLE_MIN_RANGE, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_throttle_min_range); + break; + case CODE_THROTTLE_MAX_RANGE: + ui8_throttle_max_range = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_THROTTLE_MAX_RANGE, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_throttle_max_range); + break; + + case CODE_MOTOR_SPECIFIC_ANGLE: + ui8_s_motor_angle = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_MOTOR_ANGLE, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_motor_angle); + break; + case CODE_CORRECTION_AT_ANGLE: + ui8_correction_at_angle = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_CORRECTION_AT_ANGLE, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_correction_at_angle); + break; + case CODE_HALL_ANGLE_4_0: + ui8_s_hall_angle4_0 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_4_0, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle4_0); + break; + case CODE_HALL_ANGLE_6_60: + ui8_s_hall_angle6_60 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_6_60, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle6_60); + break; + case CODE_HALL_ANGLE_2_120: + ui8_s_hall_angle2_120 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_2_120, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle2_120); + break; + case CODE_HALL_ANGLE_3_180: + ui8_s_hall_angle3_180 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_3_180, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle3_180); + break; + case CODE_HALL_ANGLE_1_240: + ui8_s_hall_angle1_240 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_1_240, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle1_240); + break; + case CODE_HALL_ANGLE_5_300: + ui8_s_hall_angle5_300 = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_HALL_ANGLE_5_300, requestedValue); + } + addPayload(requestedCodeLowByte, ui8_s_hall_angle5_300); + break; + + case CODE_PAS_TRESHOLD: + flt_s_pas_threshold = int2float(requestedValue, 4.0); + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_PAS_TRESHOLD, requestedValue); + } + addPayload(requestedCodeLowByte, float2int(flt_s_pas_threshold, 4.0)); + break; + case CODE_MOTOR_CONSTANT: + flt_s_motor_constant = int2float(requestedValue, 4.0); + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_MOTOR_CONSTANT, requestedValue); + } + addPayload(requestedCodeLowByte, float2int(flt_s_motor_constant, 4.0)); + break; + case CODE_PID_GAIN_P: + flt_s_pid_gain_p = int2float(requestedValue, 2.0); + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_PID_GAIN_P, requestedValue); + } + addPayload(requestedCodeLowByte, float2int(flt_s_pid_gain_p, 2.0)); + break; + case CODE_PID_GAIN_I: + flt_s_pid_gain_i = int2float(requestedValue, 2.0); + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_PID_GAIN_I, requestedValue); + } + addPayload(requestedCodeLowByte, float2int(flt_s_pid_gain_i, 2.0)); + break; + case CODE_RAMP_END: + ui16_s_ramp_end = requestedValue << 5; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_RAMP_END, requestedValue); + } + addPayload(requestedCodeLowByte, ui16_s_ramp_end >> 5); + break; + case CODE_TQ_CALIB: + flt_torquesensorCalibration = int2float(requestedValue, 8000.0); + uint32_torquesensorCalibration = (uint32_t)flt_torquesensorCalibration; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_TQ_CALIB, requestedValue); + } + addPayload(requestedCodeLowByte, float2int(flt_torquesensorCalibration, 8000.0)); + break; + case CODE_RAMP_START: + ui16_s_ramp_start = requestedValue << 6; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_RAMP_START, requestedValue); + } + addPayload(requestedCodeLowByte, ui16_s_ramp_start >> 6); + break; + case CODE_MAX_SPEED_DEFAULT: + ui8_speedlimit_kph = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_MAX_SPEED_DEFAULT, requestedValue); + } + setSignal(SIGNAL_SPEEDLIMIT_CHANGED); + addPayload(requestedCodeLowByte, ui8_speedlimit_kph); + break; + case CODE_MAX_SPEED_WITHOUT_PAS: + ui8_speedlimit_without_pas_kph = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_MAX_SPEED_WITHOUT_PAS, requestedValue); + } + setSignal(SIGNAL_SPEEDLIMIT_CHANGED); + addPayload(requestedCodeLowByte, ui8_speedlimit_without_pas_kph); + break; + case CODE_MAX_SPEED_WITH_THROTTLE_OVERRIDE: + ui8_speedlimit_with_throttle_override_kph = requestedValue; + if (configAddress == EEPROM_ADDRESS) { + eeprom_write(OFFSET_MAX_SPEED_WITH_THROTTLE_OVERRIDE, requestedValue); + } + setSignal(SIGNAL_SPEEDLIMIT_CHANGED); + addPayload(requestedCodeLowByte, ui8_speedlimit_with_throttle_override_kph); + break; + + + default: + addPayload(CODE_ERROR, CODE_ERROR); + + } + +} + +void display_init() { + // noop just here to have a common interface +} + +uint8_t readRtu() { + uart_fill_rx_packet_buffer(ui8_rx_buffer, 11, &ui8_rx_buffer_counter); + if (ui8_rx_buffer_counter == 11) { + ui8_rx_converted_buffer[0] = ui8_rx_buffer[0]; + ui8_rx_converted_buffer[1] = ui8_rx_buffer[1]; + ui8_rx_converted_buffer[2] = ui8_rx_buffer[2]; + ui8_rx_converted_buffer[3] = ui8_rx_buffer[3]; + ui8_rx_converted_buffer[4] = ui8_rx_buffer[4]; + ui8_rx_converted_buffer[5] = ui8_rx_buffer[5]; + ui8_rx_converted_buffer[6] = ui8_rx_buffer[6]; + // allow fetching of new data + ui8_rx_buffer_counter = 0; + return 1; + } + return 0; +} + +uint8_t readAscii() { + uart_fill_rx_packet_buffer(ui8_rx_buffer, 17, &ui8_rx_buffer_counter); + if (ui8_rx_buffer_counter == 17) { + ui8_rx_converted_buffer[0] = (hex2int(ui8_rx_buffer[1]) << 4) + hex2int(ui8_rx_buffer[2]); + ui8_rx_converted_buffer[1] = (hex2int(ui8_rx_buffer[3]) << 4) + hex2int(ui8_rx_buffer[4]); + ui8_rx_converted_buffer[2] = (hex2int(ui8_rx_buffer[5]) << 4) + hex2int(ui8_rx_buffer[6]); + ui8_rx_converted_buffer[3] = (hex2int(ui8_rx_buffer[7]) << 4) + hex2int(ui8_rx_buffer[8]); + ui8_rx_converted_buffer[4] = (hex2int(ui8_rx_buffer[9]) << 4) + hex2int(ui8_rx_buffer[10]); + ui8_rx_converted_buffer[5] = (hex2int(ui8_rx_buffer[11]) << 4) + hex2int(ui8_rx_buffer[12]); + ui8_rx_converted_buffer[6] = (hex2int(ui8_rx_buffer[13]) << 4) + hex2int(ui8_rx_buffer[14]); + // allow fetching of new data + ui8_rx_buffer_counter = 0; + return 1; + } + return 0; +} + +uint8_t readUart() { + return readRtu(); +} + +void display_update() { + + if (readUart()) { + + uint8_t calculatedLrc; + calculatedLrc = calcLRC(ui8_rx_converted_buffer, 0, 6); + + + if (calculatedLrc == ui8_rx_converted_buffer[6]) { + uint8_t requestedFunction = ui8_rx_converted_buffer[1]; + uint8_t requestedCodeLowByte = ui8_rx_converted_buffer[4]; + uint8_t requestedValueHighByte = ui8_rx_converted_buffer[3]; + uint8_t requestedValue = ui8_rx_converted_buffer[5]; + + if (ui8_rx_converted_buffer[0] == DYNAMIC_DATA_ADDRESS) { + // dynamic data requests also sends the displays assist percentage + // temporary check for backwards compatibility + if (CODE_DUMMY != requestedValue) + ui8_assist_percent_wanted = requestedValue; + prepareBasePackage(DYNAMIC_DATA_ADDRESS, requestedFunction); + gatherDynamicPayload(requestedFunction); + addPayload(CODE_LRC_CHECK, calculatedLrc); + signPackage(); + sendPreparedPackage(); + } else if (ui8_rx_converted_buffer[0] == STATIC_DATA_ADDRESS) { + // static data requests also sends the displays assist configuration + ui8_assist_percent_wanted = requestedValue; + ui8_assistlevel_global = requestedValueHighByte; + prepareBasePackage(STATIC_DATA_ADDRESS, requestedFunction); + gatherStaticPayload(requestedFunction); + addPayload(CODE_LRC_CHECK, calculatedLrc); + signPackage(); + sendPreparedPackage(); + } else if ((ui8_rx_converted_buffer[0] == CONFIG_ADDRESS) || (ui8_rx_converted_buffer[0] == EEPROM_ADDRESS)) { + prepareBasePackage(ui8_rx_converted_buffer[0], requestedFunction); + digestConfigRequest(ui8_rx_converted_buffer[0], requestedCodeLowByte, requestedValueHighByte, requestedValue); + addPayload(CODE_LRC_CHECK, calculatedLrc); + signPackage(); + sendPreparedPackage(); + } + } else { + // let sender know what was received and what the correct lrc would have been + prepareBasePackage(ERROR_ADDRESS, ui8_rx_converted_buffer[1]); + addPayload(CODE_ERROR, ui8_rx_converted_buffer[0]); + addPayload(CODE_ERROR + 1, ui8_rx_converted_buffer[1]); + addPayload(CODE_ERROR + 2, ui8_rx_converted_buffer[2]); + addPayload(CODE_ERROR + 3, ui8_rx_converted_buffer[3]); + addPayload(CODE_ERROR + 4, ui8_rx_converted_buffer[4]); + addPayload(CODE_ERROR + 5, ui8_rx_converted_buffer[5]); + addPayload(CODE_ERROR + 6, ui8_rx_converted_buffer[6]); + addPayload(CODE_LRC_CHECK, calculatedLrc); + signPackage(); + sendPreparedPackage(); + } + + } +} + + +#endif // BLUOSEC diff --git a/BOdisplay.h b/BOdisplay.h new file mode 100644 index 00000000..9b648ee6 --- /dev/null +++ b/BOdisplay.h @@ -0,0 +1,179 @@ +/* + * Copyright (c) 2018 Björn Schmidt + * + * 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, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + * File: BOdisplay.h + * Author: Björn Schmidt + * + * Created on August 30, 2018, 8:03 PM + */ + +#ifndef BODISPLAY_H +#define BODISPLAY_H + +#include "config.h" + +typedef enum { + FUN_ERROR = ((uint8_t) 0x66), + FUN_DETAIL_INFOS = ((uint8_t) 0x40), + FUN_BASIC_INFOS = ((uint8_t) 0x41), + FUN_CONFIG_INFOS_A = ((uint8_t) 0x42), + FUN_HALL_INFOS = ((uint8_t) 0x43), + FUN_SET_CONFIG = ((uint8_t) 0x44), + FUN_CONFIG_INFOS_B = ((uint8_t) 0x45), + FUN_NOOP = ((uint8_t) 0x46) +} BO_FUN_CODES; + +typedef enum { + // do not use values below 0x60 (reserved for array types) + + CODE_DUMMY_HIGH_BYTE = ((uint8_t) 0x64), + CODE_DUMMY = ((uint8_t) 0xFF), + CODE_LRC_CHECK = ((uint8_t) 0x60), + CODE_ERROR = ((uint8_t) 0x66), + + CODE_ASSIST_LEVEL = ((uint8_t) 0xA0), + CODE_BRAKE_STATUS = ((uint8_t) 0xA1), + CODE_PAS_ACTIVE = ((uint8_t) 0xA2), + CODE_PAS_DIR = ((uint8_t) 0xA3), + CODE_OFFROAD = ((uint8_t) 0xA4), + + CODE_VER_SPEED_HIGH_BYTE = ((uint8_t) 0xA5), + CODE_VER_SPEED = ((uint8_t) 0xA6), + + CODE_ASSIST_LEVEL_DYNAMIC_ADDON = ((uint8_t) 0xA7), + CODE_ASSIST_LEVEL_SMOOTHED_PERCENT = ((uint8_t) 0xA8), + CODE_ASSIST_PERCENT_WANTED = ((uint8_t) 0xA9), + + CODE_HALL_ANGLE_4_0 = ((uint8_t) 0xAA), + CODE_HALL_ANGLE_6_60 = ((uint8_t) 0xAB), + CODE_HALL_ANGLE_2_120 = ((uint8_t) 0xAC), + CODE_HALL_ANGLE_3_180 = ((uint8_t) 0xAD), + CODE_HALL_ANGLE_1_240 = ((uint8_t) 0xAE), + CODE_HALL_ANGLE_5_300 = ((uint8_t) 0xAF), + + CODE_ASSIST_PERCENT_LEVEL_1 = ((uint8_t) 0xB0), + CODE_ASSIST_PERCENT_LEVEL_2 = ((uint8_t) 0xB1), + CODE_ASSIST_PERCENT_LEVEL_3 = ((uint8_t) 0xB2), + CODE_ASSIST_PERCENT_LEVEL_4 = ((uint8_t) 0xB3), + CODE_ASSIST_PERCENT_LEVEL_5 = ((uint8_t) 0xB4), + + CODE_WHEEL_CIRCUMFENCE_HIGH_BYTE = ((uint8_t) 0xBA), + CODE_WHEEL_CIRCUMFENCE = ((uint8_t) 0xBB), + + CODE_X4VALUE_HIGH_BYTE = ((uint8_t) 0xBC), + CODE_X4VALUE = ((uint8_t) 0xBD), + + CODE_PASSCODE_HIGH_BYTE = ((uint8_t) 0xBE), + CODE_PASSCODE = ((uint8_t) 0xBF), + + CODE_MOTOR_STATE = ((uint8_t) 0xC0), + CODE_BATTERY_VOLTAGE = ((uint8_t) 0xC1), + CODE_UPTIME = ((uint8_t) 0xC2), + + CODE_BATTERY_CURRENT_HIGH_BYTE = ((uint8_t) 0xC4), + CODE_BATTERY_CURRENT = ((uint8_t) 0xC5), + CODE_CORRECTION_VALUE = ((uint8_t) 0xC6), + CODE_PHASE_CURRENT = ((uint8_t) 0xC7), + + CODE_SENSOR_RPKS_HIGH_BYTE = ((uint8_t) 0xC8), + CODE_SENSOR_RPKS = ((uint8_t) 0xC9), + + CODE_ER_SPEED_HIGH_BYTE = ((uint8_t) 0xCA), + CODE_ER_SPEED = ((uint8_t) 0xCB), + + CODE_ADC_BATTERY_VOLTAGE_CALIB = ((uint8_t) 0xCC), + CODE_ADC_BATTERY_VOLTAGE_MIN = ((uint8_t) 0xCD), + CODE_ADC_BATTERY_VOLTAGE_MAX = ((uint8_t) 0xCE), + + CODE_SUM_TORQUE = ((uint8_t) 0xD0), + CODE_SETPOINT = ((uint8_t) 0xD1), + + CODE_SETPOINT_STATE_HIGH_BYTE = ((uint8_t) 0xD2), + CODE_SETPOINT_STATE = ((uint8_t) 0xD3), + + CODE_CURRENT_RAMP_HIGH_BYTE = ((uint8_t) 0xD4), + CODE_CURRENT_RAMP = ((uint8_t) 0xD5), + + + CODE_PAS_COUNTER_HIGH_BYTE = ((uint8_t) 0xD6), + CODE_PAS_COUNTER = ((uint8_t) 0xD7), + CODE_PAS_HIGH_COUNTER_HIGH_BYTE = ((uint8_t) 0xD8), + CODE_PAS_HIGH_COUNTER = ((uint8_t) 0xD9), + + CODE_THROTTLE_HIGH_BYTE = ((uint8_t) 0xDA), + CODE_THROTTLE = ((uint8_t) 0xDB), + CODE_CURRENT_TARGET_HIGH_BYTE = ((uint8_t) 0xDC), + CODE_CURRENT_TARGET = ((uint8_t) 0xDD), + + CODE_SUM_THROTTLE = ((uint8_t) 0xDE), + + CODE_MOTOR_SPECIFIC_ANGLE = ((uint8_t) 0x80), + CODE_ERPS_FACTOR = ((uint8_t) 0x81), + CODE_CURRENT_CAL_A = ((uint8_t) 0x82), + + CODE_CURRENT_CAL_B_HIGH_BYTE = ((uint8_t) 0x88), + CODE_CURRENT_CAL_B = ((uint8_t) 0x89), + + CODE_LOCKSTATUS = ((uint8_t) 0x83), + + CODE_EEPROM_MAGIC_BYTE = ((uint8_t) 0x84), + CODE_MAX_SPEED_DEFAULT = ((uint8_t) 0x85), + + CODE_THROTTLE_MIN_RANGE = ((uint8_t) 0x86), + CODE_THROTTLE_MAX_RANGE = ((uint8_t) 0x87), + + + CODE_PAS_TRESHOLD = ((uint8_t) 0x8A), + CODE_PID_GAIN_P = ((uint8_t) 0x8B), + CODE_PID_GAIN_I = ((uint8_t) 0x8C), + CODE_RAMP_END = ((uint8_t) 0x8D), + CODE_RAMP_START = ((uint8_t) 0x8E), + CODE_TQ_CALIB = ((uint8_t) 0x8F), + + CODE_ACTUAL_MAX_SPEED = ((uint8_t) 0x90), + CODE_MAX_SPEED_WITHOUT_PAS = ((uint8_t) 0x91), + CODE_MAX_SPEED_WITH_THROTTLE_OVERRIDE = ((uint8_t) 0x92), + + CODE_MAX_BAT_CURRENT_HIGH_BYTE = ((uint8_t) 0x94), + CODE_MAX_BAT_CURRENT = ((uint8_t) 0x95), + CODE_MAX_REGEN_CURRENT = ((uint8_t) 0x96), + + CODE_ACA_FLAGS_HIGH_BYTE = ((uint8_t) 0x97), + CODE_ACA_FLAGS = ((uint8_t) 0x98), + + CODE_CORRECTION_AT_ANGLE = ((uint8_t) 0x99), + + CODE_VAR_DEBUG_A = ((uint8_t) 0x9A), + CODE_VAR_DEBUG_B = ((uint8_t) 0x9B), + CODE_VAR_DEBUG_C = ((uint8_t) 0x9C), + + CODE_ACA_EXPERIMENTAL_FLAGS_HIGH_BYTE = ((uint8_t) 0x9D), + CODE_ACA_EXPERIMENTAL_FLAGS = ((uint8_t) 0x9E), + + CODE_MOTOR_CONSTANT = ((uint8_t) 0x9F), + + CODE_60_DEG_PWM_CYCLES = ((uint8_t) 0x08), + CODE_HALL_ORDER_BASE = ((uint8_t) 0x10), + CODE_CURRENT_AT_HALL_POSITION_BASE = ((uint8_t) 0x00) +} BO_VALUE_CODES; + +typedef enum { + EEPROM_ADDRESS = ((uint8_t) 0xFE), + STATIC_DATA_ADDRESS = ((uint8_t) 0xFB), + CONFIG_ADDRESS = ((uint8_t) 0xFC), + DYNAMIC_DATA_ADDRESS = ((uint8_t) 0xFD), + ERROR_ADDRESS = ((uint8_t) 0x66) +} BO_ADDRESS_CODES; + +void display_update(); +void display_init(); + +#endif /* BODISPLAY_H */ \ No newline at end of file diff --git a/Makefile_linux b/Makefile_linux index 3c60262d..8eaa2f01 100644 --- a/Makefile_linux +++ b/Makefile_linux @@ -7,8 +7,7 @@ .PHONY: all clean #Compiler -#CC = sdcc -CC = /home/cas/software/stm8-binutils/bin/sdcc +CC = sdcc OBJCOPY = stm8-objcopy SIZE = stm8-size @@ -31,14 +30,20 @@ ELF_SECTIONS_TO_REMOVE = -R DATA -R INITIALIZED -R SSEG -R .debug_line -R .debug EXTRASRCS = \ $(SDIR)/stm8s_itc.c \ $(SDIR)/stm8s_clk.c \ + $(SDIR)/stm8s_iwdg.c \ $(SDIR)/stm8s_gpio.c \ $(SDIR)/stm8s_exti.c \ $(SDIR)/stm8s_uart2.c \ $(SDIR)/stm8s_tim1.c \ $(SDIR)/stm8s_tim2.c \ $(SDIR)/stm8s_adc1.c \ + $(SDIR)/stm8s_flash.c \ + BOdisplay.c \ + ACAcontrollerState.c \ + ACAeeprom.c \ + ACAsetPoint.c \ + ACAcommons.c \ gpio.c \ - utils.c \ cruise_control.c \ uart.c \ adc.c \ @@ -48,16 +53,18 @@ EXTRASRCS = \ motor.c \ PAS.c \ SPEED.c \ - update_setpoint.c \ + display.c \ + display_kingmeter.c -HEADERS = adc.h brake.h cruise_control.h gpio.h interrupts.h main.h motor.h pwm.h timers.h uart.h utils.h PAS.h SPEED.h update_setpoint.h +HEADERS = BOdisplay.h ACAcommons.h ACAsetPoint.h ACAcontrollerState.h ACAeeprom.h adc.h brake.h cruise_control.h gpio.h interrupts.h main.h motor.h pwm.h timers.h uart.h PAS.h SPEED.h # The list of .rel files can be derived from the list of their source files RELS = $(EXTRASRCS:.c=.rel) INCLUDES = -I$(IDIR) -I. CFLAGS = -m$(PLATFORM) --std-c99 --nolospre -ELF_FLAGS = --out-fmt-elf --debug +#ELF_FLAGS = --out-fmt-elf --debug +ELF_FLAGS = --out-fmt-elf LIBS = # This just provides the conventional target name "all"; it is optional @@ -67,8 +74,9 @@ all: $(PNAME) # How to build the overall program $(PNAME): $(MAINSRC) $(RELS) $(CC) $(INCLUDES) $(CFLAGS) $(ELF_FLAGS) $(LIBS) $(MAINSRC) $(RELS) - $(SIZE) $(PNAME).elf + $(SIZE) $(PNAME).elf -A $(OBJCOPY) -O binary $(ELF_SECTIONS_TO_REMOVE) $(PNAME).elf $(PNAME).bin + $(OBJCOPY) -O ihex $(ELF_SECTIONS_TO_REMOVE) $(PNAME).elf $(PNAME).hex # How to build any .rel file from its corresponding .c file # GNU would have you use a pattern rule for this, but that's GNU-specific @@ -83,7 +91,7 @@ hex: $(OBJCOPY) -O ihex $(ELF_SECTIONS_TO_REMOVE) $(PNAME).elf $(PNAME).ihx flash: - stm8flash -cstlinkv2 -pstm8s105?6 -w$(PNAME).ihx + stm8flash -cstlinkv2 -pstm8s105?6 -w$(PNAME).bin clean: @echo "Cleaning files..." @@ -110,5 +118,7 @@ clean: @rm -rf *.elf @rm -rf main.bin @rm -rf *.ihx + @rm -rf *.hex + @echo "Done." diff --git a/Makefile_windows b/Makefile_windows index e5f725e4..b4979787 100644 --- a/Makefile_windows +++ b/Makefile_windows @@ -1,105 +1,110 @@ -#Makefile for STM8 Examples with SDCC compiler -#Author: Saeid Yazdani -#Website: WWW.EMBEDONIX.COM -#Copyright 2016 -#LICENSE: GNU-LGPL - -.PHONY: all clean - -#Compiler -CC = sdcc -OBJCOPY = stm8-objcopy -SIZE = stm8-size - -#Platform -PLATFORM = stm8 - -#Product name -PNAME = main - -#Directory for helpers -IDIR = StdPeriphLib/inc -SDIR = StdPeriphLib/src - -# In case you ever want a different name for the main source file -MAINSRC = $(PNAME).c - -ELF_SECTIONS_TO_REMOVE = -R DATA -R INITIALIZED -R SSEG -R .debug_line -R .debug_loc -R .debug_abbrev -R .debug_info -R .debug_pubnames -R .debug_frame - -# These are the sources that must be compiled to .rel files: -EXTRASRCS = \ - $(SDIR)/stm8s_itc.c \ - $(SDIR)/stm8s_clk.c \ - $(SDIR)/stm8s_gpio.c \ - $(SDIR)/stm8s_exti.c \ - $(SDIR)/stm8s_uart2.c \ - $(SDIR)/stm8s_tim1.c \ - $(SDIR)/stm8s_tim2.c \ - $(SDIR)/stm8s_adc1.c \ - gpio.c \ - utils.c \ - cruise_control.c \ - uart.c \ - adc.c \ - brake.c \ - timers.c \ - pwm.c \ - motor.c \ - PAS.c \ - SPEED.c \ - update_setpoint.c \ - -HEADERS = adc.h brake.h cruise_control.h gpio.h interrupts.h main.h motor.h pwm.h timers.h uart.h utils.h PAS.h SPEED.h update_setpoint.h - -# The list of .rel files can be derived from the list of their source files -RELS = $(EXTRASRCS:.c=.rel) - -INCLUDES = -I$(IDIR) -I. -CFLAGS = -m$(PLATFORM) --std-c99 --nolospre -ELF_FLAGS = --out-fmt-ihx --debug -LIBS = -# This just provides the conventional target name "all"; it is optional -# Note: I assume you set PNAME via some means not exhibited in your original file -all: $(PNAME) - -# How to build the overall program - -$(PNAME): $(MAINSRC) $(RELS) - $(CC) $(INCLUDES) $(CFLAGS) $(ELF_FLAGS) $(LIBS) $(MAINSRC) $(RELS) -# $(SIZE) $(PNAME).elf -# $(OBJCOPY) -O binary $(ELF_SECTIONS_TO_REMOVE) -# $(PNAME).elf $(PNAME).bin - -# How to build any .rel file from its corresponding .c file -# GNU would have you use a pattern rule for this, but that's GNU-specific -%.rel: %.c $(HEADERS) - $(CC) -c $(INCLUDES) $(CFLAGS) $(ELF_FLAGS) $(LIBS) -o$< $< - -# Suffixes appearing in suffix rules we care about. -# Necessary because .rel is not one of the standard suffixes. -.SUFFIXES: .c .rel - -# hex: -# $(OBJCOPY) -O ihex $(ELF_SECTIONS_TO_REMOVE) $(PNAME).elf -# $(PNAME).ihx - -# flash: -# stm8flash -cstlinkv2 -pstm8s105?6 -w$(PNAME).ihx - -ENTF = cmd /C del - -clean: - @echo "Cleaning files..." - @clean.bat - @$(ENTF) *.asm - @$(ENTF) *.rel - @$(ENTF) *.lk - @$(ENTF) *.lst - @$(ENTF) *.rst - @$(ENTF) *.sym - @$(ENTF) *.cdb - @$(ENTF) *.map - @$(ENTF) *.elf - @$(ENTF) *.adb - @echo "Done." - +#Makefile for STM8 Examples with SDCC compiler +#Author: Saeid Yazdani +#Website: WWW.EMBEDONIX.COM +#Copyright 2016 +#LICENSE: GNU-LGPL + +.PHONY: all clean + +#Compiler +CC = sdcc +OBJCOPY = stm8-objcopy +SIZE = stm8-size + +#Platform +PLATFORM = stm8 + +#Product name +PNAME = main + +#Directory for helpers +IDIR = StdPeriphLib/inc +SDIR = StdPeriphLib/src + +# In case you ever want a different name for the main source file +MAINSRC = $(PNAME).c + +ELF_SECTIONS_TO_REMOVE = -R DATA -R INITIALIZED -R SSEG -R .debug_line -R .debug_loc -R .debug_abbrev -R .debug_info -R .debug_pubnames -R .debug_frame + +# These are the sources that must be compiled to .rel files: +EXTRASRCS = \ + $(SDIR)/stm8s_itc.c \ + $(SDIR)/stm8s_clk.c \ + $(SDIR)/stm8s_iwdg.c \ + $(SDIR)/stm8s_gpio.c \ + $(SDIR)/stm8s_exti.c \ + $(SDIR)/stm8s_uart2.c \ + $(SDIR)/stm8s_tim1.c \ + $(SDIR)/stm8s_tim2.c \ + $(SDIR)/stm8s_adc1.c \ + $(SDIR)/stm8s_flash.c \ + BOdisplay.c \ + ACAcontrollerState.c \ + ACAeeprom.c \ + ACAsetPoint.c \ + ACAcommons.c \ + gpio.c \ + cruise_control.c \ + uart.c \ + adc.c \ + brake.c \ + timers.c \ + pwm.c \ + motor.c \ + PAS.c \ + SPEED.c \ + display.c \ + display_kingmeter.c + +HEADERS = BOdisplay.h ACAcommons.h ACAsetPoint.h ACAcontrollerState.h ACAeeprom.h adc.h brake.h cruise_control.h gpio.h interrupts.h main.h motor.h pwm.h timers.h uart.h PAS.h SPEED.h + +# The list of .rel files can be derived from the list of their source files +RELS = $(EXTRASRCS:.c=.rel) + +INCLUDES = -I$(IDIR) -I. +CFLAGS = -m$(PLATFORM) --std-c99 --nolospre +ELF_FLAGS = --out-fmt-ihx --debug +LIBS = +# This just provides the conventional target name "all"; it is optional +# Note: I assume you set PNAME via some means not exhibited in your original file +all: $(PNAME) + +# How to build the overall program + +$(PNAME): $(MAINSRC) $(RELS) + $(CC) $(INCLUDES) $(CFLAGS) $(ELF_FLAGS) $(LIBS) $(MAINSRC) $(RELS) +# $(SIZE) $(PNAME).elf +# $(OBJCOPY) -O binary $(ELF_SECTIONS_TO_REMOVE) +# $(PNAME).elf $(PNAME).bin + +# How to build any .rel file from its corresponding .c file +# GNU would have you use a pattern rule for this, but that's GNU-specific +%.rel: %.c $(HEADERS) + $(CC) -c $(INCLUDES) $(CFLAGS) $(ELF_FLAGS) $(LIBS) -o$< $< + +# Suffixes appearing in suffix rules we care about. +# Necessary because .rel is not one of the standard suffixes. +.SUFFIXES: .c .rel + +# hex: +# $(OBJCOPY) -O ihex $(ELF_SECTIONS_TO_REMOVE) $(PNAME).elf +# $(PNAME).ihx + +# flash: +# stm8flash -cstlinkv2 -pstm8s105?6 -w$(PNAME).ihx + +ENTF = cmd /C del + +clean: + echo "Cleaning files..." + $(ENTF) *.asm + $(ENTF) *.rel + $(ENTF) *.lk + $(ENTF) *.lst + $(ENTF) *.rst + $(ENTF) *.sym + $(ENTF) *.cdb + $(ENTF) *.map + $(ENTF) *.adb + echo "Done." + diff --git a/OSEC Parameter Configurator.jar b/OSEC Parameter Configurator.jar index 0dcd3c1d..133933da 100644 Binary files a/OSEC Parameter Configurator.jar and b/OSEC Parameter Configurator.jar differ diff --git a/OSEC State Translator.jar b/OSEC State Translator.jar new file mode 100644 index 00000000..294759d8 Binary files /dev/null and b/OSEC State Translator.jar differ diff --git a/PAS.c b/PAS.c index 8b52a9ba..b6b0b741 100644 --- a/PAS.c +++ b/PAS.c @@ -16,11 +16,23 @@ #include "cruise_control.h" #include "motor.h" #include "pwm.h" +#include "ACAcontrollerState.h" // PAS signal + void EXTI_PORTD_IRQHandler(void) __interrupt(EXTI_PORTD_IRQHANDLER) -{ - ui8_PAS_Flag = 1; //just setting flag in interrupt handler +{ //find the pin that has caused the interrupt + if (!GPIO_ReadInputPin(PAS__PORT, PAS__PIN)) //PAS handling + { + ui8_PAS_Flag = 1; //just setting flag in interrupt handler + } // do not move this out of the else branch, it will reset the controller if it has no over current detection circuit + else if (!GPIO_ReadInputPin(CURRENT_MOTOR_TOTAL_OVER__PORT, CURRENT_MOTOR_TOTAL_OVER__PIN)) //over current handling + { + TIM1_CtrlPWMOutputs(DISABLE); //set phases floating + while (1) + { + } // infinitve loop, user has to + } } diff --git a/README.md b/README.md index eafb8b4b..caf45281 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,53 @@ -Please read the project documentation here: https://opensourceebikefirmware.bitbucket.io/ - -This branch adds support for bottom bracket torque sensors to make motor power dependent on human power and functions for legal use in germany. -Parameters for legality are defined in config.h. -You have to edit the path to your STVP installation,in the .bat files, if not installed directly in C:\, to use the Java-tool. - -HOW TO BUILD - -Windows: - -1. Just double click on "Start_Compiling.bat" -2. or hit "Run" Button on Eclipse -3. or hit "Write Configuartion" Button on the OSEC Parameter Configurator - -Linux: -1. clean the previous built files: $ make -f Makefile_linux clean -2. build the firmware as main.bin and main.elf: $ make -f Makefile_linux +# Important + +Caution! this is not a professional project. Anything you do with this firmware is at your own risk. In any case, use a quick fuse in the power supply to minimize the risk of destroying the controller, battery or motor! + +Please read and reread the [wiki](https://github.com/stancecoke/BMSBattery_S_controllers_firmware/wiki). + +## Preface + +Many thanks to the team of the Forumscontroller, Main functions are from [there](https://github.com/jenkie/Arduino-Pedelec-Controller)! + +This is the maintained fork of [casainho's project(not maintained)](https://github.com/OpenSource-EBike-firmware/BMSBattery_S_controllers_firmware) + +## About the project + +This firmware replaces the closed source one on Kunteng sine wave controllers. It enables you to change basically anything about the way the controller reacts to inputs (throttle, PAS, torque sensor, brake, displays) and handles different modes/states concerning power output. +[A graphical tool](https://github.com/stancecoke/BMSBattery_S_controllers_firmware/wiki/04-The-Java-Tool) is available, which allows the use of the firmware even for less experienced Windows users without programming knowledge. With the [BluOsec android app](https://github.com/stancecoke/BMSBattery_S_controllers_firmware/wiki/05-The-BluOSEC-Android-App) you can control the assist level and set many parameters at runtime. [Download it by clicking this link](https://github.com/Xnyle/bluosec-apk/raw/master/BluOsec-release.apk). + +### Help out yo + + If you want something added / changed that isn't already implemented / beta / buggy, you have to get involved yourself. This is a hobbyists project and there is no one else to blame than yourself if something isn't working as desired ;-) + +### What works + +* Sine wave control with simplified FOC +* Overvoltage (regeneration) and undervoltage protection +* Motor stop while braking +* Driving modes: + * PAS sensor with torque simulation and reverse step detection + * Throttle (also as override) + * Torque sensor + * Recuperation via analog "thumb brake" signal or digitally by brake switch + * Start-up support in torque sensor mode + * Pushing aid + * Block commutation during start-up (only if you have o motor without "weird" angle definitions. +* Displays: + * Kunteng LCD3 / LCD5 (C/P parameters mapped differently) + * BluOsec App as Display / as Trip monitor / for on the fly configuration + * Kingmeter J-LCD and Forerider App (not tested for a long time now) + +### What doesn't + +* You debug it thoroughly +* You tell us / patch it yourself ;-) + +## For further reading + +The documentation on the project, aka wiki, can be found [here](https://github.com/stancecoke/BMSBattery_S_controllers_firmware/wiki) + +The corresponding thread in the german Pedelecforum [can be found here](https://www.pedelecforum.de/forum/index.php?threads/custom-rom-f%C3%BCr-kunteng-s06s-kt36-controller.50061/) + +## Want to help + +There is always things to improve, don't hesitate to get involved if you have improvements in mind. diff --git a/SPEED.c b/SPEED.c index e7a862ad..4ea73b43 100644 --- a/SPEED.c +++ b/SPEED.c @@ -10,12 +10,12 @@ #include "stm8s.h" #include "stm8s_it.h" #include "gpio.h" -#include "main.h" #include "interrupts.h" #include "SPEED.h" #include "cruise_control.h" #include "motor.h" #include "pwm.h" +#include "ACAcontrollerState.h" // SPEED signal diff --git a/Start_Compiling.bat b/Start_Compiling.bat index 7b11de08..043906c4 100644 --- a/Start_Compiling.bat +++ b/Start_Compiling.bat @@ -1,10 +1,14 @@ -PATH = %PATH%C:\STMicroelectronics\st_toolset\stvp - -del main.hex -make -f Makefile_windows clean -make -f Makefile_windows -ren main.ihx main.hex - -STVP_CmdLine -BoardName=ST-LINK -ProgMode=SWIM -Port=USB -Device=STM8S105x6 -FileProg=main.hex -verbose - -exit +PATH = %PATH%;C:\Program Files\STMicroelectronics\st_toolset\stvp;C:\Program Files (x86)\STMicroelectronics\st_toolset\stvp;C:\SDCC\usr\local\bin +REM ;%~dp0tools\cygwin\bin +cd %~dp0 +del main.hex +sdcc --version +make -f Makefile_windows clean +make -f Makefile_windows +ren main.ihx main.hex +make -f Makefile_windows clean + +STVP_CmdLine -BoardName=ST-LINK -ProgMode=SWIM -Port=USB -Device=STM8S105x6 -FileProg=main.hex -verbose -no_loop + +pause +exit diff --git a/Tutorial for seting up the toolchain in windows environment.pdf b/Tutorial for setting up the toolchain in windows environment.pdf similarity index 74% rename from Tutorial for seting up the toolchain in windows environment.pdf rename to Tutorial for setting up the toolchain in windows environment.pdf index 0feb0dfe..df359380 100644 Binary files a/Tutorial for seting up the toolchain in windows environment.pdf and b/Tutorial for setting up the toolchain in windows environment.pdf differ diff --git a/WriteOptionBytes.bat b/WriteOptionBytes.bat index 2a1b236d..388e0734 100644 --- a/WriteOptionBytes.bat +++ b/WriteOptionBytes.bat @@ -1,5 +1,5 @@ -PATH = %PATH%C:\STMicroelectronics\st_toolset\stvp +PATH = %PATH%;C:\Program Files\STMicroelectronics\st_toolset\stvp;C:\Program Files (x86)\STMicroelectronics\st_toolset\stvp -STVP_CmdLine -BoardName=ST-LINK -ProgMode=SWIM -Port=USB -Device=STM8S105x6 -FileOption=optionbytes.hex -verbose +STVP_CmdLine -BoardName=ST-LINK -ProgMode=SWIM -Port=USB -Device=STM8S105x6 -FileOption=optionbytes.hex -verbose -no_loop exit diff --git a/adc.c b/adc.c index ca76dfda..dfba86a8 100755 --- a/adc.c +++ b/adc.c @@ -1,104 +1,165 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#include -#include "stm8s.h" -#include "gpio.h" -#include "stm8s_adc1.h" - -uint8_t adc_throttle_busy_flag = 0; -uint8_t ui8_BatteryVoltage = 0; -uint8_t ui8_BatteryCurrent = 0; - -void adc_init (void) -{ - //init GPIO for the used ADC pins - GPIO_Init(GPIOB, - (THROTTLE__PIN || CURRENT_PHASE_B__PIN || CURRENT_TOTAL__PIN), - GPIO_MODE_IN_FL_NO_IT); - GPIO_Init(GPIOE, - (BATTERY_VOLTAGE__PIN), - GPIO_MODE_IN_FL_NO_IT); - - //de-Init ADC peripheral - ADC1_DeInit(); - - //init ADC1 peripheral - ADC1_Init(ADC1_CONVERSIONMODE_CONTINUOUS, - ADC1_CHANNEL_5, - ADC1_PRESSEL_FCPU_D2, - ADC1_EXTTRIG_TIM, - DISABLE, - ADC1_ALIGN_LEFT, - (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL9), - DISABLE); -} - -uint8_t adc_read_throttle (void) -{ - uint8_t ui8_temp; - - adc_throttle_busy_flag = 1; -//Read in throttle value (just upper 8bits for performance issues) - ADC1_Init(ADC1_CONVERSIONMODE_SINGLE, - ADC1_CHANNEL_4, - ADC1_PRESSEL_FCPU_D2, - ADC1_EXTTRIG_TIM, - DISABLE, - ADC1_ALIGN_LEFT, - (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL9), - DISABLE); - - ADC1->CR1 |= ADC1_CR1_ADON; - while (!(ADC1->CSR & ADC1_FLAG_EOC)) ; - ui8_temp = ADC1->DRH; - -//Read in battery current value (just upper 8bits for performance issues) - ADC1_Init(ADC1_CONVERSIONMODE_SINGLE, - ADC1_CHANNEL_6, - ADC1_PRESSEL_FCPU_D2, - ADC1_EXTTRIG_TIM, - DISABLE, - ADC1_ALIGN_LEFT, - (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL9), - DISABLE); - - ADC1->CR1 |= ADC1_CR1_ADON; - while (!(ADC1->CSR & ADC1_FLAG_EOC)) ; - ui8_BatteryCurrent = ADC1->DRH; - - //Read in battery voltage value (just upper 8bits for performance issues) - ADC1_Init(ADC1_CONVERSIONMODE_SINGLE, - ADC1_CHANNEL_9, - ADC1_PRESSEL_FCPU_D2, - ADC1_EXTTRIG_TIM, - DISABLE, - ADC1_ALIGN_LEFT, - (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL9), - DISABLE); - - ADC1->CR1 |= ADC1_CR1_ADON; - while (!(ADC1->CSR & ADC1_FLAG_EOC)) ; - ui8_BatteryVoltage = ADC1->DRH; - -// Restart continous reading of phaseB current - ADC1_Init(ADC1_CONVERSIONMODE_CONTINUOUS, - ADC1_CHANNEL_5, - ADC1_PRESSEL_FCPU_D2, - ADC1_EXTTRIG_TIM, - DISABLE, - ADC1_ALIGN_RIGHT, - (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL9), - DISABLE); - - ADC1->CR1 |= ADC1_CR1_ADON; - - adc_throttle_busy_flag = 0; - - return ui8_temp; -} +/* + * EGG OpenSource EBike firmware + * + * Copyright (C) Casainho, 2015, 2106, 2017. + * + * Released under the GPL License, Version 3 + */ + +#include +#include +#include "stm8s.h" +#include "gpio.h" +#include "stm8s_adc1.h" +#include "adc.h" +//#include "update_setpoint.h" // FIXME, not needed any more? +#include "timers.h" +#include "ACAcontrollerState.h" + +void adc_init(void) { + uint8_t ui8_i; + + //init GPIO for the used ADC pins + GPIO_Init(GPIOB, + (THROTTLE__PIN || CURRENT_PHASE_B__PIN || CURRENT_MOTOR_TOTAL__PIN || REGEN_THROTTLE__PIN), + GPIO_MODE_IN_FL_NO_IT); + + GPIO_Init(GPIOE, + (CURRENT_MOTOR_TOTAL_FILTERED__PIN), + GPIO_MODE_IN_FL_NO_IT); + + //de-Init ADC peripheral + ADC1_DeInit(); + + //init ADC1 peripheral + ADC1_Init(ADC1_CONVERSIONMODE_SINGLE, + ADC1_CHANNEL_9, + ADC1_PRESSEL_FCPU_D2, + ADC1_EXTTRIG_TIM, + DISABLE, + ADC1_ALIGN_LEFT, + (ADC1_SCHMITTTRIG_CHANNEL4 || ADC1_SCHMITTTRIG_CHANNEL5 || ADC1_SCHMITTTRIG_CHANNEL6 || ADC1_SCHMITTTRIG_CHANNEL7 || ADC1_SCHMITTTRIG_CHANNEL8), + DISABLE); + + ADC1_ScanModeCmd(ENABLE); + ADC1_Cmd(ENABLE); + + //******************************************************************************** + // next code is for "calibrating" the offset value of ADC motor total current + // read and discard few samples of ADC, to make sure the next samples are ok + for (ui8_i = 0; ui8_i < 8; ui8_i++) { + //ui16_counter = TIM2_GetCounter () + 1000; + // while (TIM2_GetCounter () < ui16_counter) ; // delay + delay_halfms(200); + adc_trigger(); + delay_halfms(200); + ui16_current_cal_b = ui16_adc_read_motor_total_current(); + ui16_x4_cal_b = ui16_adc_read_x4_value(); + ui16_throttle_cal_b = ui8_adc_read_throttle(); + } + // is there a deeper meaning behind assigning the value 8 times above? + // compiler optimization? + // i don't dare to cleanup this code until I'm sure its just bad style :) + + ui16_current_cal_b = 0; + ui16_x4_cal_b = 0; + ui16_throttle_cal_b = 0; + + // read and average a few values of ADC + for (ui8_i = 0; ui8_i < 16; ui8_i++) { + delay_halfms(30); + adc_trigger(); + delay_halfms(30); + ui16_current_cal_b += ui16_adc_read_motor_total_current(); + ui16_x4_cal_b += ui16_adc_read_x4_value(); + ui16_throttle_cal_b += ui8_adc_read_throttle(); + } + + ui16_current_cal_b >>= 4; + ui16_current_cal_b -= 1; + ui16_x4_cal_b >>= 4; + ui16_x4_cal_b -= 1; + ui16_throttle_cal_b >>= 4; + ui16_throttle_cal_b -= 1; + + +#ifdef DIAGNOSTICS + printf("ui16_current_cal_b = %d\r\n", ui16_current_cal_b); +#endif + //ui8_motor_total_current_offset = ui16_motor_total_current_offset_10b >> 2; +} + +inline void adc_trigger(void) //inline ?! +{ + // start ADC all channels, scan conversion (buffered) + ADC1->CSR &= 0x09; // clear EOC flag first (selected also channel 9) + ADC1_StartConversion(); +} + +uint8_t ui8_adc_read_phase_B_current(void) { + // /* Read LSB first */ + // templ = *(uint8_t*)(uint16_t)((uint16_t)ADC1_BaseAddress + (uint8_t)(Buffer << 1) + 1); + // /* Then read MSB */ + // temph = *(uint8_t*)(uint16_t)((uint16_t)ADC1_BaseAddress + (uint8_t)(Buffer << 1)); + //#define ADC1_BaseAddress 0x53E0 + //phase_B_current --> ADC_AIN5 + // 0x53E0 + 2*5 = 0x53EA + return *(uint8_t*) (0x53EA); +} + +uint16_t ui16_adc_read_phase_B_current(void) { + uint16_t temph; + uint8_t templ; + + templ = *(uint8_t*) (0x53EB); + temph = *(uint8_t*) (0x53EA); + + return ((uint16_t) temph) << 2 | ((uint16_t) templ); +} + +uint8_t ui8_adc_read_throttle(void) { + // 0x53E0 + 2*4 = 0x53E8 + // return *(uint8_t*)(0x53E8); + return *(uint8_t*) (0x53E8); +} + +uint16_t ui16_adc_read_x4_value(void) { + uint16_t temph; + uint8_t templ; + + templ = *(uint8_t*) (0x53EF); + temph = *(uint8_t*) (0x53EE); + + return ((uint16_t) temph) << 2 | ((uint16_t) templ); + +} + +uint8_t ui8_adc_read_motor_total_current(void) { + // 0x53E0 + 2*8 = 0x53F0 + return *(uint8_t*) (0x53F0); +} + +uint16_t ui16_adc_read_motor_total_current(void) { + uint16_t temph; + uint8_t templ; + + templ = *(uint8_t*) (0x53F1); + temph = *(uint8_t*) (0x53F0); + + return ((uint16_t) temph) << 2 | ((uint16_t) templ); +} + +uint8_t ui8_adc_read_battery_voltage(void) { + // 0x53E0 + 2*9 = 0x53F2 + return *(uint8_t*) (0x53F2); +} + +uint16_t ui16_adc_read_battery_voltage(void) { + uint16_t temph; + uint8_t templ; + + templ = *(uint8_t*) (0x53F3); + temph = *(uint8_t*) (0x53F2); + + return ((uint16_t) temph) << 2 | ((uint16_t) templ); +} diff --git a/adc.h b/adc.h index 5a51fad0..c5b3c51e 100755 --- a/adc.h +++ b/adc.h @@ -1,21 +1,37 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#ifndef _ADC_H -#define _ADC_H - -#include "main.h" - -extern uint8_t adc_throttle_busy_flag; -extern uint8_t ui8_BatteryVoltage; -extern uint8_t ui8_BatteryCurrent; - -void adc_init (void); -uint8_t adc_read_throttle (void); - -#endif /* _ADC_H */ +/* + * EGG OpenSource EBike firmware + * + * Copyright (C) Casainho, 2015, 2106, 2017. + * + * Released under the GPL License, Version 3 + */ + +#ifndef _ADC_H +#define _ADC_H + +#include "main.h" + +#define ADC1_CHANNEL_PHASE_CURRENT_B ADC1_CHANNEL_5 +#define ADC1_CHANNEL_MOTOR_TOTAL_CURRENT ADC1_CHANNEL_6 +#define ADC1_CHANNEL_MOTOR_TOTAL_CURRENT_FILTERED ADC1_CHANNEL_8 +#define ADC1_CHANNEL_BATTERY_VOLTAGE ADC1_CHANNEL_9 +#define ADC1_CHANNEL_THROTTLE ADC1_CHANNEL_4 + +extern uint8_t adc_throttle_busy_flag; +extern uint8_t ui8_BatteryCurrent; +extern uint8_t ui8_adc_throttle_value; +extern uint8_t ui8_motor_total_current_offset; +extern uint16_t ui16_motor_total_current_offset_10b; + +void adc_init (void); +inline void adc_trigger (void); +uint8_t ui8_adc_read_phase_B_current (void); +uint16_t ui16_adc_read_phase_B_current (void); +uint8_t ui8_adc_read_throttle (void); +uint16_t ui16_adc_read_x4_value (void); +uint8_t ui8_adc_read_motor_total_current (void); +uint16_t ui16_adc_read_motor_total_current (void); +uint8_t ui8_adc_read_battery_voltage (void); +uint16_t ui16_adc_read_battery_voltage (void); + +#endif /* _ADC_H */ diff --git a/brake.c b/brake.c index beaf3038..f3454fc8 100755 --- a/brake.c +++ b/brake.c @@ -22,14 +22,14 @@ void EXTI_PORTA_IRQHandler(void) __interrupt(EXTI_PORTA_IRQHANDLER) { if (brake_is_set()) { - brake_coast_enable (); - stop_cruise_control (); + //brake_coast_enable (); + //stop_cruise_control (); } else { - brake_coast_disable (); - pwm_set_duty_cycle (0); - stop_cruise_control (); + //brake_coast_disable (); + //pwm_set_duty_cycle (0); + //stop_cruise_control (); } } diff --git a/clean.bat b/clean.bat deleted file mode 100644 index 8b526cc1..00000000 --- a/clean.bat +++ /dev/null @@ -1,13 +0,0 @@ -cd stdperiphlib\src -del *.asm -del *.rel -del *.lk -del *.lst -del *.rst -del *.sym -del *.cdb -del *.map -del *.elf -del *.bin -cd.. -cd.. \ No newline at end of file diff --git a/config.h b/config.h index e7b01566..58429669 100644 --- a/config.h +++ b/config.h @@ -8,14 +8,48 @@ #ifndef CONFIG_H_ #define CONFIG_H_ -#define limit 25 +#define NUMBER_OF_PAS_MAGS 12 +#define limit 28 #define timeout 3125 -#define wheel_circumference 2000L -#define fummelfaktor 64L +#define wheel_circumference 2230L +#define limit_without_pas 6 #define ADC_THROTTLE_MIN_VALUE 43 #define ADC_THROTTLE_MAX_VALUE 182 -#define BATTERY_VOLTAGE_MIN_VALUE 127 -#define BATTERY_CURRENT_MAX_VALUE 254 -#define THROTTLE +#define BATTERY_VOLTAGE_MIN_VALUE 148 +#define BATTERY_CURRENT_MAX_VALUE 64L +#define PHASE_CURRENT_MAX_VALUE 350L +#define REGEN_CURRENT_MAX_VALUE 50L +#define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT 246 +#define current_cal_a 46 +#define LEVEL_1 26 +#define LEVEL_2 38 +#define LEVEL_3 55 +#define LEVEL_4 80 +#define LEVEL_5 100 +#define MORSE_TIME_1 50 +#define MORSE_TIME_2 50 +#define MORSE_TIME_3 50 +#define RAMP_END 1500 +#define P_FACTOR 0.5 +#define I_FACTOR 0.2 +#define GEAR_RATIO 69L +#define PAS_THRESHOLD 1.9 +#define RAMP_START 64000 +#define limit_with_throttle_override 35 +#define CORRECTION_AT_ANGLE 127 +#define ANGLE_4_0 1 +#define ANGLE_6_60 41 +#define ANGLE_2_120 84 +#define ANGLE_3_180 129 +#define DISPLAY_TYPE_KT_LCD3 +#define ANGLE_1_240 167 +#define ANGLE_5_300 212 +#define TQS_CALIB 0.0 +#define ACA 4764 +#define EEPROM_NOINIT // eeprom will not be cleared +#define EEPROM_INIT_MAGIC_BYTE 93 // makes sure (chance of fail 1/255) eeprom is invalidated after flashing new config +#define ADC_BATTERY_VOLTAGE_K 67 +#define ACA_EXPERIMENTAL 128 +#define BATTERY_VOLTAGE_MAX_VALUE 208 #endif /* CONFIG_H_ */ diff --git a/config.h_ b/config.h_ deleted file mode 100644 index c0fffb8a..00000000 --- a/config.h_ +++ /dev/null @@ -1,19 +0,0 @@ -/* - * config.h - * - * Created on: 25.08.2017 - * Author: stancecoke - */ - -#ifndef CONFIG_H_ -#define CONFIG_H_ - -#define limit 25 // speed limit in km/h -#define wheel_circumference 4000L // wheel circumference for calculation speed, "L" for assigning a "long" variable -#define fummelfaktor 64L //factor to bring setpoint in right range -#define timeout 3125 //time after that motor stops, if you are not pedaling#define ADC_THROTTLE_MIN_VALUE 43 -#define ADC_THROTTLE_MIN_VALUE 43 //minimum value from throttle, has to be fitted to your throttle -#define ADC_THROTTLE_MAX_VALUE 182 //maximum value from throttle, has to be fitted to your throttle - - -#endif /* CONFIG_H_ */ diff --git a/display.c b/display.c new file mode 100644 index 00000000..ba2730a1 --- /dev/null +++ b/display.c @@ -0,0 +1,242 @@ +/* +Generic display init and update functions +Written by jenkie and Thomas Jarosch +Functions for the Nokia graphical screen mainly by m--k +King-Meter library and support written by Michael Fabry + +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, write to the Free Software Foundation, +Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include +#include "stm8s.h" +#include "display.h" +#include "main.h" +#include "config.h" +#include "stm8s_itc.h" +#include "uart.h" +#include "adc.h" +#include "brake.h" +#include "ACAeeprom.h" +#include "interrupts.h" +#include "ACAcontrollerState.h" + +#ifdef DISPLAY_TYPE_KT_LCD3 + +display_view_type display_view; +display_mode_type display_mode; //currently display mode +float current_display; +uint8_t battery_percent_fromcapacity = 11; //hier nur als Konstante um Batterie normal zu senden.... + + +uint8_t ui8_tx_buffer[12]; +uint8_t ui8_j; +uint8_t ui8_crc; +uint16_t ui16_wheel_period_ms = 4500; +uint16_t ui16_battery_bars_calc = 0; +uint8_t ui8_battery_soc = 12; +uint8_t ui16_error; +uint8_t ui8_rx_buffer[13]; +uint8_t ui8_rx_buffer_counter = 0; +uint8_t ui8_byte_received; +uint8_t ui8_moving_indication = 0; +uint8_t ui8_UARTCounter = 0; + +volatile struc_lcd_configuration_variables lcd_configuration_variables; + +void display_init(){ + // noop just here to have a common interface +} + +void send_message() { + + // prepare moving indication info + ui8_moving_indication = 0; + if (brake_is_set()) { + ui8_moving_indication |= (1 << 5); + } + //if (ebike_app_cruise_control_is_set ()) { ui8_moving_indication |= (1 << 3); } + //if (throttle_is_set ()) { ui8_moving_indication |= (1 << 1); } + //if (pas_is_set ()) { ui8_moving_indication |= (1 << 4); } + + + if (((ui16_aca_flags & EXTERNAL_SPEED_SENSOR) == EXTERNAL_SPEED_SENSOR)) { + if (ui16_time_ticks_between_speed_interrupt > 65000) { + ui16_wheel_period_ms = 4500; + } else { + ui16_wheel_period_ms = (uint16_t) ((float) ui16_time_ticks_between_speed_interrupt / ((float) ui16_pwm_cycles_second / 1000.0)); //must be /1000 devided in /125/8 for better resolution + } + }else{ + if (ui32_erps_filtered == 0) { + ui16_wheel_period_ms = 4500; + } else { + ui16_wheel_period_ms = (uint16_t) (1000.0 * (float) ui8_gear_ratio / (float) ui32_erps_filtered); + } + } + + // calc battery pack state of charge (SOC) + ui16_battery_bars_calc = ui8_adc_read_battery_voltage() - ui8_s_battery_voltage_min; + ui16_battery_bars_calc<<=8; + ui16_battery_bars_calc /=(ui8_s_battery_voltage_max-ui8_s_battery_voltage_min); + + if (ui16_battery_bars_calc > 200) { + ui8_battery_soc = 16; + }// 4 bars | full + else if (ui16_battery_bars_calc > 150) { + ui8_battery_soc = 12; + }// 3 bars + else if (ui16_battery_bars_calc > 100) { + ui8_battery_soc = 8; + }// 2 bars + else if (ui16_battery_bars_calc > 50) { + ui8_battery_soc = 4; + }// 1 bar + else { + ui8_battery_soc = 3; + } // empty + + ui8_tx_buffer [0] = 65; + // B1: battery level + ui8_tx_buffer [1] = ui8_battery_soc; + // B2: 24V controller + ui8_tx_buffer [2] = ui8_battery_voltage_nominal; + // B3: speed, wheel rotation period, ms; period(ms)=B3*256+B4; + ui8_tx_buffer [3] = (ui16_wheel_period_ms >> 8) & 0xff; + ui8_tx_buffer [4] = ui16_wheel_period_ms & 0xff; + + //Send confirming signal for activating offroad mode + if (ui8_offroad_state == 4) { //quitting signal for offroad mode enabled. Shows about 80 km/h for three seconds + + ui8_tx_buffer [3] = (100 >> 8) & 0xff; //100ms are about 80 km/h @ 28" 2200mm wheel circumference + ui8_tx_buffer [4] = 100 & 0xff; + } + + // B5: error info display + ui8_tx_buffer [5] = ui16_error; + // B6: CRC: xor B1,B2,B3,B4,B5,B7,B8,B9,B10,B11 + // 0 value so no effect on xor operation for now + ui8_tx_buffer [6] = 0; + // B7: moving mode indication, bit + // throttle: 2 + ui8_tx_buffer [7] = ui8_moving_indication; + // B8: 4x controller current + // Vbat = 30V: + // - B8 = 255, LCD shows 1912 watts + // - B8 = 250, LCD shows 1875 watts + // - B8 = 100, LCD shows 750 watts + // each unit of B8 = 0.25A + + + ui8_tx_buffer [8] = (uint8_t) ((((ui16_BatteryCurrent - ui16_current_cal_b + 1) << 2)*10) / ui8_current_cal_a); + // B9: motor temperature + ui8_tx_buffer [9] = i8_motor_temperature - 15; //according to documentation at endless sphere + // B10 and B11: 0 + ui8_tx_buffer [10] = 0; + ui8_tx_buffer [11] = 0; + + // calculate CRC xor + ui8_crc = 0; + for (ui8_j = 1; ui8_j <= 11; ui8_j++) { + ui8_crc ^= ui8_tx_buffer[ui8_j]; + } + ui8_tx_buffer [6] = ui8_crc; + + // send the package over UART + for (ui8_j = 0; ui8_j <= 11; ui8_j++) { + uart_put_buffered(ui8_tx_buffer [ui8_j]); + } +} + +/********************************************************************************************/ +// Process received package from the LCD +// + +void digestLcdValues(void) { + + ui8_assistlevel_global = lcd_configuration_variables.ui8_assist_level + 80; // always add max regen + ui8_walk_assist = lcd_configuration_variables.ui8_WalkModus_On; + // added by DerBastler Light On/Off + light_stat = (light_stat&~128) | lcd_configuration_variables.ui8_light_On; // only update 7th bit, 1st bit is current status + + if (lcd_configuration_variables.ui8_max_speed != ui8_speedlimit_kph) { + ui8_speedlimit_kph = lcd_configuration_variables.ui8_max_speed; + eeprom_write(OFFSET_MAX_SPEED_DEFAULT, lcd_configuration_variables.ui8_max_speed); + } +} + +// see if we have a received package to be processed + +void display_update() { + + // fill local buffer from uart ringbuffer + uart_fill_rx_packet_buffer(ui8_rx_buffer, 13, &ui8_UARTCounter); + + // Check for reception of complete message + if ((ui8_UARTCounter > 12) || (ui8_rx_buffer[ui8_UARTCounter - 1] == 0x0E)) { + ui8_UARTCounter = 0; + + // validation of the package data + ui8_crc = 0; + for (ui8_j = 0; ui8_j <= 12; ui8_j++) { + + if (ui8_j == 5) continue; // don't xor B5 + ui8_crc ^= ui8_rx_buffer[ui8_j]; + } + + // see if CRC is ok + if (((ui8_crc ^ 10) == ui8_rx_buffer [5]) || // some versions of CRC LCD5 (??) + ((ui8_crc ^ 1) == ui8_rx_buffer [5]) || // CRC LCD3 (tested with KT36/48SVPR, from PSWpower) + ((ui8_crc ^ 2) == ui8_rx_buffer [5]) || // CRC LCD5 + ((ui8_crc ^ 3) == ui8_rx_buffer [5]) || // CRC LCD5 Added display 5 Romanta + ((ui8_crc ^ 4) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 5) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 6) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 7) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 8) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 14) == ui8_rx_buffer [5]) || + ((ui8_crc ^ 9) == ui8_rx_buffer [5])) // CRC LCD3 + { + // added by DerBastler Light On/Off + lcd_configuration_variables.ui8_light_On = ui8_rx_buffer [1] & 128; + + lcd_configuration_variables.ui8_assist_level = ui8_rx_buffer [1] & 7; + + // walk assist, see https://endless-sphere.com/forums/viewtopic.php?f=2&t=73471&p=1324745&hilit=kunteng+protocol+hacked#p1109048 + if(lcd_configuration_variables.ui8_assist_level == 6)lcd_configuration_variables.ui8_WalkModus_On = 1; + else lcd_configuration_variables.ui8_WalkModus_On = 0; + + lcd_configuration_variables.ui8_max_speed = 10 + ((ui8_rx_buffer [2] & 248) >> 3) | (ui8_rx_buffer [4] & 32); + lcd_configuration_variables.ui8_wheel_size = ((ui8_rx_buffer [4] & 192) >> 6) | ((ui8_rx_buffer [2] & 7) << 2); + + lcd_configuration_variables.ui8_p1 = ui8_rx_buffer[3]; + lcd_configuration_variables.ui8_p2 = ui8_rx_buffer[4] & 0x07; + lcd_configuration_variables.ui8_p3 = ui8_rx_buffer[4] & 0x08; + lcd_configuration_variables.ui8_p4 = ui8_rx_buffer[4] & 0x10; + lcd_configuration_variables.ui8_p5 = ui8_rx_buffer[0]; + + lcd_configuration_variables.ui8_c1 = (ui8_rx_buffer[6] & 0x38) >> 3; + lcd_configuration_variables.ui8_c2 = (ui8_rx_buffer[6] & 0x37); + lcd_configuration_variables.ui8_c4 = (ui8_rx_buffer[8] & 0xE0) >> 5; + lcd_configuration_variables.ui8_c5 = (ui8_rx_buffer[7] & 0x0F); + lcd_configuration_variables.ui8_c12 = (ui8_rx_buffer[9] & 0x0F); + lcd_configuration_variables.ui8_c13 = (ui8_rx_buffer[10] & 0x1C) >> 2; + lcd_configuration_variables.ui8_c14 = (ui8_rx_buffer[7] & 0x60) >> 5; + + digestLcdValues(); + send_message(); + } + } +} + +#endif diff --git a/display.h b/display.h new file mode 100644 index 00000000..59a4d731 --- /dev/null +++ b/display.h @@ -0,0 +1,87 @@ +/* +Generic display init and update functions +Written by Thomas Jarosch + +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, write to the Free Software Foundation, +Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +*/ +#ifndef DISPLAY_H +#define DISPLAY_H + +#include "config.h" + +void display_init(); +void display_update(); +//void display_debug(HardwareSerial* localSerial); + +//void display_show_important_info(const char *str, int duration_secs); +//void display_show_important_info(const __FlashStringHelper *str, int duration_secs); + +void display_show_welcome_msg(); +void display_show_welcome_msg_temp(); + +void display_prev_view(); +void display_next_view(); + + + +//definitions for different screen mode +typedef enum {DISPLAY_MODE_TEXT, + DISPLAY_MODE_GRAPHIC, // Note: Same as _TEXT on 16x2 displays + DISPLAY_MODE_MENU, + DISPLAY_MODE_IMPORTANT_INFO + } display_mode_type; + +typedef enum {DISPLAY_VIEW_MAIN=0, + + _DISPLAY_VIEW_END + } display_view_type; + +extern display_view_type display_view; + +extern display_mode_type display_mode; //currently display mode +extern uint8_t display_force_text; //only valid for Nokia displays +extern uint16_t poti_stat; +extern uint16_t throttle_stat; +extern uint8_t battery_percent_fromvoltage; +extern uint8_t battery_percent_fromcapacity; +extern uint32_t wheel_time; +extern float current_display; + + +#ifdef DISPLAY_TYPE_KT_LCD3 +typedef struct _lcd_configuration_variables +{ + uint8_t ui8_light_On; //added by DerBasteler + uint8_t ui8_WalkModus_On; //added by DerBasteler + uint8_t ui8_assist_level; + uint8_t ui8_max_speed; + uint8_t ui8_wheel_size; + uint8_t ui8_p1; + uint8_t ui8_p2; + uint8_t ui8_p3; + uint8_t ui8_p4; + uint8_t ui8_p5; + uint8_t ui8_c1; + uint8_t ui8_c2; + uint8_t ui8_c4; + uint8_t ui8_c5; + uint8_t ui8_c12; + uint8_t ui8_c13; + uint8_t ui8_c14; +} struc_lcd_configuration_variables; + +#endif + +#endif diff --git a/display_kingmeter.c b/display_kingmeter.c new file mode 100644 index 00000000..222ac708 --- /dev/null +++ b/display_kingmeter.c @@ -0,0 +1,494 @@ +/* +Library for King-Meter displays + +Copyright © 2015 Michael Fabry (Michael@Fabry.de) + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . + */ + + +// Includes + +#include +#include "stm8s.h" +#include "config.h" +#include "display_kingmeter.h" +#include "main.h" +#include "stm8s_itc.h" +#include "uart.h" +#include "timers.h" +#include "interrupts.h" +#include "ACAcontrollerState.h" + + +#if defined (DISPLAY_TYPE) && defined (DISPLAY_TYPE_KINGMETER) + +// Definitions +#define RXSTATE_STARTCODE 0 +#define RXSTATE_SENDTXMSG 1 +#define RXSTATE_MSGBODY 2 +#define RXSTATE_DONE 3 + +int pas_tolerance; +int wheel_magnets; +int vcutoff; +int spd_max1; + +uint8_t ui8_UARTCounter = 0; +uint8_t ui8_rx[6]; +uint8_t battery_percent_fromcapacity = 11; //hier nur als Konstante um Batterie normal zu senden.... + +uint16_t k; //for debugging, number of calls of service +uint16_t KM_WHEELSIZE [8] ={ + KM_WHEELSIZE_16, + KM_WHEELSIZE_18, + KM_WHEELSIZE_20, + KM_WHEELSIZE_22, + KM_WHEELSIZE_24, + KM_WHEELSIZE_26, + KM_WHEELSIZE_700, + KM_WHEELSIZE_28 +}; + +uint8_t ui8_UART_flag = 0; + + +// Hashtable used for handshaking in 901U protocol +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) +const uint8_t KM_901U_HANDSHAKE[64] ={ + 201, 107, 13, 229, 241, 198, 108, 230, 186, 67, 39, 92, 217, 140, 177, 36, + 22, 71, 174, 39, 161, 151, 7, 140, 107, 155, 189, 195, 209, 106, 63, 191, + 218, 47, 221, 46, 135, 145, 98, 82, 35, 42, 85, 99, 35, 43, 180, 12, + 3, 126, 94, 103, 198, 10, 182, 249, 253, 86, 105, 196, 217, 183, 195, 115 +}; +#endif + + +// Local function prototypes +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) +static void KM_618U_Service(KINGMETER_t* KM_ctx); +#endif + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) +static void KM_901U_Service(KINGMETER_t* KM_ctx); +#endif + + +KINGMETER_t KM; // Context of the King-Meter object + +/* Public functions (Prototypes declared by display_kingmeter.h) */ + +/**************************************************************************************************** + * KingMeter_Init() - Initializes the display object + * + ****************************************************************************************************/ + +void KingMeter_Init(KINGMETER_t* KM_ctx){ + uint16_t i; + + + // KM_ctx->SerialPort = DisplaySerial; // Store serial port to use + + KM_ctx->RxState = RXSTATE_STARTCODE; + // KM_ctx->LastRx = millis(); + + for (i = 0; i < KM_MAX_RXBUFF; i++) { + KM_ctx->RxBuff[i] = 0x00; + } + + KM_ctx->RxCnt = 0; + + // Settings received from display: + KM_ctx->Settings.PAS_RUN_Direction = KM_PASDIR_FORWARD; + KM_ctx->Settings.PAS_SCN_Tolerance = (uint8_t) pas_tolerance; + KM_ctx->Settings.PAS_N_Ratio = 255; + KM_ctx->Settings.HND_HL_ThrParam = KM_HND_HL_NO; + KM_ctx->Settings.HND_HF_ThrParam = KM_HND_HF_NO; + KM_ctx->Settings.SYS_SSP_SlowStart = 1; + KM_ctx->Settings.SPS_SpdMagnets = (uint8_t) wheel_magnets; + KM_ctx->Settings.VOL_1_UnderVolt_x10 = (uint16_t) (vcutoff * 10); + KM_ctx->Settings.WheelSize_mm = (uint16_t) (wheel_circumference * 1000); + + // Parameters received from display in operation mode: + KM_ctx->Rx.AssistLevel = 1; + KM_ctx->Rx.Headlight = KM_HEADLIGHT_OFF; + KM_ctx->Rx.Battery = KM_BATTERY_NORMAL; + KM_ctx->Rx.PushAssist = KM_PUSHASSIST_OFF; + KM_ctx->Rx.PowerAssist = KM_POWERASSIST_ON; + KM_ctx->Rx.Throttle = KM_THROTTLE_ON; + KM_ctx->Rx.CruiseControl = KM_CRUISE_OFF; + KM_ctx->Rx.OverSpeed = KM_OVERSPEED_NO; + KM_ctx->Rx.SPEEDMAX_Limit_x10 = (uint16_t) (spd_max1 * 10); + KM_ctx->Rx.CUR_Limit_x10 = 150; + + // Parameters to be send to display in operation mode: + KM_ctx->Tx.Battery = KM_BATTERY_NORMAL; + KM_ctx->Tx.Wheeltime_ms = KM_MAX_WHEELTIME; + KM_ctx->Tx.Error = KM_ERROR_NONE; + KM_ctx->Tx.Current_x10 = 0; + + + //DisplaySerial->begin(9600); wird bereits in der main.c gestartet +} + + +/* Local functions */ + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) + +/**************************************************************************************************** + * KM_618U_Service() - Communication protocol of 618U firmware (J-LCD compatible) + * + ***************************************************************************************************/ +static void KM_618U_Service(KINGMETER_t* KM_ctx) { + uint16_t i; + uint16_t j; + + uint8_t TxBuff[KM_MAX_TXBUFF]; + + // Search for Start Code + if (KM_ctx->RxState == RXSTATE_STARTCODE) { + + if (ui8_rx[0] == 0x46 && ui8_rx[5] == 0x0D) //Check, if Message is valid + { + for (j = 0; j < 6; j++) { + KM_ctx->RxBuff[j] = ui8_rx[j]; + + } + KM_ctx->RxState = RXSTATE_SENDTXMSG; + } + + } + + if (KM_ctx->RxState == RXSTATE_SENDTXMSG) { + KM_ctx->RxState = RXSTATE_DONE; + + + // Prepare Tx message + TxBuff[0] = 0X46; // StartCode + + if (KM_ctx->Tx.Battery == KM_BATTERY_LOW) { + TxBuff[1] = 0x00; // If none of Bit[0..2] is set, display blinks + } else { + TxBuff[1] = 0x03; //Original firmware of Lishui sends "3" at this Byte + } + + TxBuff[2] = (uint8_t) ((KM_ctx->Tx.Current_x10 * 3) / 10); // Current unit: 1/3A, gives still error at first loop run? + //TxBuff[2] = 0x00; + TxBuff[3] = (uint8_t) (KM_ctx->Tx.Wheeltime_ms >> 8); // richtige Funktion der Bitmanipulation noch nicht best�tigt + TxBuff[4] = (uint8_t) (KM_ctx->Tx.Wheeltime_ms & 0xFF); // richtige Funktion der Bitmanipulation noch nicht best�tigt + TxBuff[5] = 0xA7; // Reply with WheelSize 26" / Maxspeed 25km/h (no influence on display) + TxBuff[6] = KM_ctx->Tx.Error; + + + // Send prepared message + TxBuff[7] = 0x00; + + uart_put_buffered(TxBuff[0]); // Send StartCode + + for (i = 1; i < 7; i++) { + uart_put_buffered(TxBuff[i]); // Send TxBuff[1..6] + TxBuff[7] ^= TxBuff[i]; // Calculate XOR CheckSum + } + + uart_put_buffered(TxBuff[7]); // Send XOR CheckSum + + } + + + // Message received completely + if (KM_ctx->RxState == RXSTATE_DONE) { + KM_ctx->RxState = RXSTATE_STARTCODE; + + // Decode PAS level - Display sets PAS-level to 0 when overspeed detected! + KM_ctx->Rx.AssistLevel = KM_ctx->RxBuff[1] & 0x07; //Mapping entfernt, da nur der Faktor genutzt werden soll. + + // Decode Headlight status + KM_ctx->Rx.Headlight = (KM_ctx->RxBuff[1] & 0x80) >> 7; // KM_HEADLIGHT_OFF / KM_HEADLIGHT_ON + + // KM_ctx->Rx.Battery; + + // Decode PushAssist status + KM_ctx->Rx.PushAssist = (KM_ctx->RxBuff[1] & 0x10) >> 4; // KM_PUSHASSIST_OFF / KM_PUSHASSIST_ON + + // KM_ctx->Rx.PowerAssist; + // KM_ctx->Rx.Throttle; + // KM_ctx->Rx.CruiseControl; + // KM_ctx->Rx.OverSpeed; + + // Decode Speedlimit + KM_ctx->Rx.SPEEDMAX_Limit_x10 = (((KM_ctx->RxBuff[2] & 0xF8) >> 3) + 10) * 10; + + // Decode Wheelsize by hashtable + KM_ctx->Settings.WheelSize_mm = KM_WHEELSIZE[KM_ctx->RxBuff[2] & 0x07]; + + // KM_ctx->Rx.CUR_Limit_x10; + + } + +} +#endif + + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) + +/**************************************************************************************************** + * KM_901U_Service() - Communication protocol of 901U firmware + * + ***************************************************************************************************/ +static void KM_901U_Service(KINGMETER_t* KM_ctx) { + uint8_t i; + uint16_t CheckSum; + uint8_t TxBuff[KM_MAX_TXBUFF]; + uint8_t TxCnt; + + + // Search for Start Code + if (KM_ctx->RxState == RXSTATE_STARTCODE) { + if (KM_ctx->SerialPort->available()) { + KM_ctx->LastRx = millis(); + + if (KM_ctx->SerialPort->read() == 0x3A) { + KM_ctx->RxBuff[0] = 0x3A; + KM_ctx->RxCnt = 1; + KM_ctx->RxState = RXSTATE_MSGBODY; + } else { + return; // No need to continue + } + } + } + + // Receive Message body + if (KM_ctx->RxState == RXSTATE_MSGBODY) { + while (KM_ctx->SerialPort->available()) { + KM_ctx->RxBuff[KM_ctx->RxCnt] = KM_ctx->SerialPort->read(); + KM_ctx->RxCnt++; + + if (KM_ctx->RxCnt == 5) // Range check of Data size + { + if (KM_ctx->RxBuff[4] > (KM_MAX_RXBUFF - 5 - 4)) { + KM_ctx->RxState = RXSTATE_STARTCODE; // Invalid Data size, cancel reception + break; + } + } + + + if (KM_ctx->RxCnt == (5 + KM_ctx->RxBuff[4] + 4)) // Check for reception of complete message + { + // Verify CheckSum + CheckSum = 0x0000; + for (i = 1; i < (5 + KM_ctx->RxBuff[4]); i++) { + CheckSum = CheckSum + KM_ctx->RxBuff[i]; // Calculate CheckSum + } + + if ((lowByte(CheckSum) == KM_ctx->RxBuff[i]) && (highByte(CheckSum) == KM_ctx->RxBuff[i + 1])) { + KM_ctx->RxState = RXSTATE_DONE; + } else { + KM_ctx->RxState = RXSTATE_STARTCODE; // Invalid CheckSum, ignore message + } + + break; + } + } + } + + + // Message received completely + if (KM_ctx->RxState == RXSTATE_DONE) { + KM_ctx->RxState = RXSTATE_STARTCODE; + + switch (KM_ctx->RxBuff[3]) { + case 0x52: // Operation mode + + // Decode Rx message + KM_ctx->Rx.AssistLevel = KM_ctx->RxBuff[5]; // 0..255 + KM_ctx->Rx.Headlight = (KM_ctx->RxBuff[6] & 0xC0) >> 6; // KM_HEADLIGHT_OFF / KM_HEADLIGHT_ON / KM_HEADLIGHT_LOW / KM_HEADLIGHT_HIGH + KM_ctx->Rx.Battery = (KM_ctx->RxBuff[6] & 0x20) >> 5; // KM_BATTERY_NORMAL / KM_BATTERY_LOW + KM_ctx->Rx.PushAssist = (KM_ctx->RxBuff[6] & 0x10) >> 4; // KM_PUSHASSIST_OFF / KM_PUSHASSIST_ON + KM_ctx->Rx.PowerAssist = (KM_ctx->RxBuff[6] & 0x08) >> 3; // KM_POWERASSIST_OFF / KM_POWERASSIST_ON + KM_ctx->Rx.Throttle = (KM_ctx->RxBuff[6] & 0x04) >> 2; // KM_THROTTLE_OFF / KM_THROTTLE_ON + KM_ctx->Rx.CruiseControl = (KM_ctx->RxBuff[6] & 0x02) >> 1; // KM_CRUISE_OFF / KM_CRUISE_ON + KM_ctx->Rx.OverSpeed = (KM_ctx->RxBuff[6] & 0x01); // KM_OVERSPEED_NO / KM_OVERSPEED_YES + KM_ctx->Rx.SPEEDMAX_Limit_x10 = (((uint16_t) KM_ctx->RxBuff[8]) << 8) | KM_ctx->RxBuff[7]; + KM_ctx->Rx.CUR_Limit_x10 = (((uint16_t) KM_ctx->RxBuff[10]) << 8) | KM_ctx->RxBuff[9]; + + + // Prepare Tx message + TxBuff[0] = 0X3A; // StartCode + TxBuff[1] = 0x1A; // SrcAdd: Controller + TxBuff[2] = 0X28; // DestAdd: LCD + TxBuff[3] = 0x52; // CmdCode + TxBuff[4] = 0x06; // DataSize + + if (KM_ctx->Tx.Battery == KM_BATTERY_LOW) { + TxBuff[5] = 0x40; // State data (only UnderVoltage bit has influence on display) + } else { + TxBuff[5] = 0x00; // State data (only UnderVoltage bit has influence on display) + } + + TxBuff[6] = lowByte(KM_ctx->Tx.Current_x10); // Current low + TxBuff[7] = highByte(KM_ctx->Tx.Current_x10); // Current high + TxBuff[8] = lowByte(KM_ctx->Tx.Wheeltime_ms); // WheelSpeed low + TxBuff[9] = highByte(KM_ctx->Tx.Wheeltime_ms); // WheelSpeed high + TxBuff[10] = KM_ctx->Tx.Error; // Error + + TxCnt = 11; + break; + + + case 0x53: // Settings mode + + // Decode Rx message + KM_ctx->Settings.PAS_RUN_Direction = (KM_ctx->RxBuff[5] & 0x80) >> 7; // KM_PASDIR_FORWARD / KM_PASDIR_BACKWARD + KM_ctx->Settings.PAS_SCN_Tolerance = KM_ctx->RxBuff[6]; // 2..9 + KM_ctx->Settings.PAS_N_Ratio = KM_ctx->RxBuff[7]; // 0..255 + KM_ctx->Settings.HND_HL_ThrParam = (KM_ctx->RxBuff[8] & 0x80) >> 7; // KM_HND_HL_NO / KM_HND_HL_YES + KM_ctx->Settings.HND_HF_ThrParam = (KM_ctx->RxBuff[8] & 0x40) >> 6; // KM_HND_HF_NO / KM_HND_HF_YES + KM_ctx->Settings.SYS_SSP_SlowStart = KM_ctx->RxBuff[9]; // 1..9 + KM_ctx->Settings.SPS_SpdMagnets = KM_ctx->RxBuff[10]; // 1..4 + KM_ctx->Settings.VOL_1_UnderVolt_x10 = (((uint16_t) KM_ctx->RxBuff[12]) << 8) | KM_ctx->RxBuff[11]; + KM_ctx->Settings.WheelSize_mm = (((uint16_t) KM_ctx->RxBuff[14]) << 8) | KM_ctx->RxBuff[13]; + + + // Prepare Tx message with handshake code + TxBuff[0] = 0X3A; // StartCode + TxBuff[1] = 0x1A; // SrcAdd: Controller + TxBuff[2] = 0X28; // DestAdd: LCD + TxBuff[3] = 0x53; // CmdCode + TxBuff[4] = 0x01; // DataSize + TxBuff[5] = KM_901U_HANDSHAKE[KM_ctx->RxBuff[15]]; // Handshake answer + TxCnt = 6; + break; + + default: + TxCnt = 0; + } + + + // Send prepared message + if (TxCnt != 0) { + CheckSum = 0x0000; + + KM_ctx->SerialPort->write(TxBuff[0]); // Send StartCode + + for (i = 1; i < TxCnt; i++) { + KM_ctx->SerialPort->write(TxBuff[i]); // Send TxBuff[1..x] + CheckSum = CheckSum + TxBuff[i]; // Calculate CheckSum + } + + KM_ctx->SerialPort->write(lowByte(CheckSum)); // Send CheckSum low + KM_ctx->SerialPort->write(highByte(CheckSum)); // Send CheckSum high + + KM_ctx->SerialPort->write(0x0D); // Send CR + KM_ctx->SerialPort->write(0x0A); // Send LF + } + } +} +#endif + + +/**************************************************************************************************** + * KingMeter_Service() - Communicates data from and to the display + * + ***************************************************************************************************/ +void KingMeter_Service(KINGMETER_t* KM_ctx) { +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) + KM_618U_Service(KM_ctx); +#endif + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) + KM_901U_Service(KM_ctx); +#endif +} + +void kingmeter_update(void) { + /* Prepare Tx parameters */ + + if (battery_percent_fromcapacity > 10) { + KM.Tx.Battery = KM_BATTERY_NORMAL; + } else { + KM.Tx.Battery = KM_BATTERY_LOW; + } + + if ((ui16_time_ticks_between_speed_interrupt >> 4) < KM_MAX_WHEELTIME && ui16_time_ticks_between_speed_interrupt != 0) { + // Adapt wheeltime to match displayed speedo value according config.h setting + KM.Tx.Wheeltime_ms = ui16_time_ticks_between_speed_interrupt >> 4; // is not exactly correct, factor should be 15.625, not 16 + + + } else { + KM.Tx.Wheeltime_ms = KM_MAX_WHEELTIME; + } + + KM.Tx.Error = KM_ERROR_NONE; + + if (((ui8_current_cal_a * ui16_BatteryCurrent) / 100 - ui16_current_cal_b) != 0x99) { + KM.Tx.Current_x10 = (ui8_current_cal_a * ui16_BatteryCurrent) / 100 - ui16_current_cal_b; //calculate Amps out of 10bit ADC value + } + + /* Receive Rx parameters/settings and send Tx parameters */ + KingMeter_Service(&KM); + + + /* Apply Rx parameters */ + +#ifdef SUPPORT_LIGHTS_SWITCH + if (KM.Rx.Headlight == KM_HEADLIGHT_OFF) { + digitalWrite(lights_pin, 0); + } else // KM_HEADLIGHT_ON, KM_HEADLIGHT_LOW, KM_HEADLIGHT_HIGH + { + digitalWrite(lights_pin, 1); + } +#endif + + if (KM.Rx.PushAssist == KM_PUSHASSIST_ON) { +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) + //do anything +#else + //do something else +#endif + } else { + ui8_assistlevel_global = KM.Rx.AssistLevel + 80; // always add max regen + } + + + /* Shutdown in case we received no message in the last 3s + + if((millis() - KM.LastRx) > 3000) + { + poti_stat = 0; + throttle_stat = 0; + + }*/ +} + +void display_init() { + KingMeter_Init(&KM); +} + +void display_update() { + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) + + // fill local buffer from uart ringbuffer + uart_fill_rx_packet_buffer(ui8_rx, 6, &ui8_UARTCounter); + + // Check for reception of complete message + if ((ui8_UARTCounter > 5) || (ui8_rx[ui8_UARTCounter - 1] == 0x0D)) + ui8_UARTCounter = 0; +#endif + + kingmeter_update(); +} + +#endif // (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER) diff --git a/display_kingmeter.h b/display_kingmeter.h new file mode 100644 index 00000000..259cc179 --- /dev/null +++ b/display_kingmeter.h @@ -0,0 +1,195 @@ +/* +Library for King-Meter displays + +Copyright © 2015 Michael Fabry (Michael@Fabry.de) + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . + +*/ + + +#ifndef KINGMETER_H +#define KINGMETER_H + + +// Includes +#include "config.h" + +#if defined (DISPLAY_TYPE) && defined (DISPLAY_TYPE_KINGMETER) + + +// Definitions +#define KM_MAX_WHEELTIME 0x0DAC // Maximum Wheeltime reported to the display (e.g. when wheel is stopped) + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) + #define KM_MAX_RXBUFF 6 + #define KM_MAX_TXBUFF 8 +#endif + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_901U) + #define KM_MAX_RXBUFF 20 + #define KM_MAX_TXBUFF 11 +#endif + + +#define KM_PASDIR_FORWARD 0x00 +#define KM_PASDIR_BACKWARD 0x01 + +#define KM_HND_HL_NO 0x00 +#define KM_HND_HL_YES 0x01 + +#define KM_HND_HF_NO 0x00 +#define KM_HND_HF_YES 0x01 + + +#define KM_WHEELSIZE_8 638 +#define KM_WHEELSIZE_10 798 +#define KM_WHEELSIZE_12 958 +#define KM_WHEELSIZE_14 1117 + +#define KM_WHEELSIZE_16 1277 +#define KM_WHEELSIZE_18 1436 +#define KM_WHEELSIZE_20 1596 +#define KM_WHEELSIZE_22 1756 +#define KM_WHEELSIZE_24 1915 +#define KM_WHEELSIZE_26 2075 +#define KM_WHEELSIZE_700 2154 +#define KM_WHEELSIZE_28 2234 + +#define KM_WHEELSIZE_29 2294 + + + + + + + + + +#define KM_HEADLIGHT_OFF 0x00 +#define KM_HEADLIGHT_ON 0x01 +#define KM_HEADLIGHT_LOW 0x02 +#define KM_HEADLIGHT_HIGH 0x03 + +#define KM_BATTERY_NORMAL 0x00 +#define KM_BATTERY_LOW 0x01 + +#define KM_PUSHASSIST_OFF 0x00 +#define KM_PUSHASSIST_ON 0x01 + +#define KM_POWERASSIST_OFF 0x00 +#define KM_POWERASSIST_ON 0x01 + +#define KM_THROTTLE_OFF 0x00 +#define KM_THROTTLE_ON 0x01 + +#define KM_CRUISE_OFF 0x00 +#define KM_CRUISE_ON 0x01 + +#define KM_OVERSPEED_NO 0x00 // Speed below limit +#define KM_OVERSPEED_YES 0x01 // Overspeed detected + + + + + + +#define KM_ERROR_NONE 0x00 +#define KM_ERROR_COMM 0x30 + +extern uint8_t ui8_UART_tic; + +#if (DISPLAY_TYPE == DISPLAY_TYPE_KINGMETER_618U) +/* +uint16_t KM_WHEELSIZE [8] = + { + KM_WHEELSIZE_16, + KM_WHEELSIZE_18, + KM_WHEELSIZE_20, + KM_WHEELSIZE_22, + KM_WHEELSIZE_24, + KM_WHEELSIZE_26, + KM_WHEELSIZE_700, + KM_WHEELSIZE_28 + };*/ +#endif + +typedef struct +{ + // Parameters received from display in setting mode: + uint16_t WheelSize_mm; // Unit: 1mm + uint8_t PAS_RUN_Direction; // KM_PASDIR_FORWARD / KM_PASDIR_BACKWARD + uint8_t PAS_SCN_Tolerance; // Number of PAS signals to start the motor + uint8_t PAS_N_Ratio; // 0..255 PAS ratio + uint8_t HND_HL_ThrParam; // KM_HND_HL_NO / KM_HND_HL_YES + uint8_t HND_HF_ThrParam; // KM_HND_HF_NO / KM_HND_HF_YES + uint8_t SYS_SSP_SlowStart; // 1..4 Level of soft ramping at start + uint8_t SPS_SpdMagnets; // Number of magnets of speedsensor + uint16_t VOL_1_UnderVolt_x10; // Unit: 0.1V + +}RX_SETTINGS_t; + +typedef struct +{ + // Parameters received from display in operation mode: + uint8_t AssistLevel; // 0..255 Power Assist Level + uint8_t Headlight; // KM_HEADLIGHT_OFF / KM_HEADLIGHT_ON / KM_HEADLIGHT_LOW / KM_HEADLIGHT_HIGH + uint8_t Battery; // KM_BATTERY_NORMAL / KM_BATTERY_LOW + uint8_t PushAssist; // KM_PUSHASSIST_OFF / KM_PUSHASSIST_ON + uint8_t PowerAssist; // KM_POWERASSIST_OFF / KM_POWERASSIST_ON + uint8_t Throttle; // KM_THROTTLE_OFF / KM_THROTTLE_ON + uint8_t CruiseControl; // KM_CRUISE_OFF / KM_CRUISE_ON + uint8_t OverSpeed; // KM_OVERSPEED_OFF / KM_OVERSPEED_ON + uint16_t SPEEDMAX_Limit_x10; // Unit: 0.1km/h + uint16_t CUR_Limit_x10; // Unit: 0.1A + +}RX_PARAM_t; + +typedef struct +{ + // Parameters to be send to display in operation mode: + uint8_t Battery; // KM_BATTERY_NORMAL / KM_BATTERY_LOW + uint16_t Wheeltime_ms; // Unit:1ms + uint8_t Error; // KM_ERROR_NONE, .. + uint16_t Current_x10; // Unit: 0.1A + +}TX_PARAM_t; + + + +typedef struct +{ + + //HardwareSerial* SerialPort; + + + uint8_t RxState; + uint32_t LastRx; + + uint8_t RxBuff[KM_MAX_RXBUFF]; + uint8_t RxCnt; + + RX_SETTINGS_t Settings; + RX_PARAM_t Rx; + TX_PARAM_t Tx; + +}KINGMETER_t; + + +void display_init(); +void display_update(); + + +#endif // (DISPLAY_TYPE & DISPLAY_TYPE_KINGMETER) +#endif // KINGMETER_H \ No newline at end of file diff --git a/documentation/LCD3_to_S12SN-1.txt b/documentation/LCD3_to_S12SN-1.txt new file mode 100644 index 00000000..db302bd0 --- /dev/null +++ b/documentation/LCD3_to_S12SN-1.txt @@ -0,0 +1,145 @@ +S-LCD3 to S12SN communication protocol. + +Usable bits from C and P parameters: +(ui8_rx_buffer [X+2] <- BX) + +MX 6 B2&0xF8>>3 + B4&0x20 (+10 on display) +WZ 5 B2&0x07<<2 + B4&0xC0>>6 + +P1 8 B3 +P2 2 B4&0x07 +P3 1 B4&0x08 +P4 1 B4&0x10 +P5 8 B0 + +C1 3 B6&0x38>>3 +C2 3 B6&0x37 +C4 3 B8&0xE0>>5 +C5 4 B7&0x0F +C12 4 B9&0x0F +C13 3 B10&0x1C>>2 +C14 2 B7&0x60>>5 + +Packet consist of 13 bytes. 9600 baud, 8-n-1, byte-by-byte + +B0 B1 B2 B3 B4 B5 B6 B7 B8 B9 B10 B11 B12 + +(e.g: 12 0 149 160 41 102 18 74 4 20 0 50 14) + +for the P and C parameters description please see S-LCD3 user manual available at the bmsbattery.com + +B0: parameter P5. +B1: assist level, front light. +b7b6b5b4 b3b2b1b0 +. . . . . l2l1l0 assist level 0-5, 6-walk (long push down arrow) +f0. . . . . . . bit (mask 0x80) front light, display backlight +B3: parameter P1. +B2 and B4 max speed, wheel size, P2,P3,P4 +B2: b7b6b5b4 b3b2b1b0 and B4:b7b6b5b4 b3b2b1b0 + s4s3s2s1 s0. . . . . s5. . . . . max speed minus 10, + . . . . . . . . . . . . . . . . km/h; 6bit + . . . . . w4w3w2 w1w0. . . . . . wheel size:0x0e-10", + . . . . . . . . . . . . . . . . 0x02-12", 0x06-14", + . . . . . . . . . . . . . . . . 0x00-16",0x04-18", + . . . . . . . . . . . . . . . . 0x08-20", 0x0c-22", + . . . . . . . . . . . . . . . . 0x10-24", 0x14"-26", + . . . . . . . . . . . . . . . . 0x18-700c + . . . . . . . . . . . . . p2p1p0 par. P2 (B4&&0x07) + . . . . . . . . . . . . p0. . . par. P3 (B4&&0x08) + . . . . . . . . . . . p0 . . . . par. P4 (B4&&0x10) +Example: + 0 1 1 1 1 . . . . . 0. . . . . 25km/h (15+10) + 1 1 1 1 0 . . . . . 0. . . . . 40km/h (30+10) + 1 0 0 1 0 . . . . . 1. . . . . 60km/h (50+10) +B5: CRC = (xor B0,B1,B2,B3,B4,B6,B7,B8,B9,B10,B11,B12) xor 2. +B6: parameters C1 and C2 +b7b6b5b4 b3b2b1b0 +. . c2c1 c0. . . param C1 (mask 0x38) +. . . . . c2c1c0 param C2 (mask 0x07) +B7: parameter C5 and C7 +b7b6b5b4 b3b2b1b0 +. . . . c3c2c1c0 param C5 (mask 0x0F) +. c1c0. . . . . param C14 (mask 0x60) +B8: parameter C4 +b7b6b5b4 b3b2b1b0 +c2c1c0. . . . . param C4 (mask 0xE0) +B9: parameter C12 +b7b6b5b4 b3b2b1b0 +. . . . c3c2c1c0 param C12 (mask 0x0F) +B10: parameter C13 +b7b6b5b4 b3b2b1b0 +. . . c2 c1c0. . param C13 (mask 0x1C) +B11: 50 dec (0x32) +B12: 14 dec (0x0E) +parameters C3, C6, C7, C8, C9, C10 not sent to MCU + +if C11 set to 2 (used to copy LCD to LCD), LCD repeatedly sends 23 bytes, byte by byte, no separators, underlines show not identified values + + +255, wheel diam (in), maxspeed (kmh), level, P1, P2, P3, P4, P5, C1, C2 , C3, C4, C5, C6, C7, C8, 0, 20, C12, C13 ,C14, 55 + +Example: +255 26 60 0 160 1 1 0 12 2 1 8 0 10 3 0 1 0 20 4 0 2 55 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/documentation/S12SN_to_LCD3.txt b/documentation/S12SN_to_LCD3.txt new file mode 100644 index 00000000..af0182ea --- /dev/null +++ b/documentation/S12SN_to_LCD3.txt @@ -0,0 +1,25 @@ +S12SN to LCD3 communication protocol. + +Packet consist of 12 bytes. 9600 baud, 8-n-1, byte-by-byte, no separators + +B0 B1 B2 B3 B4 B5 B6 B7 B8 B9 B10 B11 + +(e.g: 65 16 48 0 139 0 164 2 13 0 0 0) + +B0: 65 dec (0x41) +B1: battery level: 0: empty box, 1: border flashing, 2: animated charging, 3: empty, 4: 1 bar, 8: 2 bars, 16: 4 bars (full), +B2: 48 dec (0x30) +B3,B4: speed, wheel rotation period, ms; period(ms)=B3*256+B4; +B5 error info display: 0x20: "0info", 0x21: "6info", 0x22: "1info", 0x23: "2info", 0x24: "3info", 0x25: "0info", 0x26: "4info", 0x28: "0info" +B6: CRC: xor B0,B1,B2,B3,B4,B5,B7,B8,B9,B10,B11 +B7: moving mode indication, bit +b7b6b5b4 b3b2b1b0 +. . . . . . . m0 if set animated circle "throttle" +. . . . m0 . . . if set "C" (cruise) is shown +. . . m0 . . . . if set "assist" shown +B8: power in 13 wt increments (48V version of the controller) +B9: motor temperature, can be negative or positive,T(C)=(int8)B8+15, + if temperature > 120C LCD screen is flashing. + e.g 0xDA T=-23C, 0x34 T=67C +B10: 0 +B11: 0 diff --git a/documentation/phasederivation.ods b/documentation/phasederivation.ods new file mode 100644 index 00000000..03b06a1b Binary files /dev/null and b/documentation/phasederivation.ods differ diff --git a/experimental settings/48V-28in-DD-12FET-BluOsecX.ini b/experimental settings/48V-28in-DD-12FET-BluOsecX.ini new file mode 100644 index 00000000..b7c7b660 --- /dev/null +++ b/experimental settings/48V-28in-DD-12FET-BluOsecX.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +100 +300 +50 +238 +46 +1.6 +-110.4 +12 +21 +30 +59 +100 +50 +50 +50 +1500 +0.5 +0.2 +24 +13 +1.9 +false +64000 +35 +127 +false +true +false +false +true +false +false +1 +38 +82 +128 +true +false +166 +210 +0.0 +4764 +67 +128 +208 diff --git a/experimental settings/48V-28in-Q128C-12FET-BluOsecX.ini b/experimental settings/48V-28in-Q128C-12FET-BluOsecX.ini new file mode 100644 index 00000000..91984720 --- /dev/null +++ b/experimental settings/48V-28in-Q128C-12FET-BluOsecX.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +64 +350 +50 +246 +46 + + +26 +38 +55 +80 +100 +50 +50 +50 +1500 +0.5 +0.2 +69 + +1.9 +false +64000 +35 +127 + + +false +false + + +false +1 +41 +84 +129 +true +false +167 +212 +0.0 +4764 +67 +128 +208 diff --git a/experimental settings/48V-28in-XF15-12FET-BluOsecX.ini b/experimental settings/48V-28in-XF15-12FET-BluOsecX.ini new file mode 100644 index 00000000..ebb0ea0a --- /dev/null +++ b/experimental settings/48V-28in-XF15-12FET-BluOsecX.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +66 +350 +50 +238 +46 +1.6 +-110.4 +22 +33 +50 +75 +100 +50 +50 +50 +1500 +0.5 +0.2 +51 +13 +1.9 +false +64000 +35 +127 +false +true +false +false + + +false +1 +46 +88 +128 +true +false +172 +213 +0.0 +4764 +67 +128 +208 diff --git a/gpio.c b/gpio.c index 2dd3ce9b..bcb76dd5 100644 --- a/gpio.c +++ b/gpio.c @@ -32,4 +32,22 @@ void debug_pin_reset (void) GPIO_WriteLow(DEBUG__PORT, DEBUG__PIN); } +// added by DerBastler - Light +void light_pin_init (void) +{ + GPIO_Init(LIGHT__PORT, + LIGHT__PIN, + GPIO_MODE_OUT_PP_HIGH_FAST); +} +// added by DerBastler - Light +void light_pin_set (void) +{ + GPIO_WriteHigh(LIGHT__PORT, LIGHT__PIN); +} +// added by DerBastler - Light +void light_pin_reset (void) +{ + GPIO_WriteLow(LIGHT__PORT, LIGHT__PIN); +} + diff --git a/gpio.h b/gpio.h index 94595f7f..4aa1b19c 100644 --- a/gpio.h +++ b/gpio.h @@ -1,112 +1,132 @@ -/* - * BMSBattery S series motor controllers firmware - * - * Copyright (C) Casainho, 2017. - * - * Released under the GPL License, Version 3 - */ - -/* Connections: - * - * Motor PHASE_A: yellow wire - * Motor PHASE_B: green wire - * Motor PHASE_C: blue wire - * - * - * PIN | IN/OUT| Works?|Function - * ---------------------------------------------------------- - * PB5 (ADC_AIN5) | in | ?? | current_phase_B - * PB6 (ADC_AIN6) | in | ?? | total_current - * - * PE6 (ADC_AIN9) | in | ?? | battery_voltage - * - * PE0 | in | ?? | Hall_sensor_A - * PE1 | in | ?? | Hall_sensor_B - * PE2 | in | ?? | Hall_sensor_C - * - * PB2 (TIM1_CH3N) | out | ?? | PWM_phase_A_low - * PB1 (TIM1_CH2N) | out | ?? | PWM_phase_B_low - * PB0 (TIM1_CH1N) | out | ?? | PWM_phase_C_low - * PC3 (TIM1_CH3) | out | ?? | PWM_phase_A_high - * PC2 (TIM1_CH2) | out | ?? | PWM_phase_B_high - * PC1 (TIM1_CH1) | out | ?? | PWM_phase_C_high - * - * PD5 (UART2_TX) | out | ?? | usart_tx - * PD6 (UART2_RX) | out | ?? | usart_rx - * - * PA4 | in | ?? | brake - * PB4 (ADC_AIN4) | in | ?? | throttle - * PD0 | in | ?? | PAS - * PC5 | in | ?? | SPEED - * - * - */ - -#ifndef _GPIO_H_ -#define _GPIO_H_ - -#include "main.h" -#include "stm8s_gpio.h" - -#define CURRENT_PHASE_B__PIN GPIO_PIN_5 -#define CURRENT_PHASE_B__PORT GPIOB -#define CURRENT_TOTAL__PIN GPIO_PIN_6 -#define CURRENT_TOTAL__PORT GPIOB - -#define BATTERY_VOLTAGE__PIN GPIO_PIN_6 -#define BATTERY_VOLTAGE__PORT GPIOE - -#if ((MOTOR_TYPE == MOTOR_TYPE_Q85) || (MOTOR_TYPE == MOTOR_TYPE_EUC2)) - #define HALL_SENSOR_A__PIN GPIO_PIN_0 - #define HALL_SENSOR_B__PIN GPIO_PIN_1 - #define HALL_SENSOR_C__PIN GPIO_PIN_2 -#else -#endif - -#define HALL_SENSOR_A__PORT GPIOE -#define HALL_SENSOR_B__PORT GPIOE -#define HALL_SENSOR_C__PORT GPIOE -#define HALL_SENSORS__PORT GPIOE -#define HALL_SENSORS_MASK (HALL_SENSOR_A__PIN | HALL_SENSOR_B__PIN | HALL_SENSOR_C__PIN) - -#define PMW_PHASE_A_LOW__PIN GPIO_PIN_2 -#define PMW_PHASE_A_LOW__PORT GPIOB -#define PMW_PHASE_B_LOW__PIN GPIO_PIN_1 -#define PMW_PHASE_B_LOW__PORT GPIOB -#define PMW_PHASE_C_LOW__PIN GPIO_PIN_0 -#define PMW_PHASE_C_LOW__PORT GPIOB -#define PMW_PHASE_A_HIGH__PIN GPIO_PIN_3 -#define PMW_PHASE_A_HIGH__PORT GPIOC -#define PMW_PHASE_B_HIGH__PIN GPIO_PIN_2 -#define PMW_PHASE_B_HIGH__PORT GPIOC -#define PMW_PHASE_C_HIGH__PIN GPIO_PIN_1 -#define PMW_PHASE_C_HIGH__PORT GPIOC - -#define UART2_TX__PIN GPIO_PIN_5 -#define UART2_TX__PORT GPIOD -#define UART2_RX__PIN GPIO_PIN_6 -#define UART2_RX__PORT GPIOD - -#define BRAKE__PIN GPIO_PIN_4 -#define BRAKE__PORT GPIOA - -#define THROTTLE__PIN GPIO_PIN_4 -#define THROTTLE__PORT GPIOB - -#define PAS__PIN GPIO_PIN_0 -#define PAS__PORT GPIOD - -#define SPEED__PIN GPIO_PIN_5 -#define SPEED__PORT GPIOC - -#define DEBUG__PIN GPIO_PIN_2 -#define DEBUG__PORT GPIOD - -void gpio_init (void); -void debug_pin_init (void); -void debug_pin_set (void); -void debug_pin_reset (void); -void PAS_init (void); -void SPEED_init (void); - -#endif /* GPIO_H_ */ +/* + * BMSBattery S series motor controllers firmware + * + * Copyright (C) Casainho, 2017. + * + * Released under the GPL License, Version 3 + */ + +/* Connections: + * + * Motor PHASE_A: yellow wire + * Motor PHASE_B: green wire + * Motor PHASE_C: blue wire + * + * + * PIN | IN/OUT| Works?|Function + * ---------------------------------------------------------- + * PB5 (ADC_AIN5) | in | ?? | current_phase_B + * PB6/PE7 (ADC_AIN6/8) | in | ?? | total_current ???? + * + * PE6 (ADC_AIN9) | in | ?? | battery_voltage + * + * PE0 | in | ?? | Hall_sensor_A + * PE1 | in | ?? | Hall_sensor_B + * PE2 | in | ?? | Hall_sensor_C + * + * PB2 (TIM1_CH3N) | out | ?? | PWM_phase_A_low + * PB1 (TIM1_CH2N) | out | ?? | PWM_phase_B_low + * PB0 (TIM1_CH1N) | out | ?? | PWM_phase_C_low + * PC3 (TIM1_CH3) | out | ?? | PWM_phase_A_high + * PC2 (TIM1_CH2) | out | ?? | PWM_phase_B_high + * PC1 (TIM1_CH1) | out | ?? | PWM_phase_C_high + * + * PD5 (UART2_TX) | out | ?? | usart_tx + * PD6 (UART2_RX) | out | ?? | usart_rx + * + * PA4 | in | ?? | brake + * PB4 (ADC_AIN4) | in | ?? | throttle + * PD0 | in | ?? | PAS + * PC5 | in | ?? | SPEED + * PD7 | in | ?? | OverCurrent + * + * + */ + +#ifndef _GPIO_H_ +#define _GPIO_H_ + +#include "main.h" +#include "stm8s_gpio.h" + +#define CURRENT_PHASE_B__PIN GPIO_PIN_5 +#define CURRENT_PHASE_B__PORT GPIOB + +#define CURRENT_TOTAL__PIN GPIO_PIN_7 +#define CURRENT_TOTAL__PORT GPIOE + +#define CURRENT_MOTOR_TOTAL__PIN GPIO_PIN_6 +#define CURRENT_MOTOR_TOTAL__PORT GPIOB + +#define CURRENT_MOTOR_TOTAL_FILTERED__PIN GPIO_PIN_7 +#define CURRENT_MOTOR_TOTAL_FILTRED__PORT GPIOE + +#define BATTERY_VOLTAGE__PIN GPIO_PIN_6 +#define BATTERY_VOLTAGE__PORT GPIOE + +#define CURRENT_MOTOR_TOTAL_OVER__PIN GPIO_PIN_7 +#define CURRENT_MOTOR_TOTAL_OVER__PORT GPIOD + + +#define HALL_SENSOR_A__PIN GPIO_PIN_0 +#define HALL_SENSOR_B__PIN GPIO_PIN_1 +#define HALL_SENSOR_C__PIN GPIO_PIN_2 + + +#define HALL_SENSOR_A__PORT GPIOE +#define HALL_SENSOR_B__PORT GPIOE +#define HALL_SENSOR_C__PORT GPIOE +#define HALL_SENSORS__PORT GPIOE +#define HALL_SENSORS_MASK (HALL_SENSOR_A__PIN | HALL_SENSOR_B__PIN | HALL_SENSOR_C__PIN) + +#define PMW_PHASE_A_LOW__PIN GPIO_PIN_2 +#define PMW_PHASE_A_LOW__PORT GPIOB +#define PMW_PHASE_B_LOW__PIN GPIO_PIN_1 +#define PMW_PHASE_B_LOW__PORT GPIOB +#define PMW_PHASE_C_LOW__PIN GPIO_PIN_0 +#define PMW_PHASE_C_LOW__PORT GPIOB +#define PMW_PHASE_A_HIGH__PIN GPIO_PIN_3 +#define PMW_PHASE_A_HIGH__PORT GPIOC +#define PMW_PHASE_B_HIGH__PIN GPIO_PIN_2 +#define PMW_PHASE_B_HIGH__PORT GPIOC +#define PMW_PHASE_C_HIGH__PIN GPIO_PIN_1 +#define PMW_PHASE_C_HIGH__PORT GPIOC + +#define UART2_TX__PIN GPIO_PIN_5 +#define UART2_TX__PORT GPIOD +#define UART2_RX__PIN GPIO_PIN_6 +#define UART2_RX__PORT GPIOD + +#define BRAKE__PIN GPIO_PIN_4 +#define BRAKE__PORT GPIOA + +#define THROTTLE__PIN GPIO_PIN_4 +#define THROTTLE__PORT GPIOB + +#define REGEN_THROTTLE__PIN GPIO_PIN_7 +#define REGEN_THROTTLE__PORT GPIOB + +#define PAS__PIN GPIO_PIN_0 +#define PAS__PORT GPIOD + +#define SPEED__PIN GPIO_PIN_5 +#define SPEED__PORT GPIOC + +#define DEBUG__PIN GPIO_PIN_2 +#define DEBUG__PORT GPIOD + +#define LIGHT__PIN GPIO_PIN_4 +#define LIGHT__PORT GPIOC + + +void gpio_init (void); +void debug_pin_init (void); +void debug_pin_set (void); +void debug_pin_reset (void); +void PAS_init (void); +void SPEED_init (void); +void light_pin_init (void); +void light_pin_set (void); +void light_pin_reset (void); + +#endif /* GPIO_H_ */ diff --git a/interrupts.h b/interrupts.h index f7070073..7bb05503 100644 --- a/interrupts.h +++ b/interrupts.h @@ -16,5 +16,6 @@ #define TIM1_UPD_OVF_TRG_BRK_IRQHANDLER 11 #define TIM2_UPD_OVF_TRG_BRK_IRQHANDLER 13 #define ADC1_IRQHANDLER 22 +#define UART2_IRQHANDLER 21 #endif diff --git a/main.c b/main.c index 8596730b..0dc1a12e 100644 --- a/main.c +++ b/main.c @@ -1,216 +1,221 @@ -/* - * BMSBattery S series motor controllers firmware - * - * Copyright (C) Casainho, 2017. - * - * Released under the GPL License, Version 3 - */ - -#include -#include -#include "stm8s.h" -#include "gpio.h" -#include "stm8s_itc.h" -#include "stm8s_gpio.h" -#include "interrupts.h" -#include "stm8s_tim2.h" -#include "motor.h" -#include "uart.h" -#include "adc.h" -#include "brake.h" -#include "utils.h" -#include "cruise_control.h" -#include "timers.h" -#include "pwm.h" -#include "PAS.h" -#include "SPEED.h" -#include "update_setpoint.h" -#include "config.h" - - -//uint16_t ui16_LPF_angle_adjust = 0; -//uint16_t ui16_LPF_angle_adjust_temp = 0; - -uint16_t ui16_log1 = 0; -uint16_t ui16_log2 = 0; -uint8_t ui8_log = 0; -uint8_t ui8_i= 0; //counter for ... next loop -uint16_t ui16_torque[NUMBER_OF_PAS_MAGS]; //array for torque values of one crank revolution -uint16_t ui16_sum_torque = 0; //sum of array elements -uint8_t ui8_torque_index=0 ; //counter for torque array -uint8_t a = 0; //loop counter - -static uint16_t ui16_throttle_counter = 0; -uint16_t ui16_temp_delay = 0; - - - - -uint8_t ui8_adc_read_throttle_busy = 0; -uint16_t ui16_SPEED_Counter = 0; //time tics for speed measurement -uint16_t ui16_SPEED = 32000; //speed in timetics -uint16_t ui16_PAS_Counter = 0; //time tics for cadence measurement -uint16_t ui16_PAS = 32000; //cadence in timetics -uint8_t ui8_PAS_Flag = 0; //flag for PAS interrupt -uint8_t ui8_SPEED_Flag = 0; //flag for SPEED interrupt - -///////////////////////////////////////////////////////////////////////////////////////////// -//// Functions prototypes - -// main -- start of firmware and main loop -int main (void); - -//With SDCC, interrupt service routine function prototypes must be placed in the file that contains main () -//in order for an vector for the interrupt to be placed in the the interrupt vector space. It's acceptable -//to place the function prototype in a header file as long as the header file is included in the file that -//contains main (). SDCC will not generate any warnings or errors if this is not done, but the vector will -//not be in place so the ISR will not be executed when the interrupt occurs. - -//Calling a function from interrupt not always works, SDCC manual says to avoid it. Maybe the best is to put -//all the code inside the interrupt - -// Local VS global variables -// Sometimes I got the following error when compiling the firmware: motor.asm:750: Error: relocation error -// and the solution was to avoid using local variables and define them as global instead - -// Brake signal interrupt -void EXTI_PORTA_IRQHandler(void) __interrupt(EXTI_PORTA_IRQHANDLER); -// Speed signal interrupt -void EXTI_PORTC_IRQHandler(void) __interrupt(EXTI_PORTC_IRQHANDLER); -// PAS signal interrupt -void EXTI_PORTD_IRQHandler(void) __interrupt(EXTI_PORTD_IRQHANDLER); - -// Timer1/PWM period interrupt -void TIM1_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM1_UPD_OVF_TRG_BRK_IRQHANDLER); - -///////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////////////////////////////// - -int main (void) -{ -// static uint32_t ui32_cruise_counter = 0; -// static uint8_t ui8_cruise_duty_cycle = 0; - static uint16_t ui16_setpoint = ADC_THROTTLE_MIN_VALUE; - static uint8_t ui8_temp = 0; - - - //set clock at the max 16MHz - CLK_HSIPrescalerConfig(CLK_PRESCALER_HSIDIV1); - - gpio_init (); - brake_init (); - while (brake_is_set()) ; // hold here while brake is pressed -- this is a protection for development - debug_pin_init (); - timer2_init (); - uart_init (); - pwm_init (); - hall_sensor_init (); - adc_init (); - PAS_init(); - SPEED_init(); - -// ITC_SetSoftwarePriority (ITC_IRQ_TIM1_OVF, ITC_PRIORITYLEVEL_2); - - enableInterrupts(); -#if (SVM_TABLE == SVM) - TIM1_SetCompare1(126 << 1); - TIM1_SetCompare2(126 << 1); - TIM1_SetCompare3(126 << 1); -#elif (SVM_TABLE == SINE) || (SVM_TABLE == SINE_SVM) - TIM1_SetCompare1(126 << 2); - TIM1_SetCompare2(126 << 2); - TIM1_SetCompare3(126 << 2); -#endif - - hall_sensors_read_and_action (); // needed to start the motor -printf("Back in Main.c\n"); - - for(a = 0; a < NUMBER_OF_PAS_MAGS;a++) {// array init - ui16_torque[a]=0; - } -printf("Torquearray initialized\n"); - while (1) - { - static uint32_t ui32_counter = 0; - uint16_t ui16_temp = 0; - uint16_t ui32_temp = 0; - static float f_temp = 0; - - // Update speed after speed interrupt occurrence - if (ui8_SPEED_Flag == 1) - { - ui16_SPEED=ui16_SPEED_Counter; //save recent speed - ui16_SPEED_Counter=0; //reset speed counter - ui8_SPEED_Flag =0; //reset interrupt flag - //printf("SPEEDtic\n"); - } - - -#ifdef TORQUESENSOR - // Update cadence and torque after PAS interrupt occurrence - if (ui8_PAS_Flag == 1) - { - ui16_PAS=ui16_PAS_Counter; //save recent cadence - //printf("PAStic %d\n", ui16_PAS_Counter); - ui16_PAS_Counter=0; //reset PAS Counter - - ui8_PAS_Flag =0; //reset interrupt flag - - ui8_adc_read_throttle_busy = 1; - ui8_temp = adc_read_throttle (); //read in recent torque value - ui8_adc_read_throttle_busy = 0; - ui16_torque[ui8_torque_index]= (uint8_t) map (ui8_temp , ADC_THROTTLE_MIN_VALUE, ADC_THROTTLE_MAX_VALUE, 0, SETPOINT_MAX_VALUE); //map throttle to limits - ui16_sum_torque = 0; - for(a = 0; a < NUMBER_OF_PAS_MAGS; a++) { // sum up array content - ui16_sum_torque+= ui16_torque[a]; - } - ui8_torque_index++; - if (ui8_torque_index>NUMBER_OF_PAS_MAGS-1){ui8_torque_index=0;} //reset index counter - - } - -#else // just read in throttle value - ui8_adc_read_throttle_busy = 1; - ui8_temp= adc_read_throttle (); //read in recent torque value - ui16_sum_torque = (uint8_t) map (ui8_temp , ADC_THROTTLE_MIN_VALUE, ADC_THROTTLE_MAX_VALUE, 0, SETPOINT_MAX_VALUE); //map throttle to limits - ui8_adc_read_throttle_busy = 0; - -#endif -// scheduled update of setpoint and duty cycle (every 100ms) - ui16_temp_delay = TIM2_GetCounter (); - - if ((ui16_temp_delay - ui16_throttle_counter) > 100) - { - ui16_throttle_counter = ui16_temp_delay; - //printf("Timetic!"); - -//#define DO_CRUISE_CONTROL 1 -#if DO_CRUISE_CONTROL == 1 - ui16_setpoint = (uint16_t)update_setpoint (ui16_SPEED,ui16_PAS,ui16_sum_torque,ui16_setpoint); //update setpoint -/* -//Read in throttle for debugging to test, if motor runs with additional interrupts from PAS and SPEEDk - ui8_adc_read_throttle_busy = 1; - ui16_setpoint = (uint16_t) adc_read_throttle (); //read in recent torque value - ui8_adc_read_throttle_busy = 0; - -*/ - ui16_setpoint = cruise_control (ui16_setpoint); -#endif - -#if TORQUESENSOR - pwm_set_duty_cycle ((uint8_t)ui16_setpoint); -#else if THROTTLE - pwm_set_duty_cycle ((uint8_t)ui16_sum_torque); -#endif - - getchar1 (); - - // printf("Main: spd %d, pas %d, sumtor %d, setpoint %d\n", ui16_SPEED, ui16_PAS, ui16_sum_torque, ui16_setpoint); - if(ui16_speed_inverse < 60 ) { ui8_position_correction_value = 152-((ui16_speed_inverse*44)/100);} - else {ui8_position_correction_value=127;} - -// printf("%d, %d\n", ui16_speed_inverse, ui8_position_correction_value); - printf("%d, %d\n", ui8_motor_state, ui16_PWM_cycles_counter_total); - } - } -} +/* + * BMSBattery S series motor controllers firmware + * + * Copyright (C) Casainho, 2017. + * + * Released under the GPL License, Version 3 + */ + +#include +#include +#include "stm8s.h" +#include "gpio.h" +#include "stm8s_itc.h" +#include "stm8s_gpio.h" +#include "interrupts.h" +#include "stm8s_tim2.h" +#include "motor.h" +#include "main.h" +#include "uart.h" +#include "adc.h" +#include "brake.h" +#include "cruise_control.h" +#include "timers.h" +#include "pwm.h" +#include "PAS.h" +#include "SPEED.h" +//#include "update_setpoint.h" +#include "ACAsetPoint.h" +#include "config.h" +#include "display.h" +#include "display_kingmeter.h" +#include "ACAcontrollerState.h" +#include "BOdisplay.h" +#include "ACAeeprom.h" +#include "ACAcommons.h" + +//uint16_t ui16_LPF_angle_adjust = 0; +//uint16_t ui16_LPF_angle_adjust_temp = 0; + +uint16_t ui16_log1 = 0; +uint8_t ui8_slowloop_flag = 0; +uint8_t ui8_veryslowloop_counter = 0; +uint8_t ui8_ultraslowloop_counter = 0; +uint16_t ui16_log2 = 0; +uint8_t ui8_log = 0; +uint8_t ui8_i = 0; //counter for ... next loop + +float float_kv = 0; +float float_R = 0; +uint8_t a = 0; //loop counter + +static int16_t i16_deziAmps; + + +///////////////////////////////////////////////////////////////////////////////////////////// +//// Functions prototypes + +// main -- start of firmware and main loop +int main(void); + +//With SDCC, interrupt service routine function prototypes must be placed in the file that contains main () +//in order for an vector for the interrupt to be placed in the the interrupt vector space. It's acceptable +//to place the function prototype in a header file as long as the header file is included in the file that +//contains main (). SDCC will not generate any warnings or errors if this is not done, but the vector will +//not be in place so the ISR will not be executed when the interrupt occurs. + +//Calling a function from interrupt not always works, SDCC manual says to avoid it. Maybe the best is to put +//all the code inside the interrupt + +// Local VS global variables +// Sometimes I got the following error when compiling the firmware: motor.asm:750: Error: relocation error +// and the solution was to avoid using local variables and define them as global instead + +// Brake signal interrupt +void EXTI_PORTA_IRQHandler(void) __interrupt(EXTI_PORTA_IRQHANDLER); +// Speed signal interrupt +void EXTI_PORTC_IRQHandler(void) __interrupt(EXTI_PORTC_IRQHANDLER); +// PAS signal interrupt +void EXTI_PORTD_IRQHandler(void) __interrupt(EXTI_PORTD_IRQHANDLER); + +// Timer1/PWM period interrupt +void TIM1_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM1_UPD_OVF_TRG_BRK_IRQHANDLER); + +// Timer2/slow control loop +void TIM2_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM2_UPD_OVF_TRG_BRK_IRQHANDLER); + +// UART2 receivce handler +void UART2_IRQHandler(void) __interrupt(UART2_IRQHANDLER); + + +///////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// + +int main(void) { + //set clock at the max 16MHz + CLK_HSIPrescalerConfig(CLK_PRESCALER_HSIDIV1); + + gpio_init(); + brake_init(); + while (brake_is_set()); // hold here while brake is pressed -- this is a protection for development + debug_pin_init(); + light_pin_init(); + timer2_init(); + uart_init(); + eeprom_init(); + controllerstate_init(); + initErpsRatio(); + pwm_init(); + hall_sensor_init(); + adc_init(); + PAS_init(); + SPEED_init(); + aca_setpoint_init(); +#if (defined (DISPLAY_TYPE) && defined (DISPLAY_TYPE_KINGMETER)) || defined DISPLAY_TYPE_KT_LCD3 || defined BLUOSEC + display_init(); +#endif + + // ITC_SetSoftwarePriority (ITC_IRQ_TIM1_OVF, ITC_PRIORITYLEVEL_2); + + enableInterrupts(); + + watchdog_init(); //init watchdog after enabling interrupt to have fast loop running already + +#if (SVM_TABLE == SVM) + TIM1_SetCompare1(126 << 1); + TIM1_SetCompare2(126 << 1); + TIM1_SetCompare3(126 << 1); +#elif (SVM_TABLE == SINE) || (SVM_TABLE == SINE_SVM) + TIM1_SetCompare1(126 << 2); + TIM1_SetCompare2(126 << 2); + TIM1_SetCompare3(126 << 2); +#endif + + hall_sensors_read_and_action(); // needed to start the motor + //printf("Back in Main.c\n"); + + for (a = 0; a < NUMBER_OF_PAS_MAGS; a++) {// array init + ui16_torque[a] = 0; + } +#ifdef DIAGNOSTICS + printf("System initialized\r\n"); +#endif + while (1) { + + uart_send_if_avail(); + + updateSpeeds(); + updatePasStatus(); + +#if (defined (DISPLAY_TYPE) && defined (DISPLAY_TYPE_KINGMETER)) || defined DISPLAY_TYPE_KT_LCD3 || defined BLUOSEC + display_update(); +#endif + + // scheduled update of setpoint and duty cycle (slow loop, 50 Hz) + if (ui8_slowloop_flag) { + //printf("MainSlowLoop\n"); + debug_pin_set(); + ui8_slowloop_flag = 0; //reset flag for slow loop + ui8_veryslowloop_counter++; // increase counter for very slow loop + + checkPasInActivity(); + updateRequestedTorque(); //now calculates tq for sensor as well + updateSlowLoopStates(); + updateX4(); + updateLight(); + ui16_setpoint = (uint16_t) aca_setpoint(ui16_time_ticks_between_pas_interrupt, ui16_setpoint); //update setpoint + + //#define DO_CRUISE_CONTROL 1 +#if DO_CRUISE_CONTROL == 1 + ui16_setpoint = cruise_control(ui16_setpoint); +#endif + + pwm_set_duty_cycle((uint8_t) ui16_setpoint); + + //pwm_set_duty_cycle ((uint8_t)ui16_sum_throttle); + + + + + /****************************************************************************/ + //very slow loop for communication + if (ui8_veryslowloop_counter > 5) { + + ui8_ultraslowloop_counter++; + ui8_veryslowloop_counter = 0; + + if (ui8_ultraslowloop_counter > 10) { + ui8_ultraslowloop_counter = 0; + ui8_uptime++; + } + +#ifdef DIAGNOSTICS + //uint32_torquesensorCalibration=80; + printf("%u,%u, %u, %u, %u, %u\r\n", ui16_control_state, (uint16_t) uint32_current_target, PAS_is_active, ui16_BatteryCurrent, ui16_sum_torque, (uint16_t)uint32_torquesensorCalibration); + + //printf("erps %d, motorstate %d, cyclecountertotal %d\r\n", ui16_motor_speed_erps, ui8_possible_motor_state|ui8_dynamic_motor_state, ui16_PWM_cycles_counter_total); + + //printf("cheatstate, %d, km/h %lu, Voltage, %d, setpoint %d, erps %d, current %d, correction_value, %d\n", ui8_offroad_state, ui32_speed_sensor_rpks, ui8_BatteryVoltage, ui16_setpoint, ui16_motor_speed_erps, ui16_BatteryCurrent, ui8_position_correction_value); + + //printf("kv %d, erps %d, R %d\n", (uint16_t)(float_kv*10.0) , ui16_motor_speed_erps, (uint16_t)(float_R*1000.0)); + + /*for(a = 0; a < 6; a++) { // sum up array content + putchar(uint8_t_hall_case[a]); + } + putchar(ui16_ADC_iq_current>>2); + putchar(ui8_position_correction_value); + putchar(255);*/ + // printf("%d, %d, %d, %d, %d, %d\r\n", (uint16_t) uint8_t_hall_case[0], (uint16_t)uint8_t_hall_case[1],(uint16_t) uint8_t_hall_case[2],(uint16_t) uint8_t_hall_case[3], (uint16_t)uint8_t_hall_case[4], (uint16_t)uint8_t_hall_case[5]); + //printf("%d, %d, %d, %d, %d, %d, %d,\r\n", ui8_position_correction_value, ui16_BatteryCurrent, ui16_setpoint, ui8_regen_throttle, ui16_motor_speed_erps, ui16_ADC_iq_current>>2,ui16_adc_read_battery_voltage()); + + + //printf("correction angle %d, Current %d, Voltage %d, sumtorque %d, setpoint %d, km/h %lu\n",ui8_position_correction_value, i16_deziAmps, ui8_BatteryVoltage, ui16_sum_throttle, ui16_setpoint, ui32_speed_sensor_rpks); +#endif + }//end of very slow loop + + debug_pin_reset(); + }// end of slow loop + }// end of while(1) loop +} + + diff --git a/main.h b/main.h index 87382615..4b0ec1bd 100644 --- a/main.h +++ b/main.h @@ -9,64 +9,36 @@ #ifndef _MAIN_H_ #define _MAIN_H_ -#define MOTOR_TYPE_Q85 1 -#define MOTOR_TYPE_EUC2 2 - -#define MOTOR_TYPE MOTOR_TYPE_EUC2 - -#define SVM_TABLE_LEN 256 +#define SVM_VIRTUAL_TABLE_LEN 65 // 256/4 +1, we need only one quarter #define SVM_TABLE_LEN_x1024 262144 //(256 * 1024) -#define SETPOINT_MAX_VALUE 237 //maximum value for setpoint, taken from map function - -#define NUMBER_OF_PAS_MAGS 16 //16 for sensor from BMSBattery, 32 for Sempu-Sensor - +#define SETPOINT_MAX_VALUE 255 //maximum value for setpoint, taken from map function #define ADC_MOTOR_TOTAL_CURRENT_ZERO_AMPS 81 // 1.59V; 325 (10bits) = 81 (8bits) #define ADC_MOTOR_TOTAL_CURRENT_MAX 20 // 20 (8bits) ~ 2 Amps #define ADC_MOTOR_TOTAL_CURRENT_MAX_POSITIVE 90 // +2A #define ADC_MOTOR_TOTAL_CURRENT_MAX_NEGATIVE 70 // +2A +#define MORSE_TOLERANCE 40 // Tolerance for "morse" code detection -// Phase current: max of +-15.5 amps -// 512 --> 15.5 amps -// 1 ADC increment --> 0.030A -// 1 ADC RMS --> 0.03 * 0.707 -- > 0.021A -#define ADC_PHASE_B_CURRENT_STEP 21 // 0.021 * 1000 = 21 - -#if MOTOR_TYPE == MOTOR_TYPE_Q85 -//#define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT 77// best value found (at max speed, minimum current and power supply voltage keeps the same) -#define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT 4// value for ui8_position_correction_value = 0 initially @ shenyi middrive motor -#elif MOTOR_TYPE == MOTOR_TYPE_EUC2 -#define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT 35// best value found -#endif -#define PWM_CYCLES_COUNTER_MAX 625 // bei h�heren Werten wird angenommen, der Motor steht. -#define PWM_CYCLES_SECOND 15625L // 1 / 64us(PWM period) +#define PWM_CYCLES_COUNTER_MAX 3000 // higher values assume motor is at standstill. +#define PWM_CPS_NORMAL_SPEED 15625L +#define PWM_CPS_HIGH_SPEED 20833L // 2 seconds to get up to max PWM duty cycle value of 255 (127 * 255 * 64us ~= 2 seconds) #define PWM_DUTY_CYCLE_CONTROLLER_COUNTER 127 -#define SPEED_INVERSE_INTERPOLATION 16000 // experimental value; min speed aftwer which interpolation starts +#define SPEED_INVERSE_INTERPOLATION 350 // experimental value; min speed aftwer which interpolation starts #define PWM_VALUE_DUTY_CYCLE_MAX (256 - 1) #define MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX (PWM_VALUE_DUTY_CYCLE_MAX/2) -#define ANGLE_1 0 //(360/256) / 300 -#define ANGLE_60 42 -#define ANGLE_120 85 -#define ANGLE_180 127 -#define ANGLE_240 170 -#define ANGLE_300 212 -#define ANGLE_360 255 extern uint16_t ui16_log1; extern uint16_t ui16_log2; extern uint8_t ui8_log; -extern uint8_t ui8_adc_read_throttle_busy; -extern uint16_t ui16_SPEED_Counter; //Counter for bike speed -extern uint16_t ui16_PAS_Counter; //Counter for cadence -extern uint8_t ui8_PAS_Flag; //Flag for PAS Interrupt detected -extern uint8_t ui8_SPEED_Flag; //Flag for PAS Interrupt detected +extern uint8_t ui8_slowloop_flag; //Flag for slow loop timing +uint8_t update_advance_angle(); #endif diff --git a/motor.c b/motor.c index 5495e009..b69df4aa 100755 --- a/motor.c +++ b/motor.c @@ -1,192 +1,307 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#include -#include -#include "stm8s_gpio.h" -#include "stm8s_tim1.h" -#include "motor.h" -#include "gpio.h" -#include "motor.h" -#include "pwm.h" -#include "config.h" - -uint16_t ui16_PWM_cycles_counter = 0; -uint16_t ui16_motor_speed_erps = 0; -uint16_t ui16_speed_inverse = 0; -uint8_t ui8_motor_rotor_position = 0; // in 360/256 degrees -uint8_t ui8_motor_rotor_absolute_position = 0; // in 360/256 degrees -uint8_t ui8_position_correction_value = 127; // in 360/256 degrees -uint16_t ui16_PWM_cycles_counter_total = 0; -uint16_t ui16_PWM_cycles_counter_total_div_4 = 0; -uint8_t ui8_interpolation_angle = 0; - -uint16_t ui16_adc_current_phase_B = 0; -uint16_t ui16_adc_current_phase_B_accumulated = 0; -uint16_t ui16_adc_current_phase_B_filtered = 0; - -uint8_t ui8_motor_state = MOTOR_STATE_COAST; - -int8_t hall_sensors; -int8_t hall_sensors_last = 0; - -void TIM1_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM1_UPD_OVF_TRG_BRK_IRQHANDLER) -{ - hall_sensors_read_and_action (); - - /**************************************************************** - * Motor control: angle interpolation and PWM control - */ - motor_fast_loop (); - /****************************************************************/ - - // clear the interrupt pending bit for TIM1 - TIM1_ClearITPendingBit(TIM1_IT_UPDATE); -} - -void hall_sensor_init (void) -{ - GPIO_Init(HALL_SENSORS__PORT, - (GPIO_Pin_TypeDef)(HALL_SENSOR_A__PIN | HALL_SENSOR_B__PIN | HALL_SENSOR_C__PIN), - GPIO_MODE_IN_FL_NO_IT); -} - -void hall_sensors_read_and_action (void) -{ - // read hall sensors signal pins and mask other pins - hall_sensors = (GPIO_ReadInputData (HALL_SENSORS__PORT) & (HALL_SENSORS_MASK)); - if ((hall_sensors != hall_sensors_last) || - (ui8_motor_state == MOTOR_STATE_COAST)) // let's run the code when motor is stopped/coast so it can pick right motor position for correct startup - { - hall_sensors_last = hall_sensors; - - switch (hall_sensors) - { - case 3: - if (ui8_motor_state != MOTOR_STATE_RUNNING) - { - ui8_motor_rotor_absolute_position = (uint8_t) (ANGLE_120 + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - } - break; - - case 1: - if (ui8_motor_state != MOTOR_STATE_RUNNING) - { - ui8_motor_rotor_absolute_position = (uint8_t) (ANGLE_180 + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - } - break; - - case 5: - if (ui8_motor_state != MOTOR_STATE_RUNNING) - { - ui8_motor_rotor_absolute_position = (uint8_t) (ANGLE_240 + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - } - break; - - // start of phase B current sinusoid - case 4: - ui16_PWM_cycles_counter_total = ui16_PWM_cycles_counter; - ui16_PWM_cycles_counter_total_div_4 = ui16_PWM_cycles_counter_total >> 2; - ui16_PWM_cycles_counter = 0; - ui8_interpolation_angle = 0; - ui16_motor_speed_erps = PWM_CYCLES_SECOND / ui16_PWM_cycles_counter_total; // this division takes ~4.2us - -// if (motor_state != MOTOR_STATE_COAST) -// { -// ui16_speed_inverse = ui16_PWM_cycles_counter_total; -// } - - // update motor state based on motor speed - if (ui16_PWM_cycles_counter_total > SPEED_INVERSE_INTERPOLATION) - { - ui8_motor_state = MOTOR_STATE_RUNNING_VERY_SLOW; - } - else - { - ui8_motor_state = MOTOR_STATE_RUNNING; - } - - ui8_motor_rotor_absolute_position = ANGLE_300; - ui8_motor_rotor_absolute_position = (uint8_t) (ui8_motor_rotor_absolute_position + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - break; - - case 6: - if (ui8_motor_state != MOTOR_STATE_RUNNING) - { - ui8_motor_rotor_absolute_position = ANGLE_1; - ui8_motor_rotor_absolute_position = (uint8_t) (ui8_motor_rotor_absolute_position + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - } - break; - - case 2: - if (ui8_motor_state != MOTOR_STATE_RUNNING) - { - ui8_motor_rotor_absolute_position = ANGLE_60; - ui8_motor_rotor_absolute_position = (uint8_t) (ui8_motor_rotor_absolute_position + MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT); - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value); - } - break; - - default: - return; - break; - } - } -} - -// runs every 64us (PWM frequency) -void motor_fast_loop (void) -{ - if(ui16_SPEED_Counter < 65530) {ui16_SPEED_Counter++;} //increase SPEED Counter but avoid overflow - if(ui16_PAS_Counter < 65530) {ui16_PAS_Counter++;} //increase PAS Counter but avoid overflow - - // count number of fast loops / PWM cycles - if (ui16_PWM_cycles_counter < PWM_CYCLES_COUNTER_MAX) - { - ui16_PWM_cycles_counter++; - } - else - { - ui16_PWM_cycles_counter = 0xffff; - ui16_PWM_cycles_counter_total = 0xffff; //(SVM_TABLE_LEN_x1024) / PWM_CYCLES_COUNTER_MAX; - ui16_speed_inverse = 0xffff; - - // next code is need for motor startup correctly - ui8_interpolation_angle = 0; - ui8_motor_state = MOTOR_STATE_COAST; - hall_sensors_read_and_action (); - } - -#define DO_INTERPOLATION 1 // may be usefull when debugging -#if DO_INTERPOLATION == 1 - - if (ui8_motor_state == MOTOR_STATE_RUNNING) - { - // calculate the interpolation angle, to avoid overflow use 127 steps at low speed - if(ui16_PWM_cycles_counter_total>255){ - ui8_interpolation_angle = (uint8_t) ((ui16_PWM_cycles_counter << 7) / (ui16_PWM_cycles_counter_total>>1));} - else { - ui8_interpolation_angle = (uint8_t) ((ui16_PWM_cycles_counter << 8) / (ui16_PWM_cycles_counter_total));} - - ui8_motor_rotor_position = (uint8_t) (ui8_motor_rotor_absolute_position + ui8_position_correction_value - ui8_interpolation_angle); - } - else - { - // reset phase B current value as at very low speeds it has no meaning - ui16_adc_current_phase_B_accumulated = 0; - ui16_adc_current_phase_B_filtered = 0; - } -#endif - - pwm_duty_cycle_controller (); -} +/* + * EGG OpenSource EBike firmware + * + * Copyright (C) Casainho,Björn Schmidt 2015, 2106, 2017, 2019 + * + * Released under the GPL License, Version 3 + */ + +#include +#include +#include "stm8s_iwdg.h" +#include "stm8s_gpio.h" +#include "stm8s_tim1.h" +#include "motor.h" +#include "gpio.h" +#include "pwm.h" +#include "config.h" +#include "adc.h" +#include "ACAcontrollerState.h" +#include "ACAcommons.h" + +uint8_t ui8_counter = 0; +uint8_t ui8_half_rotation_flag = 0; +uint8_t ui8_foc_enable_flag = 0; + +uint16_t ui16_PWM_cycles_counter = 0; +uint16_t ui16_PWM_cycles_counter_6 = 0; +uint16_t ui16_PWM_cycles_counter_total = 0; + +uint8_t ui8_assumed_motor_position = 0; +uint8_t ui8_sinetable_position = 0; // in 360/256 degrees +uint8_t ui8_motor_rotor_hall_position = 0; // in 360/256 degrees +uint8_t ui8_sinetable_precalc = 0; +uint8_t ui8_interpolation_start_position = 0; + +uint8_t ui8_interpolation_angle = 0; + +uint16_t ui16_adc_current_phase_B = 0; +uint16_t ui16_adc_current_phase_B_accumulated = 0; +uint16_t ui16_adc_current_phase_B_filtered = 0; + +int8_t hall_sensors; +int8_t hall_sensors_last = 0; + +uint16_t ui16_ADC_iq_current_accumulated = 4096; +uint16_t ui16_iq_current_ma = 0; + +void TIM1_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM1_UPD_OVF_TRG_BRK_IRQHANDLER) { + adc_trigger(); + hall_sensors_read_and_action(); + + motor_fast_loop(); + + // clear the interrupt pending bit for TIM1 + TIM1_ClearITPendingBit(TIM1_IT_UPDATE); +} + +void hall_sensor_init(void) { + + GPIO_Init(HALL_SENSORS__PORT, + (GPIO_Pin_TypeDef) (HALL_SENSOR_A__PIN | HALL_SENSOR_B__PIN | HALL_SENSOR_C__PIN), + GPIO_MODE_IN_FL_NO_IT); +} + +void hall_sensors_read_and_action(void) { + // read hall sensors signal pins and mask other pins + hall_sensors = (GPIO_ReadInputData(HALL_SENSORS__PORT) & (HALL_SENSORS_MASK)); + if ((hall_sensors != hall_sensors_last) || (ui8_possible_motor_state == MOTOR_STATE_COAST)) // let's run the code when motor is stopped/coast so it can pick right motor position for correct startup + { + if (hall_sensors_last >0 && hall_sensors_last < 7) { + uint8_t_60deg_pwm_cycles[hall_sensors_last-1] = ui16_PWM_cycles_counter_6; + } + updateHallOrder(hall_sensors); + + //printf("hall change! %d, %d \n", hall_sensors, hall_sensors_last ); + hall_sensors_last = hall_sensors; + + if (ui8_possible_motor_state == MOTOR_STATE_COAST) { + ui8_possible_motor_state = MOTOR_STATE_RUNNING_NO_INTERPOLATION; + } + + + switch (hall_sensors) { + case 3://rotor position 180 degree + // full electric revolution recognized, update counters + uint8_t_hall_case[3] = ui8_adc_read_phase_B_current(); + + + if (ui8_half_rotation_flag) { + ui8_half_rotation_flag = 0; + if (ui16_PWM_cycles_counter > 20) ui16_PWM_cycles_counter_total = ui16_PWM_cycles_counter; + + ui16_PWM_cycles_counter = 0; + ui16_motor_speed_erps = ((uint16_t) ui16_pwm_cycles_second) / ui16_PWM_cycles_counter_total; // this division takes ~4.2us + + } + + if (ui16_motor_speed_erps == -1) { + ui16_motor_speed_erps = 0; + } + // update motor state based on motor speed + if (ui16_motor_speed_erps > 75) { + ui8_possible_motor_state = MOTOR_STATE_RUNNING_INTERPOLATION_360; + }else if (ui16_motor_speed_erps > 3) { + ui8_possible_motor_state = MOTOR_STATE_RUNNING_INTERPOLATION_60; + } else { + ui8_possible_motor_state = MOTOR_STATE_RUNNING_NO_INTERPOLATION; + } + + ui8_motor_rotor_hall_position = ui8_s_hall_angle3_180; + break; + + case 1: + uint8_t_hall_case[4] = ui8_adc_read_phase_B_current(); + ui8_motor_rotor_hall_position = ui8_s_hall_angle1_240; + break; + + case 5: //rotor position 300 degree + + uint8_t_hall_case[5] = ui8_adc_read_phase_B_current(); + ui8_motor_rotor_hall_position = ui8_s_hall_angle5_300; + break; + + case 4: //rotor position 0 degree + ui8_half_rotation_flag = 1; + ui8_foc_enable_flag = 1; + + uint8_t_hall_case[0] = ui8_adc_read_phase_B_current(); + //debug_pin_reset(); + ui8_motor_rotor_hall_position = ui8_s_hall_angle4_0; + break; + + case 6://rotor position 60 degree + uint8_t_hall_case[1] = ui8_adc_read_phase_B_current(); + ui8_motor_rotor_hall_position = ui8_s_hall_angle6_60; + break; + + case 2://rotor position 120 degree + uint8_t_hall_case[2] = ui8_adc_read_phase_B_current(); + ui8_motor_rotor_hall_position = ui8_s_hall_angle2_120; + break; + + default: + return; + break; + } + + ui16_PWM_cycles_counter_6 = 0; + } +} + +void updateCorrection() { + + if (ui8_duty_cycle_target > 5) { + ui16_ADC_iq_current_accumulated -= ui16_ADC_iq_current_accumulated >> 3; + ui16_ADC_iq_current_accumulated += ui16_adc_read_phase_B_current(); + ui16_ADC_iq_current = ui16_ADC_iq_current_accumulated >> 3; // this value is regualted to be zero by FOC + } + + if ((ui16_aca_flags & ANGLE_CORRECTION_ENABLED) != ANGLE_CORRECTION_ENABLED) { + ui8_position_correction_value = 127; //set advance angle to neutral value + return; + } + + if (ui16_motor_speed_erps > 3 && ui16_BatteryCurrent > ui16_current_cal_b + 3) { //normal riding, + if (ui16_ADC_iq_current >> 2 > 128 && ui8_position_correction_value < 143) { + ui8_position_correction_value++; + } else if (ui16_ADC_iq_current >> 2 < 126 && ui8_position_correction_value > 111) { + ui8_position_correction_value--; + } + } else if (ui16_motor_speed_erps > 3 && ui16_BatteryCurrent < ui16_current_cal_b - 3) {//regen + ui8_position_correction_value = 127; //set advance angle to neutral value + } else if (ui16_motor_speed_erps < 3) { + ui8_position_correction_value = 127; //reset advance angle at very low speed) + } + +} + +// runs every 64us (PWM frequency) + +void motor_fast_loop(void) { + if (ui16_time_ticks_for_uart_timeout < 65530) { + ui16_time_ticks_for_uart_timeout++; + } + if (ui16_time_ticks_for_speed_calculation < 65530) { + ui16_time_ticks_for_speed_calculation++; + } //increase SPEED Counter but avoid overflow + if (ui16_time_ticks_for_pas_calculation < 65530) { + ui16_time_ticks_for_pas_calculation++; + } //increase PAS Counter but avoid overflow + if (GPIO_ReadInputPin(PAS__PORT, PAS__PIN) && ui16_PAS_High_Counter < 65530) { + ui16_PAS_High_Counter++; + } + + + // count number of fast loops / PWM cycles + if (ui16_PWM_cycles_counter >= PWM_CYCLES_COUNTER_MAX) { + //ui16_PWM_cycles_counter = 0; + //ui16_PWM_cycles_counter_6 = 0; + ui16_PWM_cycles_counter_total = 0xffff; //(SVM_TABLE_LEN_x1024) / PWM_CYCLES_COUNTER_MAX; + ui8_position_correction_value = 127; + hall_sensors_last = 0; + ui16_motor_speed_erps = 0; + + + // next code is need for motor startup correctly + ui8_possible_motor_state = MOTOR_STATE_COAST; + hall_sensors_read_and_action(); + } + + + // // calculate the interpolation angle + // // interpolation seems a problem when motor starts, so avoid to do it at very low speed + if (((ui8_possible_motor_state == MOTOR_STATE_RUNNING_INTERPOLATION_60)||(ui8_possible_motor_state == MOTOR_STATE_RUNNING_INTERPOLATION_360)) && ((ui16_aca_experimental_flags & DISABLE_INTERPOLATION) != DISABLE_INTERPOLATION)) { + + if ( + ((ui16_aca_experimental_flags & DISABLE_60_DEG_INTERPOLATION) == DISABLE_60_DEG_INTERPOLATION)|| + (((ui16_aca_experimental_flags & SWITCH_360_DEG_INTERPOLATION) == SWITCH_360_DEG_INTERPOLATION) && (ui8_possible_motor_state == MOTOR_STATE_RUNNING_INTERPOLATION_360)) + ){ + + if (ui16_PWM_cycles_counter>255){ + ui8_interpolation_angle = (ui16_PWM_cycles_counter <<5) / (ui16_PWM_cycles_counter_total>>3); + }else{ + ui8_interpolation_angle = (ui16_PWM_cycles_counter <<8) / (ui16_PWM_cycles_counter_total); + } + ui8_interpolation_start_position = ui8_s_hall_angle3_180; // that's where ui16_PWM_cycles_counter is being reset + ui8_dynamic_motor_state = MOTOR_STATE_RUNNING_INTERPOLATION_360; + }else{ + + if (ui16_PWM_cycles_counter_6>255){ + ui8_interpolation_angle = (ui16_PWM_cycles_counter_6 <<5) / (ui16_PWM_cycles_counter_total>>3); + }else{ + ui8_interpolation_angle = (ui16_PWM_cycles_counter_6 << 8) / ui16_PWM_cycles_counter_total; + } + ui8_interpolation_start_position = ui8_motor_rotor_hall_position; + ui8_dynamic_motor_state = MOTOR_STATE_RUNNING_INTERPOLATION_60; + } + + ui16_PWM_cycles_counter_6++; + }else {// MOTOR_STATE_COAST || MOTOR_STATE_RUNNING_NO_INTERPOLATION + + ui8_interpolation_angle = 0; + + ui8_interpolation_start_position = ui8_motor_rotor_hall_position; + ui8_dynamic_motor_state = MOTOR_STATE_RUNNING_NO_INTERPOLATION; + + } + ui16_PWM_cycles_counter++; + + ui8_sinetable_precalc = ui8_interpolation_start_position + ui8_s_motor_angle + ui8_position_correction_value -127 + ui8_interpolation_angle; + if ((ui16_aca_experimental_flags & AVOID_MOTOR_CYCLES_JITTER) != AVOID_MOTOR_CYCLES_JITTER){ + ui8_sinetable_position = ui8_sinetable_precalc; + }else{ + // if we would go back slightly, stay at current position; if we would go back a lot, still apply (startup/coast/corner cases) + if (((uint8_t)(ui8_sinetable_precalc-ui8_sinetable_position))<248){ + ui8_sinetable_position = ui8_sinetable_precalc; + + }else{ + // debug anti jitter occurrences and angles + ui8_variableDebugB += 1; + ui8_variableDebugC = ui8_interpolation_angle; + ui8_variableDebugA = ui8_motor_rotor_hall_position; + } + } + + //ui8_assumed_motor_position = ui8_interpolation_start_position + ui8_interpolation_angle + ui8_s_motor_angle + ui8_position_correction_value - 127; + ui8_assumed_motor_position = ui8_interpolation_start_position + ui8_interpolation_angle + ui8_s_motor_angle; + + + // check if FOC control is needed + if ((ui8_foc_enable_flag) && ((ui8_assumed_motor_position) >= (ui8_correction_at_angle)) && ((ui8_assumed_motor_position) < (ui8_correction_at_angle + 4))) { + // make sure we just execute one time per ERPS, so reset the flag + ui8_foc_enable_flag = 0; + + //ui8_variableDebugA = ui8_assumed_motor_position; + //ui8_variableDebugB = ui8_assumed_motor_position + ui8_position_correction_value - 127; + + updateCorrection(); + } + + + //reset watchdog + IWDG->KR = IWDG_KEY_REFRESH; + pwm_duty_cycle_controller(); +} + +void watchdog_init(void) { + IWDG_Enable(); + IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); + IWDG_SetPrescaler(IWDG_Prescaler_4); + + // Timeout period + // The timeout period can be configured through the IWDG_PR and IWDG_RLR registers. It + // is determined by the following equation: + // T = 2 * T LSI * P * R + // where: + // T = Timeout period + // T LSI = 1/f LSI + // P = 2 (PR[2:0] + 2) + // R = RLR[7:0]+1 + // + // 0.0001 = 2 * (1 / 128000) * 4 * R + // R = 1.6 ; rounding to R = 2 + // R = 2 means a value of reload register = 1 + IWDG_SetReload(2); // 187.5us; for some reason, a value of 1 don't work, only 2 + IWDG_ReloadCounter(); +} diff --git a/motor.h b/motor.h index 4dd0e0aa..8528266e 100755 --- a/motor.h +++ b/motor.h @@ -1,30 +1,33 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#ifndef _MOTOR_H -#define _MOTOR_H - -#include "main.h" -#include "interrupts.h" - -// motor states -#define MOTOR_STATE_COAST 0 -#define MOTOR_STATE_RUNNING_VERY_SLOW 1 -#define MOTOR_STATE_RUNNING 2 - -extern uint8_t ui8_motor_rotor_position; -extern uint8_t ui8_position_correction_value; -extern uint16_t ui16_speed_inverse; -extern uint8_t ui8_motor_state; -extern uint16_t ui16_PWM_cycles_counter_total; - -void hall_sensor_init (void); -void hall_sensors_read_and_action (void); -void motor_fast_loop (void); - -#endif /* _MOTOR_H_ */ +/* + * EGG OpenSource EBike firmware + * + * Copyright (C) Casainho, 2015, 2106, 2017. + * + * Released under the GPL License, Version 3 + */ + +#ifndef _MOTOR_H +#define _MOTOR_H + +#include "main.h" +#include "interrupts.h" + +// motor states +#define MOTOR_STATE_COAST 0 +#define MOTOR_STATE_RUNNING_NO_INTERPOLATION 1 +#define MOTOR_STATE_RUNNING_INTERPOLATION_60 2 +#define MOTOR_STATE_RUNNING_INTERPOLATION_360 3 + +extern uint8_t ui8_sinetable_position; +extern uint16_t ui16_speed_inverse; +extern uint16_t ui16_PWM_cycles_counter_total; +extern uint16_t ui16_iq_current_ma; +extern uint16_t ui16_log; + + +void hall_sensor_init (void); +void hall_sensors_read_and_action (void); +void motor_fast_loop (void); +void watchdog_init (void); + +#endif /* _MOTOR_H_ */ diff --git a/nbproject/configurations.xml b/nbproject/configurations.xml new file mode 100644 index 00000000..8656b9da --- /dev/null +++ b/nbproject/configurations.xml @@ -0,0 +1,423 @@ + + + + + + + stm8s.h + stm8s_adc1.h + stm8s_adc2.h + stm8s_awu.h + stm8s_beep.h + stm8s_can.h + stm8s_clk.h + stm8s_conf.h + stm8s_exti.h + stm8s_flash.h + stm8s_gpio.h + stm8s_i2c.h + stm8s_it.h + stm8s_itc.h + stm8s_iwdg.h + stm8s_rst.h + stm8s_spi.h + stm8s_tim1.h + stm8s_tim2.h + stm8s_tim3.h + stm8s_tim4.h + stm8s_tim5.h + stm8s_tim6.h + stm8s_uart1.h + stm8s_uart2.h + stm8s_uart3.h + stm8s_uart4.h + stm8s_wwdg.h + + + stm8s_adc1.c + stm8s_adc2.c + stm8s_awu.c + stm8s_beep.c + stm8s_can.c + stm8s_clk.c + stm8s_exti.c + stm8s_flash.c + stm8s_gpio.c + stm8s_i2c.c + stm8s_it.c + stm8s_itc.c + stm8s_iwdg.c + stm8s_rst.c + stm8s_spi.c + stm8s_tim1.c + stm8s_tim2.c + stm8s_tim3.c + stm8s_tim4.c + stm8s_tim5.c + stm8s_tim6.c + stm8s_uart1.c + stm8s_uart2.c + stm8s_uart3.c + stm8s_uart4.c + stm8s_wwdg.c + + + + + midpoint_clamp_192_gen.c + midpoint_clamp_192_svm_orig.c + midpoint_clamp_255_gen.c + midpoint_clamp_255_svm_orig.c + nip_tuck_192_gen.c + nip_tuck_255_gen.c + pure_sine_192_gen.c + pure_sine_255_gen.c + third_harmonic_192_gen.c + third_harmonic_255_gen.c + + + + midpoint_clamp_192_gen.c + midpoint_clamp_255_gen.c + nip_tuck_192_gen.c + nip_tuck_255_gen.c + pure_sine_192_gen.c + pure_sine_255_gen.c + six_step_255.c + third_harmonic_192_gen.c + third_harmonic_255_gen.c + trapezodial_255.c + + ACAcommons.c + ACAcommons.h + ACAcontrollerState.c + ACAcontrollerState.h + ACAsetPoint.c + BOdisplay.c + BOdisplay.h + PAS.c + PAS.h + SPEED.c + SPEED.h + adc.c + adc.h + brake.c + brake.h + config.h + cruise_control.c + cruise_control.h + display.c + display.h + display_kingmeter.c + display_kingmeter.h + gpio.c + gpio.h + interrupts.h + main.c + main.h + motor.c + motor.h + pwm.c + pwm.h + timers.c + timers.h + uart.c + uart.h + + + StdPeriphLib/README.md + + + nbproject/private/launcher.properties + + + ^(nbproject)$ + + . + + + + + + default + false + false + + + + + + . + echo build + echo clean + + + + . + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nbproject/project.properties b/nbproject/project.properties new file mode 100644 index 00000000..f6d23654 --- /dev/null +++ b/nbproject/project.properties @@ -0,0 +1 @@ +#Mon Aug 27 20:14:16 CEST 2018 diff --git a/nbproject/project.xml b/nbproject/project.xml new file mode 100644 index 00000000..eba7bd2d --- /dev/null +++ b/nbproject/project.xml @@ -0,0 +1,26 @@ + + + org.netbeans.modules.cnd.makeproject + + + BMSBattery_S_controllers_firmware + c + + h + UTF-8 + + + . + + + + Default + 0 + + + + false + + + + diff --git a/proven settings/#outdated-Cute_Q100C.ini b/proven settings/#outdated-Cute_Q100C.ini new file mode 100644 index 00000000..9fd8d6a7 --- /dev/null +++ b/proven settings/#outdated-Cute_Q100C.ini @@ -0,0 +1,44 @@ +6 +25 +2000 +2160 +64 +48 +188 +140 +400 +412 +0 +200 +10 +1.6 +-110.4 +20 +40 +60 +80 +100 +50 +50 +50 +600 +0.5 +0.1 +113 +12 +1.9 +false +false +false +true +false +true +true +false +true +false +false +false +false +true +false diff --git a/proven settings/36V-28in-BionX_IGH3-BluOsec.ini b/proven settings/36V-28in-BionX_IGH3-BluOsec.ini new file mode 100644 index 00000000..68f6a35f --- /dev/null +++ b/proven settings/36V-28in-BionX_IGH3-BluOsec.ini @@ -0,0 +1,53 @@ +12 +25 +3125 +2200 +6 +43 +182 +111 +100 +150 +50 +26 +100 +1.6 +-110.4 +20 +40 +60 +80 +100 +50 +50 +50 +1500 +0.5 +0.2 +11 +10 +1.7 +false +64000 +25 +127 +false +true +false +false +true +false +false +1 +43 +86 +128 +true +false +171 +213 +0.0 +4760 +70 +128 +150 diff --git a/proven settings/48V-28in-DD-12FET-BluOsec.ini b/proven settings/48V-28in-DD-12FET-BluOsec.ini new file mode 100644 index 00000000..76b91bb1 --- /dev/null +++ b/proven settings/48V-28in-DD-12FET-BluOsec.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +100 +300 +50 +238 +46 +1.6 +-110.4 +12 +21 +30 +59 +100 +50 +50 +50 +1500 +0.5 +0.2 +24 +13 +1.9 +false +64000 +35 +127 +false +true +false +false + + +false +1 +43 +86 +128 +true +false +171 +213 +0.0 +4764 +67 +128 +208 diff --git a/proven settings/48V-28in-Q128C-12FET-BluOsecX.ini b/proven settings/48V-28in-Q128C-12FET-BluOsecX.ini new file mode 100644 index 00000000..703f86f8 --- /dev/null +++ b/proven settings/48V-28in-Q128C-12FET-BluOsecX.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +64 +350 +50 +246 +46 + + +26 +38 +55 +80 +100 +50 +50 +50 +1500 +0.5 +0.2 +69 + +1.9 +false +64000 +35 +127 + + +false +false + + +false +1 +43 +86 +128 +true +false +171 +213 +0.0 +4764 +67 +128 +208 diff --git a/proven settings/48V-28in-XF15-12FET-BluOsecX.ini b/proven settings/48V-28in-XF15-12FET-BluOsecX.ini new file mode 100644 index 00000000..37bfd9c4 --- /dev/null +++ b/proven settings/48V-28in-XF15-12FET-BluOsecX.ini @@ -0,0 +1,53 @@ +12 +28 +3125 +2230 +6 +43 +182 +148 +66 +350 +50 +238 +46 +1.6 +-110.4 +22 +33 +50 +75 +100 +50 +50 +50 +1500 +0.5 +0.2 +51 +13 +1.9 +false +64000 +35 +127 +false +true +false +false + + +false +1 +43 +86 +128 +true +false +171 +213 +0.0 +4764 +67 +128 +208 diff --git a/proven settings/THR-PAS,KT36-48svpr,12s,26inch,Q128h.ini b/proven settings/THR-PAS,KT36-48svpr,12s,26inch,Q128h.ini new file mode 100644 index 00000000..1c8b5c0f --- /dev/null +++ b/proven settings/THR-PAS,KT36-48svpr,12s,26inch,Q128h.ini @@ -0,0 +1,53 @@ +10 +25 +3125 +2040 +6 +80 +185 +148 +150 +500 +0 +237 +76 +1.6 +-110.4 +30 +45 +60 +80 +100 +50 +50 +50 +1500 +0.5 +0.1 +106 +12 +1.9 +false +64000 +40 +127 +false +true +true +false +false +true +false +1 +43 +86 +128 +false +false +171 +213 +0.0 +4736 +69 +128 +208 diff --git a/proven settings/TQSR,KT36-48svpr,12s,26inch,Q128h.ini b/proven settings/TQSR,KT36-48svpr,12s,26inch,Q128h.ini new file mode 100644 index 00000000..97d19ce7 --- /dev/null +++ b/proven settings/TQSR,KT36-48svpr,12s,26inch,Q128h.ini @@ -0,0 +1,54 @@ +48 +25 +3125 +2040 +6 +72 +172 +148 +150 +500 +0 +237 +76 +1.6 +-110.4 +50 +62 +74 +87 +100 +50 +50 +50 +0 +0.5 +0.1 +106 +12 +1.9 +true +0 +40 +127 +false +true +true +false +false +true +false +1 +43 +86 +128 +false +false +171 +213 +1500.0 +6817 +69 +128 +208 + diff --git a/pwm.c b/pwm.c index 8ec52a50..84d0cd80 100755 --- a/pwm.c +++ b/pwm.c @@ -1,7 +1,7 @@ /* * EGG OpenSource EBike firmware * - * Copyright (C) Casainho, 2015, 2106, 2017. + * Copyright (C) Casainho,Björn Schmidt 2015, 2106, 2017, 2019 * * Released under the GPL License, Version 3 */ @@ -14,976 +14,232 @@ #include "gpio.h" #include "motor.h" #include "pwm.h" +#include "config.h" +#include "ACAcontrollerState.h" -#if (SVM_TABLE == SVM) -uint8_t ui8_svm_table [SVM_TABLE_LEN] = -{ - 127 , - 133 , - 138 , - 144 , - 149 , - 154 , - 160 , - 165 , - 170 , - 176 , - 181 , - 186 , - 191 , - 197 , - 202 , - 207 , - 212 , - 217 , - 222 , - 227 , - 231 , - 236 , - 239 , - 240 , - 242 , - 243 , - 244 , - 245 , - 247 , - 248 , - 249 , - 250 , - 250 , - 251 , - 252 , - 253 , - 253 , - 254 , - 254 , - 254 , - 255 , - 255 , - 255 , - 255 , - 255 , - 255 , - 254 , - 254 , - 254 , - 253 , - 253 , - 252 , - 251 , - 251 , - 250 , - 249 , - 248 , - 247 , - 246 , - 245 , - 243 , - 242 , - 241 , - 239 , - 238 , - 239 , - 241 , - 242 , - 243 , - 245 , - 246 , - 247 , - 248 , - 249 , - 250 , - 251 , - 251 , - 252 , - 253 , - 253 , - 254 , - 254 , - 254 , - 255 , - 255 , - 255 , - 255 , - 255 , - 255 , - 254 , - 254 , - 254 , - 253 , - 253 , - 252 , - 251 , - 250 , - 250 , - 249 , - 248 , - 247 , - 245 , - 244 , - 243 , - 242 , - 240 , - 239 , - 236 , - 231 , - 227 , - 222 , - 217 , - 212 , - 207 , - 202 , - 197 , - 191 , - 186 , - 181 , - 176 , - 170 , - 165 , - 160 , - 154 , - 149 , - 144 , - 138 , - 133 , - 127 , - 122 , - 116 , - 111 , - 106 , - 100 , - 95 , - 89 , - 84 , - 79 , - 74 , - 68 , - 63 , - 58 , - 53 , - 48 , - 43 , - 38 , - 33 , - 28 , - 23 , - 18 , - 16 , - 14 , - 13 , - 12 , - 10 , - 9 , - 8 , - 7 , - 6 , - 5 , - 4 , - 3 , - 3 , - 2 , - 1 , - 1 , - 1 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 1 , - 1 , - 2 , - 2 , - 3 , - 4 , - 5 , - 6 , - 6 , - 8 , - 9 , - 10 , - 11 , - 12 , - 14 , - 15 , - 17 , - 15 , - 14 , - 12 , - 11 , - 10 , - 9 , - 8 , - 6 , - 6 , - 5 , - 4 , - 3 , - 2 , - 2 , - 1 , - 1 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 1 , - 1 , - 1 , - 2 , - 3 , - 3 , - 4 , - 5 , - 6 , - 7 , - 8 , - 9 , - 10 , - 12 , - 13 , - 14 , - 16 , - 18 , - 23 , - 28 , - 33 , - 38 , - 43 , - 48 , - 53 , - 58 , - 63 , - 68 , - 74 , - 79 , - 84 , - 89 , - 95 , - 100 , - 106 , - 111 , - 116 , - 122 -}; -#elif (SVM_TABLE == SINE_SVM_ORIGINAL) -uint8_t ui8_svm_table [SVM_TABLE_LEN] = -{ - 127 , - 133 , - 138 , - 144 , - 149 , - 154 , - 160 , - 165 , - 170 , - 176 , - 181 , - 186 , - 191 , - 197 , - 202 , - 207 , - 212 , - 217 , - 222 , - 227 , - 231 , - 236 , - 239 , - 240 , - 242 , - 243 , - 244 , - 245 , - 247 , - 248 , - 249 , - 250 , - 250 , - 251 , - 252 , - 253 , - 253 , - 254 , - 254 , - 254 , - 255 , - 255 , - 255 , - 255 , - 255 , - 255 , - 254 , - 254 , - 254 , - 253 , - 253 , - 252 , - 251 , - 251 , - 250 , - 249 , - 248 , - 247 , - 246 , - 245 , - 243 , - 242 , - 241 , - 239 , - 238 , - 239 , - 241 , - 242 , - 243 , - 245 , - 246 , - 247 , - 248 , - 249 , - 250 , - 251 , - 251 , - 252 , - 253 , - 253 , - 254 , - 254 , - 254 , - 255 , - 255 , - 255 , - 255 , - 255 , - 255 , - 254 , - 254 , - 254 , - 253 , - 253 , - 252 , - 251 , - 250 , - 250 , - 249 , - 248 , - 247 , - 245 , - 244 , - 243 , - 242 , - 240 , - 239 , - 236 , - 231 , - 227 , - 222 , - 217 , - 212 , - 207 , - 202 , - 197 , - 191 , - 186 , - 181 , - 176 , - 170 , - 165 , - 160 , - 154 , - 149 , - 144 , - 138 , - 133 , - 127 , - 122 , - 116 , - 111 , - 106 , - 100 , - 95 , - 89 , - 84 , - 79 , - 74 , - 68 , - 63 , - 58 , - 53 , - 48 , - 43 , - 38 , - 33 , - 28 , - 23 , - 18 , - 16 , - 14 , - 13 , - 12 , - 10 , - 9 , - 8 , - 7 , - 6 , - 5 , - 4 , - 3 , - 3 , - 2 , - 1 , - 1 , - 1 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 1 , - 1 , - 2 , - 2 , - 3 , - 4 , - 5 , - 6 , - 6 , - 8 , - 9 , - 10 , - 11 , - 12 , - 14 , - 15 , - 17 , - 15 , - 14 , - 12 , - 11 , - 10 , - 9 , - 8 , - 6 , - 6 , - 5 , - 4 , - 3 , - 2 , - 2 , - 1 , - 1 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 1 , - 1 , - 1 , - 2 , - 3 , - 3 , - 4 , - 5 , - 6 , - 7 , - 8 , - 9 , - 10 , - 12 , - 13 , - 14 , - 16 , - 18 , - 23 , - 28 , - 33 , - 38 , - 43 , - 48 , - 53 , - 58 , - 63 , - 68 , - 74 , - 79 , - 84 , - 89 , - 95 , - 100 , - 106 , - 111 , - 116 , - 122 -}; -#elif (SVM_TABLE == SINE) -uint8_t ui8_svm_table [SVM_TABLE_LEN] = -{ - 5 , - 12 , - 18 , - 24 , - 30 , - 37 , - 43 , - 49 , - 55 , - 61 , - 67 , - 73 , - 79 , - 85 , - 91 , - 97 , - 103 , - 108 , - 114 , - 120 , - 125 , - 131 , - 136 , - 141 , - 146 , - 151 , - 156 , - 161 , - 166 , - 171 , - 176 , - 180 , - 184 , - 189 , - 193 , - 197 , - 201 , - 205 , - 208 , - 212 , - 215 , - 219 , - 222 , - 225 , - 228 , - 231 , - 233 , - 236 , - 238 , - 240 , - 243 , - 244 , - 246 , - 248 , - 249 , - 251 , - 252 , - 253 , - 254 , - 255 , - 255 , - 256 , - 256 , - 256 , - 256 , - 256 , - 255 , - 255 , - 254 , - 253 , - 253 , - 251 , - 250 , - 249 , - 247 , - 246 , - 244 , - 242 , - 240 , - 237 , - 235 , - 232 , - 230 , - 227 , - 224 , - 221 , - 217 , - 214 , - 211 , - 207 , - 203 , - 199 , - 195 , - 191 , - 187 , - 183 , - 178 , - 174 , - 169 , - 164 , - 159 , - 154 , - 149 , - 144 , - 139 , - 134 , - 128 , - 123 , - 117 , - 112 , - 106 , - 100 , - 94 , - 89 , - 83 , - 77 , - 71 , - 65 , - 59 , - 53 , - 46 , - 40 , - 34 , - 28 , - 22 , - 15 , - 9 , - 3 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 , - 0 -}; -#endif +#include "wavetables/midpoint_clamp_255_gen.c" +#include "wavetables/third_harmonic_255_gen.c" +#include "wavetables/pure_sine_255_gen.c" +#include "wavetables/nip_tuck_255_gen.c" +#include "wavetables/six_step_255.c" uint8_t ui8_duty_cycle = 0; uint8_t ui8_duty_cycle_target = 0; -uint8_t ui8_value_a; -uint8_t ui8_value_b; -uint8_t ui8_value_c; +uint8_t ui8_value_new_A; +uint8_t ui8_value_new_B; +uint8_t ui8_value_new_C; uint16_t ui16_value; -void pwm_set_duty_cycle (uint8_t value) -{ - ui8_duty_cycle_target = value; +void pwm_set_duty_cycle(uint8_t value) { + ui8_duty_cycle_target = value; } -void pwm_init (void) -{ -// TIM1 Peripheral Configuration - TIM1_DeInit(); +void pwm_init(void) { + // TIM1 Peripheral Configuration + TIM1_DeInit(); #if (SVM_TABLE == SVM) - TIM1_TimeBaseInit(0, // TIM1_Prescaler = 0 - TIM1_COUNTERMODE_CENTERALIGNED1, - (512 - 1), // clock = 16MHz; counter period = 1024; PWM freq = 16MHz / 1024 = 15.625kHz; - //(BUT PWM center aligned mode needs twice the frequency) - 1); // will fire the TIM1_IT_UPDATE at every PWM period cycle + TIM1_TimeBaseInit(0, // TIM1_Prescaler = 0 + TIM1_COUNTERMODE_CENTERALIGNED1, + (16000000 / ui16_pwm_cycles_second / 2 - 1), // clock = 16MHz; counter period = 1024; PWM freq = 16MHz / 1024 = 15.625kHz; + //(BUT PWM center aligned mode needs twice the frequency) + 1); // will fire the TIM1_IT_UPDATE at every PWM period cycle #elif (SVM_TABLE == SINE) || (SVM_TABLE == SINE_SVM_ORIGINAL) - TIM1_TimeBaseInit(0, // TIM1_Prescaler = 0 - TIM1_COUNTERMODE_UP, - (1024 - 1), // clock = 16MHz; counter period = 1024; PWM freq = 16MHz / 1024 = 15.625kHz; - 0); // will fire the TIM1_IT_UPDATE at every PWM period + TIM1_TimeBaseInit(0, // TIM1_Prescaler = 0 + TIM1_COUNTERMODE_UP, + (1024 - 1), // clock = 16MHz; counter period = 1024; PWM freq = 16MHz / 1024 = 15.625kHz; + 0); // will fire the TIM1_IT_UPDATE at every PWM period #endif -//#define DISABLE_PWM_CHANNELS_1_3 + //#define DISABLE_PWM_CHANNELS_1_3 - TIM1_OC1Init(TIM1_OCMODE_PWM1, + TIM1_OC1Init(TIM1_OCMODE_PWM1, #ifdef DISABLE_PWM_CHANNELS_1_3 - TIM1_OUTPUTSTATE_DISABLE, - TIM1_OUTPUTNSTATE_DISABLE, + TIM1_OUTPUTSTATE_DISABLE, + TIM1_OUTPUTNSTATE_DISABLE, #else - TIM1_OUTPUTSTATE_ENABLE, - TIM1_OUTPUTNSTATE_ENABLE, + TIM1_OUTPUTSTATE_ENABLE, + TIM1_OUTPUTNSTATE_ENABLE, #endif - 0, // initial duty_cycle value - TIM1_OCPOLARITY_HIGH, - TIM1_OCNPOLARITY_LOW, - TIM1_OCIDLESTATE_RESET, - TIM1_OCNIDLESTATE_SET); - - TIM1_OC2Init(TIM1_OCMODE_PWM1, - TIM1_OUTPUTSTATE_ENABLE, - TIM1_OUTPUTNSTATE_ENABLE, - 0, // initial duty_cycle value - TIM1_OCPOLARITY_HIGH, - TIM1_OCNPOLARITY_LOW, - TIM1_OCIDLESTATE_RESET, - TIM1_OCNIDLESTATE_SET); - - TIM1_OC3Init(TIM1_OCMODE_PWM1, + 0, // initial duty_cycle value + TIM1_OCPOLARITY_HIGH, + TIM1_OCNPOLARITY_LOW, + TIM1_OCIDLESTATE_RESET, + TIM1_OCNIDLESTATE_SET); + + TIM1_OC2Init(TIM1_OCMODE_PWM1, + TIM1_OUTPUTSTATE_ENABLE, + TIM1_OUTPUTNSTATE_ENABLE, + 0, // initial duty_cycle value + TIM1_OCPOLARITY_HIGH, + TIM1_OCNPOLARITY_LOW, + TIM1_OCIDLESTATE_RESET, + TIM1_OCNIDLESTATE_SET); + + TIM1_OC3Init(TIM1_OCMODE_PWM1, #ifdef DISABLE_PWM_CHANNELS_1_3 - TIM1_OUTPUTSTATE_DISABLE, - TIM1_OUTPUTNSTATE_DISABLE, + TIM1_OUTPUTSTATE_DISABLE, + TIM1_OUTPUTNSTATE_DISABLE, #else - TIM1_OUTPUTSTATE_ENABLE, - TIM1_OUTPUTNSTATE_ENABLE, + TIM1_OUTPUTSTATE_ENABLE, + TIM1_OUTPUTNSTATE_ENABLE, #endif - 0, // initial duty_cycle value - TIM1_OCPOLARITY_HIGH, - TIM1_OCNPOLARITY_LOW, - TIM1_OCIDLESTATE_RESET, - TIM1_OCNIDLESTATE_SET); - - TIM1_OC1PreloadConfig (ENABLE); - TIM1_OC2PreloadConfig (ENABLE); - TIM1_OC3PreloadConfig (ENABLE); - - // break, dead time and lock configuration - TIM1_BDTRConfig(TIM1_OSSISTATE_ENABLE, - TIM1_LOCKLEVEL_OFF, - // hardware nees a dead time of 1us - 16, // DTG = 0; dead time in 62.5 ns steps; 1us/62.5ns = 16 - TIM1_BREAK_DISABLE, - TIM1_BREAKPOLARITY_LOW, - TIM1_AUTOMATICOUTPUT_DISABLE); - - TIM1_ITConfig(TIM1_IT_UPDATE, ENABLE); - - TIM1_Cmd(ENABLE); // TIM1 counter enable - TIM1_CtrlPWMOutputs(ENABLE); // main Output Enable + 0, // initial duty_cycle value + TIM1_OCPOLARITY_HIGH, + TIM1_OCNPOLARITY_LOW, + TIM1_OCIDLESTATE_RESET, + TIM1_OCNIDLESTATE_SET); + + TIM1_OC1PreloadConfig(ENABLE); + TIM1_OC2PreloadConfig(ENABLE); + TIM1_OC3PreloadConfig(ENABLE); + + // break, dead time and lock configuration + TIM1_BDTRConfig(TIM1_OSSISTATE_ENABLE, + TIM1_LOCKLEVEL_OFF, + // hardware nees a dead time of 1us + 16, // DTG = 0; dead time in 62.5 ns steps; 1us/62.5ns = 16 + TIM1_BREAK_DISABLE, + TIM1_BREAKPOLARITY_LOW, + TIM1_AUTOMATICOUTPUT_DISABLE); + + TIM1_ITConfig(TIM1_IT_UPDATE, ENABLE); + + TIM1_Cmd(ENABLE); // TIM1 counter enable + TIM1_CtrlPWMOutputs(DISABLE); // main Output disable for start up } -void pwm_duty_cycle_controller (void) -{ - // limit PWM increase/decrease rate --- comment from stancecoke: this part does just nothing? ui8_counter is never increased?! - static uint8_t ui8_counter; - if (ui8_counter++ > PWM_DUTY_CYCLE_CONTROLLER_COUNTER) - { - ui8_counter = 0; - - // increment or decrement duty_cycle - if (ui8_duty_cycle_target > ui8_duty_cycle) { ui8_duty_cycle++; } - else if (ui8_duty_cycle_target < ui8_duty_cycle) { ui8_duty_cycle--; } - } - -//#define DO_DUTY_CYCLE_RAMP 1 +void pwm_duty_cycle_controller(void) { + //#define DO_DUTY_CYCLE_RAMP 1 #if DO_DUTY_CYCLE_RAMP == 1 - pwm_apply_duty_cycle (ui8_duty_cycle); + // limit PWM increase/decrease rate --- comment from stancecoke: this part does just nothing? ui8_counter is never increased?! + static uint8_t ui8_counter; + if (ui8_counter++ > PWM_DUTY_CYCLE_CONTROLLER_COUNTER) { + ui8_counter = 0; + + // increment or decrement duty_cycle + if (ui8_duty_cycle_target > ui8_duty_cycle) { + ui8_duty_cycle++; + } else if (ui8_duty_cycle_target < ui8_duty_cycle) { + ui8_duty_cycle--; + } + } + + pwm_apply_duty_cycle(ui8_duty_cycle); #else - pwm_apply_duty_cycle (ui8_duty_cycle_target); + pwm_apply_duty_cycle(ui8_duty_cycle_target); #endif } -void pwm_apply_duty_cycle (uint8_t ui8_duty_cycle_value) -{ +uint8_t fetch_table_value(uint8_t table_pos_in) { + + // we only store a quarter of the values and generate the other 4 quarter with a simple translation + uint8_t translated_table_pos = table_pos_in&~192; + uint8_t table_val; + + if (table_pos_in & 64) { + translated_table_pos = 64 - translated_table_pos; + } + + if (ui8_dynamic_motor_state == MOTOR_STATE_RUNNING_NO_INTERPOLATION){ + table_val = six_step[translated_table_pos]; + }else if ((ui16_aca_experimental_flags & (USE_ALTERNATE_WAVETABLE|USE_ALTERNATE_WAVETABLE_B)) == (0)){ + // default + table_val = midpoint_clamp_gen[translated_table_pos]; + }else if ((ui16_aca_experimental_flags & (USE_ALTERNATE_WAVETABLE|USE_ALTERNATE_WAVETABLE_B)) == (USE_ALTERNATE_WAVETABLE)){ + table_val = pure_sine_gen[translated_table_pos]; + }else if ((ui16_aca_experimental_flags & (USE_ALTERNATE_WAVETABLE|USE_ALTERNATE_WAVETABLE_B)) == (USE_ALTERNATE_WAVETABLE_B)){ + table_val = third_harmonic_gen[translated_table_pos]; + }else if ((ui16_aca_experimental_flags & (USE_ALTERNATE_WAVETABLE|USE_ALTERNATE_WAVETABLE_B)) == (USE_ALTERNATE_WAVETABLE|USE_ALTERNATE_WAVETABLE_B)){ + table_val = nip_tuck_gen[translated_table_pos]; + }else{ + // fallback + table_val = midpoint_clamp_gen[translated_table_pos]; + } + if (ui16_pwm_cycles_second == PWM_CPS_HIGH_SPEED){ + // high speed motor runs at 25% higher pwm frequency so the pwm counters max value is 25% lower + table_val = table_val-(table_val>>2); + } + if (table_pos_in & 128) { + if (ui16_pwm_cycles_second == PWM_CPS_NORMAL_SPEED){ + table_val = 255 - table_val; + }else{ + table_val = 192 - table_val; + } + } + return table_val; + +} + +void pwm_apply_duty_cycle(uint8_t ui8_duty_cycle_value) { #if (SVM_TABLE == SVM) - static uint8_t ui8__duty_cycle; - static uint8_t ui8_temp; - - ui8__duty_cycle = ui8_duty_cycle_value; - - // scale and apply _duty_cycle - ui8_temp = ui8_svm_table[ui8_motor_rotor_position]; - if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) - { - ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_a = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; - } - else - { - ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_a = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; - } - - // add 120 degrees and limit - ui8_temp = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 85 /* 120º */)]; - if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) - { - ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_b = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; - } - else - { - ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_b = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; - } - - // subtract 120 degrees and limit - ui8_temp = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 171 /* 240º */)]; - if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) - { - ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_c = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; - } - else - { - ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; - ui8_temp = (uint8_t) (ui16_value >> 8); - ui8_value_c = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; - } - - // set final duty_cycle value - TIM1_SetCompare1((uint16_t) (ui8_value_a << 1)); - TIM1_SetCompare2((uint16_t) (ui8_value_c << 1)); - TIM1_SetCompare3((uint16_t) (ui8_value_b << 1)); + static uint8_t ui8__duty_cycle; + static uint8_t ui8_temp; + + ui8__duty_cycle = ui8_duty_cycle_value; + + // scale and apply _duty_cycle + // add 120 degrees for phase C (new definition is assumed motor angle = table position = phase B = phase where current is measured) + ui8_temp = fetch_table_value((uint8_t) (ui8_sinetable_position + 85 /* 120° */)); + if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) { + ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_C = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; + } else { + ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_C = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; + } + + // add 240 degrees for phase A (new definition is assumed motor angle = table position = phase B = phase where current is measured) + ui8_temp = fetch_table_value((uint8_t) (ui8_sinetable_position + 171 /* 240° */)); + if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) { + ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_A = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; + } else { + ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_A = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; + } + + // new definition is assumed motor angle = table position = phase B = phase where current is measured + ui8_temp = fetch_table_value(ui8_sinetable_position); + if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX) { + ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_B = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp; + } else { + ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle; + ui8_temp = (uint8_t) (ui16_value >> 8); + ui8_value_new_B = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp; + } + + // set final duty_cycle value + TIM1_SetCompare1((uint16_t) (ui8_value_new_C << 1)); + TIM1_SetCompare2((uint16_t) (ui8_value_new_B << 1)); + TIM1_SetCompare3((uint16_t) (ui8_value_new_A << 1)); #elif (SVM_TABLE == SINE) || (SVM_TABLE == SINE_SVM_ORIGINAL) - // scale and apply _duty_cycle - ui8_value_a = ui8_svm_table[ui8_motor_rotor_position]; - ui16_value = (uint16_t) (ui8_value_a * ui8_duty_cycle_value); - ui8_value_a = (uint8_t) (ui16_value >> 8); - - // add 120 degrees and limit - ui8_value_b = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 85 /* 120º */)]; - ui16_value = (uint16_t) (ui8_value_b * ui8_duty_cycle_value); - ui8_value_b = (uint8_t) (ui16_value >> 8); - - // subtract 120 degrees and limit - ui8_value_c = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 171 /* 240º */)]; - ui16_value = (uint16_t) (ui8_value_c * ui8_duty_cycle_value); - ui8_value_c = (uint8_t) (ui16_value >> 8); - - // set final duty_cycle value - TIM1_SetCompare1((uint16_t) (ui8_value_a << 2)); - TIM1_SetCompare2((uint16_t) (ui8_value_c << 2)); - TIM1_SetCompare3((uint16_t) (ui8_value_b << 2)); + // scale and apply _duty_cycle + ui8_value_a = fetch_table_value(ui8_sinetable_position); + ui16_value = (uint16_t) (ui8_value_a * ui8_duty_cycle_value); + ui8_value_a = (uint8_t) (ui16_value >> 8); + + // add 120 degrees and limit + ui8_value_b = fetch_table_value((uint8_t) (ui8_sinetable_position + 85 /* 120º */)); + ui16_value = (uint16_t) (ui8_value_b * ui8_duty_cycle_value); + ui8_value_b = (uint8_t) (ui16_value >> 8); + + // subtract 120 degrees and limit + ui8_value_c = fetch_table_value((uint8_t) (ui8_sinetable_position + 171 /* 240º */); + ui16_value = (uint16_t) (ui8_value_c * ui8_duty_cycle_value); + ui8_value_c = (uint8_t) (ui16_value >> 8); + + // set final duty_cycle value + TIM1_SetCompare1((uint16_t) (ui8_value_a << 2)); + TIM1_SetCompare2((uint16_t) (ui8_value_c << 2)); + TIM1_SetCompare3((uint16_t) (ui8_value_b << 2)); #endif } diff --git a/pwm.h b/pwm.h index b341f51d..77d0d96d 100755 --- a/pwm.h +++ b/pwm.h @@ -18,7 +18,7 @@ #define SVM_TABLE SVM -extern uint8_t ui8_svm_table [SVM_TABLE_LEN]; +extern uint8_t ui8_duty_cycle_target; void pwm_init (void); void pwm_duty_cycle_controller (void); diff --git a/test/motorTest b/test/motorTest new file mode 100755 index 00000000..19e120dd Binary files /dev/null and b/test/motorTest differ diff --git a/test/motorTest.c b/test/motorTest.c new file mode 100644 index 00000000..ec38d94d --- /dev/null +++ b/test/motorTest.c @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2018 Björn Schmidt + * + * 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 . + * + * This file now has mainly become a wavetable creator / tester + * + */ + +#include +#include +#include + +#include "wavetables/midpoint_clamp_255_svm_orig.c" +#include "wavetables/midpoint_clamp_192_svm_orig.c" +#include "wavetables/third_harmonic_255_gen.c" +#include "wavetables/third_harmonic_192_gen.c" +#include "wavetables/pure_sine_255_gen.c" +#include "wavetables/pure_sine_192_gen.c" +#include "wavetables/midpoint_clamp_255_gen.c" +#include "wavetables/midpoint_clamp_192_gen.c" +#include "wavetables/nip_tuck_255_gen.c" +#include "wavetables/nip_tuck_192_gen.c" + +uint16_t ui16; +uint16_t ui16b; +uint8_t ui8Y; +uint8_t ui8b; +uint8_t ui8a; +uint8_t ui8X; + +uint8_t fetch_table_value(uint8_t table_pos_in, uint8_t* table, uint8_t max, uint8_t downscaleForHighSpeed) { + + // we only store a quarter of the values and generate the other 4 quarter with a simple lookop translation + uint8_t translated_table_pos = table_pos_in&~192; + uint8_t table_val; + + if (table_pos_in & 64) { + translated_table_pos = 64 - translated_table_pos; + } + + table_val = table[translated_table_pos]; + + if (downscaleForHighSpeed){ + table_val = table_val-(table_val>>2); + } + if (table_pos_in & 128) { + table_val = max - table_val; + } + + return table_val; + +} + +// this is top and bottom clamp which equals nip and tuck with the given hardcoded amplitude 1.154734* (100%plus 15.5%gain compared to sine) +// top and bottom clamp changes it's waveform with amplitude, so we can't use that with simplified foc +// but we can use nip&tuck because that doesn't change it's waveform with amplitude +uint8_t gen_natspwm(uint8_t table_pos_in, uint16_t range, uint8_t print){ + float x = 2*M_PI*table_pos_in/256.0; + float a= 1.154734*sin(x); + float b= 1.154734*sin(x+(2.0/3.0)*M_PI); + float c= 1.154734*sin(x+(4.0/3.0)*M_PI); + + float z = 1-(fmaxf(fmaxf(a,b),c)); + if ((a>0 && b>0)||(a>0 && c>0)||(b>0 && c>0)){ + z = -1-(fminf(fminf(a,b),c)); + } + + int disc = round(((range-0.01)/2.0) + (a) * (((range)/2.0))); + int discplus = round(((range-0.01)/2.0) + (a+z) * (((range)/2.0))); + + if (print) + printf("%3d %4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 6.4f %3d %3d\n",table_pos_in, x,a,b,c,z,a+z,disc,discplus); + + return discplus; +} + +uint8_t gen_mcspwm(uint8_t table_pos_in, uint16_t range, uint8_t print){ + float x = 2*M_PI*table_pos_in/256.0; + float a= sin(x); + float b= sin(x+(2.0/3.0)*M_PI); + float c= sin(x+(4.0/3.0)*M_PI); + float z = 1/2 - 0.5* (fminf(fminf(a,b),c)+fmaxf(fmaxf(a,b),c)); + + int disc = round(((range-0.01)/2.0) + (a) * (((range)/2.0))); + int discplus = round(((range-0.01)/2.0) + (a+z) *1.154734* (((range)/2.0))); + + if (print) + printf("%3d %4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 6.4f %3d %3d\n",table_pos_in, x,a,b,c,z,a+z,disc,discplus); + + return discplus; +} + +uint8_t gen_sspwm(uint8_t table_pos_in, uint16_t range, uint8_t print){ + float x = 2*M_PI*table_pos_in/256.0; + float s= sin(x); + + int disc = round(((range-0.01)/2.0) + s * (((range)/2.0))); + + if (print) + printf("%3d %.2f %.2f %d \n",table_pos_in, x,s,disc); + + return disc; +} + +uint8_t gen_thipwm(uint8_t table_pos_in, uint16_t range, uint8_t print){ + float x = 2*M_PI*table_pos_in/256.0; + float s; + + //s= 2.0/3.0*(sqrt(3.0)*sin(x)+1.0/3.0*sin(3*x)); + s= 2.0/3.0*(sqrt(3.0)*sin(x)+0.275*sin(3*x)); + + + //0.5-a*Math.cos(Math.PI*6*u)/6; + + int disc = round(((range-0.01)/2.0) + s * (((range)/2.0))); + + if (print) + printf("%3d %.2f %.2f %d \n",table_pos_in, x,s,disc); + + return disc; +} + +void main() { + + printf("\r\ncurve test\r\n"); + for (int i = 0; i < 256; i++) { + + uint8_t compare_base = midpoint_clamp_192_svm_orig[i]; + //uint8_t compare_base = midpoint_clamp_255_svm_orig[i]; + //uint8_t compare_base = third_harmonic_255_gen[i]; + //uint8_t compare_base = pure_sine_255_gen[i]; + //uint8_t compare_base = pure_sine_192_gen[i]; + //uint8_t compare_base = third_harmonic_192_gen[i]; + + //uint8_t compare_a = midpoint_clamp_255_gen[i]; + //uint8_t compare_a = midpoint_clamp_192_gen[i]; + + //uint8_t compare_base = pure_sine_255_gen[i]; + //uint8_t compare_base = pure_sine_192_gen[i]; + + //uint8_t compare_base = nip_tuck_255_gen[i]; + //uint8_t compare_base = nip_tuck_192_gen[i]; + + + //uint8_t compare_a = fetch_table_value(i,third_harmonic_255_gen,255,0); + //uint8_t compare_a = fetch_table_value(i,midpoint_clamp_255_svm_orig,255,0); + //uint8_t compare_a = fetch_table_value(i,pure_sine_255_gen,255,0); + //uint8_t compare_a = fetch_table_value(i,nip_tuck_255_gen,255,0); + + uint8_t compare_a = fetch_table_value(i,pure_sine_255_gen,192,1); + //uint8_t compare_a = fetch_table_value(i,third_harmonic_255_gen,192,1); + //uint8_t compare_a = fetch_table_value(i,midpoint_clamp_255_gen,192,1); + //uint8_t compare_a = fetch_table_value(i,nip_tuck_255_gen,192,1); + + + //uint8_t compare_a = gen_mcspwm(i,192,0); + //uint8_t compare_a = gen_sspwm(i,192,0); + //uint8_t compare_a = gen_thipwm(i,192,0); + //uint8_t compare_a = gen_natspwm(i,192,0); + + printf("%3d %3d %3d %2d \r\n", i, compare_base, compare_a, compare_a - compare_base); + //printf(" %3d,\r\n", compare_a); + + + + } + + +// printf("\r\ngenerator\r\n"); +// for (int i = 0; i < 256; i++) { +// +// gen_thipwm(i,255,1); +// } + +} + + +//https://www.infineon.com/dgdl/AP1609710_different_PWM_for_three_phase_ACIM.pdf?fileId=db3a304412b407950112b40a1bf20453 diff --git a/test/motorTest.sh b/test/motorTest.sh new file mode 100755 index 00000000..4d2cfc2e --- /dev/null +++ b/test/motorTest.sh @@ -0,0 +1,2 @@ +#/bin/sh +gcc -lm -o motorTest motorTest.c && ./motorTest diff --git a/test/offRoadTest.c b/test/offRoadTest.c new file mode 100644 index 00000000..27a35caa --- /dev/null +++ b/test/offRoadTest.c @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2018 Björn Schmidt + * + * 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 +#include +#include +#include +#include +#include + +#define MORSE_TIME_1 50 +#define MORSE_TIME_2 50 +#define MORSE_TIME_3 50 +#define MORSE_TOLERANCE 50 + +uint8_t ui8_offroad_counter; +uint8_t ui8_offroad_state = 18; +uint8_t ui16_aca_flags = 158; +uint8_t fake_brake_set = 127; + +typedef enum { + RESET = 0, SET = !RESET +} BitStatus; + +typedef enum { + // values from 0-31 are allowed as signals are stored in a single uint32_t + ASSIST_LVL_AFFECTS_THROTTLE = ((uint16_t) 1), + OFFROAD_ENABLED = ((uint16_t) 2), + BRAKE_DISABLES_OFFROAD = ((uint16_t) 4), + + DIGITAL_REGEN = ((uint16_t) 8), + SPEED_INFLUENCES_REGEN = ((uint16_t) 16), + SPEED_INFLUENCES_TORQUESENSOR = ((uint16_t) 32), + PAS_INVERTED = ((uint16_t) 64), + + DUMMY_ALWAYS_ON = ((uint16_t) 128) +} ACA_FLAGS; + +BitStatus GPIO_ReadInputPin() { + return ((BitStatus) (fake_brake_set<127 ? RESET:SET)); +} + +int kbhit(void) { + struct termios oldt, newt; + int ch; + int oldf; + + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + if (ch != EOF) { + ungetc(ch, stdin); + return 1; + } + + return 0; +} + +void updateSlowLoopStates(void) { + + if (((ui16_aca_flags & BRAKE_DISABLES_OFFROAD) == BRAKE_DISABLES_OFFROAD) && (ui8_offroad_state > 4)) { + // if disabling is enabled :) + if (!GPIO_ReadInputPin()) { + ui8_offroad_counter++; + if (ui8_offroad_counter == 255) {//disable on pressing brake for 5 seconds + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else { + ui8_offroad_counter = 0; + } + } + + // check if offroad mode is enabled + if (0 == (ui16_aca_flags & OFFROAD_ENABLED)) { + return; + } + + if (ui8_offroad_state == 0 && !GPIO_ReadInputPin()) {//first step, brake on. + ui8_offroad_state = 1; + } else if (ui8_offroad_state == 1) {//second step, make sure the brake is hold according to definded time + ui8_offroad_counter++; + if (GPIO_ReadInputPin() && ui8_offroad_counter < MORSE_TIME_1) {//brake is released too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_1 + MORSE_TOLERANCE) {//brake is released according to cheatcode + ui8_offroad_state = 2; + ui8_offroad_counter = 0; + } else if (!GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_1 + MORSE_TOLERANCE) {//brake is released too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 2) {//third step, make sure the brake is released according to definded time + ui8_offroad_counter++; + if (!GPIO_ReadInputPin() && ui8_offroad_counter < MORSE_TIME_2) { //brake is hold too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (!GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_2 + MORSE_TOLERANCE) {//brake is hold according to cheatcode + ui8_offroad_state = 3; + ui8_offroad_counter = 0; + + } else if (GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_2 + MORSE_TOLERANCE) {//brake is hold too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 3) {//second step, make sure the brake is hold according to definded time + ui8_offroad_counter++; + if (GPIO_ReadInputPin() && ui8_offroad_counter < MORSE_TIME_3) {//brake is released too early + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } else if (GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_3 + MORSE_TOLERANCE) {//brake is released according to cheatcode + ui8_offroad_state = 4; + ui8_offroad_counter = 0; + + } else if (!GPIO_ReadInputPin() && ui8_offroad_counter > MORSE_TIME_3 + MORSE_TOLERANCE) {//brake is released too late + ui8_offroad_state = 0; + ui8_offroad_counter = 0; + } + } else if (ui8_offroad_state == 4) { + // wait 3 seconds in state 4 for display feedback + ui8_offroad_counter++; + if (ui8_offroad_counter > 150) { + ui8_offroad_state = 255; + ui8_offroad_counter = 0; + } + } + + +} + +void main() { + + // disable echo + struct termios t; + tcgetattr(STDIN_FILENO, &t); + t.c_lflag &= ~ECHO; + tcsetattr(STDIN_FILENO, TCSANOW, &t); + + while (1) { + + if (kbhit()) { + getchar(); + fake_brake_set-=32; + } else { + fake_brake_set++; + } + if (fake_brake_set <80){ + fake_brake_set = 80; + } + if (fake_brake_set >127){ + fake_brake_set = 127; + } + + nanosleep((const struct timespec[]){ + {0, 20000000L} + }, NULL); + updateSlowLoopStates(); + + if (((ui16_aca_flags & BRAKE_DISABLES_OFFROAD) == BRAKE_DISABLES_OFFROAD) && 1 == 1) { + printf(" disabler on "); + } else { + printf(" disabler off "); + } + + if (0 == (ui16_aca_flags & OFFROAD_ENABLED)) { + printf(" enabler off "); + + } else { + printf(" enabler on "); + } + + if (!GPIO_ReadInputPin()) { + printf(" brake on "); + + } else { + printf(" brake off "); + } + + printf("%d %d ", ui8_offroad_state, ui8_offroad_counter); + + printf("\r\n"); + } +} + + diff --git a/test/offroadTest b/test/offroadTest new file mode 100755 index 00000000..e4cbf29c Binary files /dev/null and b/test/offroadTest differ diff --git a/test/offroadTest.sh b/test/offroadTest.sh new file mode 100644 index 00000000..d3000360 --- /dev/null +++ b/test/offroadTest.sh @@ -0,0 +1,2 @@ +#/bin/sh +gcc -o offroadTest offRoadTest.c && ./offroadTest diff --git a/test/wavetables/midpoint_clamp_192_gen.c b/test/wavetables/midpoint_clamp_192_gen.c new file mode 100644 index 00000000..ad37d48c --- /dev/null +++ b/test/wavetables/midpoint_clamp_192_gen.c @@ -0,0 +1,260 @@ +uint8_t midpoint_clamp_192_gen [256] = { + 96, + 100, + 104, + 108, + 112, + 116, + 120, + 124, + 128, + 132, + 136, + 140, + 144, + 148, + 152, + 156, + 160, + 163, + 167, + 171, + 174, + 178, + 180, + 181, + 182, + 183, + 184, + 185, + 186, + 187, + 187, + 188, + 189, + 189, + 190, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 190, + 189, + 189, + 188, + 188, + 187, + 186, + 185, + 184, + 183, + 182, + 181, + 180, + 179, + 180, + 181, + 182, + 183, + 184, + 185, + 186, + 187, + 188, + 188, + 189, + 189, + 190, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 190, + 189, + 189, + 188, + 187, + 187, + 186, + 185, + 184, + 183, + 182, + 181, + 180, + 178, + 174, + 171, + 167, + 163, + 160, + 156, + 152, + 148, + 144, + 140, + 136, + 132, + 128, + 124, + 120, + 116, + 112, + 108, + 104, + 100, + 96, + 92, + 88, + 84, + 80, + 76, + 72, + 68, + 64, + 60, + 56, + 52, + 48, + 44, + 40, + 36, + 32, + 29, + 25, + 21, + 18, + 14, + 12, + 11, + 10, + 9, + 8, + 7, + 6, + 5, + 5, + 4, + 3, + 3, + 2, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 2, + 3, + 3, + 4, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 12, + 11, + 10, + 9, + 8, + 7, + 6, + 5, + 4, + 4, + 3, + 3, + 2, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 2, + 3, + 3, + 4, + 5, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 14, + 18, + 21, + 25, + 29, + 32, + 36, + 40, + 44, + 48, + 52, + 56, + 60, + 64, + 68, + 72, + 76, + 80, + 84, + 88, + 92, + + +}; \ No newline at end of file diff --git a/test/wavetables/midpoint_clamp_192_svm_orig.c b/test/wavetables/midpoint_clamp_192_svm_orig.c new file mode 100644 index 00000000..57983cdd --- /dev/null +++ b/test/wavetables/midpoint_clamp_192_svm_orig.c @@ -0,0 +1,258 @@ +uint8_t midpoint_clamp_192_svm_orig [256] = { +95, + 100, + 103, + 108, + 112, + 115, + 120, + 124, + 128, + 132, + 136, + 140, + 143, + 148, + 152, + 155, + 159, + 163, + 167, + 170, + 173, + 177, + 179, + 180, + 182, + 182, + 183, + 184, + 185, + 186, + 187, + 188, + 188, + 188, + 189, + 190, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 190, + 189, + 188, + 188, + 188, + 187, + 186, + 185, + 185, + 184, + 182, + 182, + 181, + 179, + 179, + 179, + 181, + 182, + 182, + 184, + 185, + 185, + 186, + 187, + 188, + 188, + 188, + 189, + 190, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 190, + 189, + 188, + 188, + 188, + 187, + 186, + 185, + 184, + 183, + 182, + 182, + 180, + 179, + 177, + 173, + 170, + 167, + 163, + 159, + 155, + 152, + 148, + 143, + 140, + 136, + 132, + 128, + 124, + 120, + 115, + 112, + 108, + 103, + 100, + 95, + 91, + 87, + 83, + 79, + 75, + 71, + 67, + 63, + 59, + 55, + 51, + 47, + 43, + 39, + 36, + 32, + 28, + 24, + 21, + 17, + 13, + 12, + 10, + 9, + 9, + 7, + 6, + 6, + 5, + 4, + 3, + 3, + 2, + 2, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 2, + 3, + 3, + 4, + 4, + 6, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 11, + 10, + 9, + 8, + 7, + 6, + 6, + 4, + 4, + 3, + 3, + 2, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 2, + 2, + 3, + 3, + 4, + 5, + 6, + 6, + 7, + 9, + 9, + 10, + 12, + 13, + 17, + 21, + 24, + 28, + 32, + 36, + 39, + 43, + 47, + 51, + 55, + 59, + 63, + 67, + 71, + 75, + 79, + 83, + 87, +91 +}; \ No newline at end of file diff --git a/test/wavetables/midpoint_clamp_255_gen.c b/test/wavetables/midpoint_clamp_255_gen.c new file mode 100644 index 00000000..677f2318 --- /dev/null +++ b/test/wavetables/midpoint_clamp_255_gen.c @@ -0,0 +1,259 @@ +uint8_t midpoint_clamp_255_gen [256] = { + 127, + 133, + 138, + 144, + 149, + 155, + 160, + 165, + 171, + 176, + 181, + 186, + 192, + 197, + 202, + 207, + 212, + 217, + 222, + 227, + 232, + 236, + 239, + 240, + 242, + 243, + 244, + 246, + 247, + 248, + 249, + 250, + 251, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 253, + 253, + 252, + 252, + 251, + 250, + 249, + 248, + 247, + 246, + 245, + 244, + 242, + 241, + 239, + 238, + 239, + 241, + 242, + 244, + 245, + 246, + 247, + 248, + 249, + 250, + 251, + 252, + 252, + 253, + 253, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 253, + 253, + 252, + 251, + 251, + 250, + 249, + 248, + 247, + 246, + 244, + 243, + 242, + 240, + 239, + 236, + 232, + 227, + 222, + 217, + 212, + 207, + 202, + 197, + 192, + 186, + 181, + 176, + 171, + 165, + 160, + 155, + 149, + 144, + 138, + 133, + 127, + 122, + 117, + 111, + 106, + 100, + 95, + 90, + 84, + 79, + 74, + 69, + 63, + 58, + 53, + 48, + 43, + 38, + 33, + 28, + 23, + 19, + 16, + 15, + 13, + 12, + 11, + 9, + 8, + 7, + 6, + 5, + 4, + 4, + 3, + 2, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 2, + 2, + 3, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 13, + 14, + 16, + 17, + 16, + 14, + 13, + 11, + 10, + 9, + 8, + 7, + 6, + 5, + 4, + 3, + 3, + 2, + 2, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 2, + 3, + 4, + 4, + 5, + 6, + 7, + 8, + 9, + 11, + 12, + 13, + 15, + 16, + 19, + 23, + 28, + 33, + 38, + 43, + 48, + 53, + 58, + 63, + 69, + 74, + 79, + 84, + 90, + 95, + 100, + 106, + 111, + 117, + 122, + +}; \ No newline at end of file diff --git a/test/wavetables/midpoint_clamp_255_svm_orig.c b/test/wavetables/midpoint_clamp_255_svm_orig.c new file mode 100644 index 00000000..b15b4e09 --- /dev/null +++ b/test/wavetables/midpoint_clamp_255_svm_orig.c @@ -0,0 +1,258 @@ +uint8_t midpoint_clamp_255_svm_orig [256] = { + 127, + 133, + 138, + 144, + 149, + 154, + 160, + 165, + 170, + 176, + 181, + 186, + 191, + 197, + 202, + 207, + 212, + 217, + 222, + 227, + 231, + 236, + 239, + 240, + 242, + 243, + 244, + 245, + 247, + 248, + 249, + 250, + 250, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 253, + 253, + 252, + 251, + 251, + 250, + 249, + 248, + 247, + 246, + 245, + 243, + 242, + 241, + 239, + 238, + 239, + 241, + 242, + 243, + 245, + 246, + 247, + 248, + 249, + 250, + 251, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 253, + 253, + 252, + 251, + 250, + 250, + 249, + 248, + 247, + 245, + 244, + 243, + 242, + 240, + 239, + 236, + 231, + 227, + 222, + 217, + 212, + 207, + 202, + 197, + 191, + 186, + 181, + 176, + 170, + 165, + 160, + 154, + 149, + 144, + 138, + 133, + 127, + 122, + 116, + 111, + 106, + 100, + 95, + 89, + 84, + 79, + 74, + 68, + 63, + 58, + 53, + 48, + 43, + 38, + 33, + 28, + 23, + 18, + 16, + 14, + 13, + 12, + 10, + 9, + 8, + 7, + 6, + 5, + 4, + 3, + 3, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 2, + 2, + 3, + 4, + 5, + 6, + 6, + 8, + 9, + 10, + 11, + 12, + 14, + 15, + 17, + 15, + 14, + 12, + 11, + 10, + 9, + 8, + 6, + 6, + 5, + 4, + 3, + 2, + 2, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 3, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 12, + 13, + 14, + 16, + 18, + 23, + 28, + 33, + 38, + 43, + 48, + 53, + 58, + 63, + 68, + 74, + 79, + 84, + 89, + 95, + 100, + 106, + 111, + 116, + 122 +}; \ No newline at end of file diff --git a/test/wavetables/nip_tuck_192_gen.c b/test/wavetables/nip_tuck_192_gen.c new file mode 100644 index 00000000..e5a2f7f8 --- /dev/null +++ b/test/wavetables/nip_tuck_192_gen.c @@ -0,0 +1,259 @@ +uint8_t nip_tuck_192_gen [256] = { + 96, + 100, + 104, + 108, + 112, + 116, + 119, + 123, + 127, + 130, + 134, + 137, + 140, + 143, + 146, + 149, + 152, + 155, + 158, + 161, + 163, + 165, + 168, + 170, + 172, + 174, + 176, + 178, + 180, + 181, + 183, + 184, + 185, + 187, + 188, + 189, + 189, + 190, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 190, + 189, + 189, + 188, + 187, + 185, + 184, + 183, + 181, + 180, + 178, + 176, + 174, + 172, + 170, + 168, + 165, + 163, + 161, + 158, + 155, + 152, + 149, + 146, + 143, + 140, + 137, + 134, + 130, + 127, + 123, + 119, + 116, + 112, + 108, + 104, + 100, + 96, + 92, + 88, + 84, + 80, + 76, + 73, + 69, + 65, + 62, + 58, + 55, + 52, + 49, + 46, + 43, + 40, + 37, + 34, + 31, + 29, + 27, + 24, + 22, + 20, + 18, + 16, + 14, + 12, + 11, + 9, + 8, + 7, + 5, + 4, + 3, + 3, + 2, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 2, + 3, + 3, + 4, + 5, + 7, + 8, + 9, + 11, + 12, + 14, + 16, + 18, + 20, + 22, + 24, + 27, + 29, + 31, + 34, + 37, + 40, + 43, + 46, + 49, + 52, + 55, + 58, + 62, + 65, + 69, + 73, + 76, + 80, + 84, + 88, + 92, + +}; \ No newline at end of file diff --git a/test/wavetables/nip_tuck_255_gen.c b/test/wavetables/nip_tuck_255_gen.c new file mode 100644 index 00000000..cfc2679c --- /dev/null +++ b/test/wavetables/nip_tuck_255_gen.c @@ -0,0 +1,260 @@ +uint8_t nip_tuck_255_gen [256] = { + 127, + 133, + 138, + 143, + 149, + 154, + 159, + 163, + 168, + 173, + 177, + 182, + 186, + 190, + 194, + 198, + 202, + 206, + 210, + 213, + 217, + 220, + 223, + 226, + 229, + 231, + 234, + 236, + 239, + 241, + 243, + 245, + 246, + 248, + 249, + 251, + 252, + 253, + 253, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 253, + 253, + 252, + 251, + 249, + 248, + 246, + 245, + 243, + 241, + 239, + 236, + 234, + 231, + 229, + 226, + 223, + 220, + 217, + 213, + 210, + 206, + 202, + 198, + 194, + 190, + 186, + 182, + 177, + 173, + 168, + 163, + 159, + 154, + 149, + 143, + 138, + 133, + 127, + 122, + 117, + 112, + 106, + 101, + 96, + 92, + 87, + 82, + 78, + 73, + 69, + 65, + 61, + 57, + 53, + 49, + 45, + 42, + 38, + 35, + 32, + 29, + 26, + 24, + 21, + 19, + 16, + 14, + 12, + 10, + 9, + 7, + 6, + 4, + 3, + 2, + 2, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 2, + 2, + 3, + 4, + 6, + 7, + 9, + 10, + 12, + 14, + 16, + 19, + 21, + 24, + 26, + 29, + 32, + 35, + 38, + 42, + 45, + 49, + 53, + 57, + 61, + 65, + 69, + 73, + 78, + 82, + 87, + 92, + 96, + 101, + 106, + 112, + 117, + 122, + + +}; \ No newline at end of file diff --git a/test/wavetables/pure_sine_192_gen.c b/test/wavetables/pure_sine_192_gen.c new file mode 100644 index 00000000..87ee20ae --- /dev/null +++ b/test/wavetables/pure_sine_192_gen.c @@ -0,0 +1,258 @@ +uint8_t pure_sine_192_gen [256] = { + 96, + 98, + 101, + 103, + 105, + 108, + 110, + 112, + 115, + 117, + 119, + 122, + 124, + 126, + 128, + 131, + 133, + 135, + 137, + 139, + 141, + 143, + 145, + 147, + 149, + 151, + 153, + 155, + 157, + 159, + 160, + 162, + 164, + 166, + 167, + 169, + 170, + 172, + 173, + 174, + 176, + 177, + 178, + 180, + 181, + 182, + 183, + 184, + 185, + 186, + 186, + 187, + 188, + 189, + 189, + 190, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 190, + 189, + 189, + 188, + 187, + 186, + 186, + 185, + 184, + 183, + 182, + 181, + 180, + 178, + 177, + 176, + 174, + 173, + 172, + 170, + 169, + 167, + 166, + 164, + 162, + 160, + 159, + 157, + 155, + 153, + 151, + 149, + 147, + 145, + 143, + 141, + 139, + 137, + 135, + 133, + 131, + 128, + 126, + 124, + 122, + 119, + 117, + 115, + 112, + 110, + 108, + 105, + 103, + 101, + 98, + 96, + 94, + 91, + 89, + 87, + 84, + 82, + 80, + 77, + 75, + 73, + 70, + 68, + 66, + 64, + 61, + 59, + 57, + 55, + 53, + 51, + 49, + 47, + 45, + 43, + 41, + 39, + 37, + 35, + 33, + 32, + 30, + 28, + 26, + 25, + 23, + 22, + 20, + 19, + 18, + 16, + 15, + 14, + 12, + 11, + 10, + 9, + 8, + 7, + 6, + 6, + 5, + 4, + 3, + 3, + 2, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 2, + 3, + 3, + 4, + 5, + 6, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 14, + 15, + 16, + 18, + 19, + 20, + 22, + 23, + 25, + 26, + 28, + 30, + 32, + 33, + 35, + 37, + 39, + 41, + 43, + 45, + 47, + 49, + 51, + 53, + 55, + 57, + 59, + 61, + 64, + 66, + 68, + 70, + 73, + 75, + 77, + 80, + 82, + 84, + 87, + 89, + 91, + 94, +}; \ No newline at end of file diff --git a/test/wavetables/pure_sine_255_gen.c b/test/wavetables/pure_sine_255_gen.c new file mode 100644 index 00000000..2681b8cf --- /dev/null +++ b/test/wavetables/pure_sine_255_gen.c @@ -0,0 +1,259 @@ +uint8_t pure_sine_255_gen [256] = { + + 127, + 131, + 134, + 137, + 140, + 143, + 146, + 149, + 152, + 155, + 158, + 162, + 165, + 167, + 170, + 173, + 176, + 179, + 182, + 185, + 188, + 190, + 193, + 196, + 198, + 201, + 203, + 206, + 208, + 211, + 213, + 215, + 218, + 220, + 222, + 224, + 226, + 228, + 230, + 232, + 234, + 235, + 237, + 238, + 240, + 241, + 243, + 244, + 245, + 246, + 248, + 249, + 250, + 250, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 253, + 253, + 252, + 251, + 250, + 250, + 249, + 248, + 246, + 245, + 244, + 243, + 241, + 240, + 238, + 237, + 235, + 234, + 232, + 230, + 228, + 226, + 224, + 222, + 220, + 218, + 215, + 213, + 211, + 208, + 206, + 203, + 201, + 198, + 196, + 193, + 190, + 188, + 185, + 182, + 179, + 176, + 173, + 170, + 167, + 165, + 162, + 158, + 155, + 152, + 149, + 146, + 143, + 140, + 137, + 134, + 131, + 127, + 124, + 121, + 118, + 115, + 112, + 109, + 106, + 103, + 100, + 97, + 93, + 90, + 88, + 85, + 82, + 79, + 76, + 73, + 70, + 67, + 65, + 62, + 59, + 57, + 54, + 52, + 49, + 47, + 44, + 42, + 40, + 37, + 35, + 33, + 31, + 29, + 27, + 25, + 23, + 21, + 20, + 18, + 17, + 15, + 14, + 12, + 11, + 10, + 9, + 7, + 6, + 5, + 5, + 4, + 3, + 2, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 2, + 3, + 4, + 5, + 5, + 6, + 7, + 9, + 10, + 11, + 12, + 14, + 15, + 17, + 18, + 20, + 21, + 23, + 25, + 27, + 29, + 31, + 33, + 35, + 37, + 40, + 42, + 44, + 47, + 49, + 52, + 54, + 57, + 59, + 62, + 65, + 67, + 70, + 73, + 76, + 79, + 82, + 85, + 88, + 90, + 93, + 97, + 100, + 103, + 106, + 109, + 112, + 115, + 118, + 121, + 124 +}; \ No newline at end of file diff --git a/test/wavetables/third_harmonic_192_gen.c b/test/wavetables/third_harmonic_192_gen.c new file mode 100644 index 00000000..23ee0162 --- /dev/null +++ b/test/wavetables/third_harmonic_192_gen.c @@ -0,0 +1,259 @@ +uint8_t third_harmonic_192_gen [256] = { + 96, + 100, + 104, + 108, + 112, + 116, + 120, + 124, + 127, + 131, + 135, + 138, + 142, + 145, + 148, + 152, + 155, + 158, + 160, + 163, + 166, + 168, + 171, + 173, + 175, + 177, + 179, + 180, + 182, + 183, + 185, + 186, + 187, + 188, + 189, + 189, + 190, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 191, + 190, + 190, + 190, + 190, + 190, + 190, + 189, + 189, + 189, + 189, + 189, + 189, + 189, + 189, + 189, + 190, + 190, + 190, + 190, + 190, + 190, + 191, + 191, + 191, + 191, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 192, + 191, + 191, + 191, + 190, + 189, + 189, + 188, + 187, + 186, + 185, + 183, + 182, + 180, + 179, + 177, + 175, + 173, + 171, + 168, + 166, + 163, + 160, + 158, + 155, + 152, + 148, + 145, + 142, + 138, + 135, + 131, + 127, + 124, + 120, + 116, + 112, + 108, + 104, + 100, + 96, + 92, + 88, + 84, + 80, + 76, + 72, + 68, + 65, + 61, + 57, + 54, + 50, + 47, + 44, + 40, + 37, + 34, + 32, + 29, + 26, + 24, + 21, + 19, + 17, + 15, + 13, + 12, + 10, + 9, + 7, + 6, + 5, + 4, + 3, + 3, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 1, + 2, + 2, + 2, + 2, + 2, + 2, + 3, + 3, + 3, + 3, + 3, + 3, + 3, + 3, + 3, + 2, + 2, + 2, + 2, + 2, + 2, + 1, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 3, + 3, + 4, + 5, + 6, + 7, + 9, + 10, + 12, + 13, + 15, + 17, + 19, + 21, + 24, + 26, + 29, + 32, + 34, + 37, + 40, + 44, + 47, + 50, + 54, + 57, + 61, + 65, + 68, + 72, + 76, + 80, + 84, + 88, + 92 + +}; \ No newline at end of file diff --git a/test/wavetables/third_harmonic_255_gen.c b/test/wavetables/third_harmonic_255_gen.c new file mode 100644 index 00000000..714e2d4f --- /dev/null +++ b/test/wavetables/third_harmonic_255_gen.c @@ -0,0 +1,260 @@ +uint8_t third_harmonic_255_gen [256] = { + 127, + 133, + 138, + 143, + 149, + 154, + 159, + 164, + 169, + 174, + 179, + 184, + 188, + 193, + 197, + 201, + 205, + 209, + 213, + 217, + 220, + 223, + 227, + 229, + 232, + 235, + 237, + 239, + 242, + 243, + 245, + 247, + 248, + 249, + 251, + 251, + 252, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 254, + 253, + 253, + 253, + 252, + 252, + 252, + 252, + 252, + 252, + 251, + 251, + 251, + 251, + 251, + 252, + 252, + 252, + 252, + 252, + 252, + 253, + 253, + 253, + 254, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 253, + 252, + 251, + 251, + 249, + 248, + 247, + 245, + 243, + 242, + 239, + 237, + 235, + 232, + 229, + 227, + 223, + 220, + 217, + 213, + 209, + 205, + 201, + 197, + 193, + 188, + 184, + 179, + 174, + 169, + 164, + 159, + 154, + 149, + 143, + 138, + 133, + 127, + 122, + 117, + 112, + 106, + 101, + 96, + 91, + 86, + 81, + 76, + 71, + 67, + 62, + 58, + 54, + 50, + 46, + 42, + 38, + 35, + 32, + 28, + 26, + 23, + 20, + 18, + 16, + 13, + 12, + 10, + 8, + 7, + 6, + 4, + 4, + 3, + 2, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 1, + 2, + 2, + 2, + 3, + 3, + 3, + 3, + 3, + 3, + 4, + 4, + 4, + 4, + 4, + 3, + 3, + 3, + 3, + 3, + 3, + 2, + 2, + 2, + 1, + 1, + 1, + 1, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 1, + 1, + 1, + 2, + 3, + 4, + 4, + 6, + 7, + 8, + 10, + 12, + 13, + 16, + 18, + 20, + 23, + 26, + 28, + 32, + 35, + 38, + 42, + 46, + 50, + 54, + 58, + 62, + 67, + 71, + 76, + 81, + 86, + 91, + 96, + 101, + 106, + 112, + 117, + 122, + + +}; \ No newline at end of file diff --git a/timers.c b/timers.c index ddeff11e..f5320bfc 100644 --- a/timers.c +++ b/timers.c @@ -6,13 +6,56 @@ * Released under the GPL License, Version 3 */ +#include +#include +#include "stm8s_gpio.h" +#include "stm8s_tim1.h" #include "stm8s.h" #include "stm8s_tim2.h" +#include "main.h" +#include "motor.h" +#include "gpio.h" + + + void timer2_init (void) { // TIM2 Peripheral Configuration TIM2_DeInit(); - TIM2_TimeBaseInit(TIM2_PRESCALER_16384, 0xffff); // each incremment at every ~1ms + + TIM2_TimeBaseInit(TIM2_PRESCALER_8, 25536); //timetic with 50HZ + TIM2_ITConfig(TIM2_IT_UPDATE,ENABLE); + TIM2_Cmd(ENABLE); // TIM2 counter enable + +} + + + + +//interrupt routine for slow control loop timing +void TIM2_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM2_UPD_OVF_TRG_BRK_IRQHANDLER) +{ + ui8_slowloop_flag=1; + //printf("SlowTimetic\n"); + // clear the interrupt pending bit for TIM2 + TIM2_ClearITPendingBit(TIM2_IT_UPDATE); +} + + +#define CLOCKFREQ (16000000L) +/* Instructions per millisecond. */ +#define INSNS_PER_MS (CLOCKFREQ / 4000L) +/* Delay loop is about 10 cycles per iteration. */ +#define LOOPS_PER_MS (INSNS_PER_MS/20L) +void delay_halfms(uint16_t ms) { + uint16_t u; + while (ms--) { + /* Inner loop takes about 10 cycles per iteration + 4 cycles setup. */ + for (u = 0; u < LOOPS_PER_MS; u++) { + /* Prevent this loop from being optimized away. */ + __asm nop __endasm; + } + } } diff --git a/timers.h b/timers.h index 73f1c388..391c2156 100644 --- a/timers.h +++ b/timers.h @@ -10,5 +10,6 @@ #define _TIMERS_H_ void timer2_init (void); +void delay_halfms(uint16_t ms) ; #endif /* _TIMERS_H_ */ diff --git a/tools/JavaConfigurator/build.xml b/tools/JavaConfigurator/build.xml new file mode 100644 index 00000000..f452ba96 --- /dev/null +++ b/tools/JavaConfigurator/build.xml @@ -0,0 +1,73 @@ + + + + + + + + + + + Builds, tests, and runs the project JavaConfigurator. + + + diff --git a/tools/JavaConfigurator/makeJar.bat b/tools/JavaConfigurator/makeJar.bat new file mode 100644 index 00000000..9edb8042 --- /dev/null +++ b/tools/JavaConfigurator/makeJar.bat @@ -0,0 +1,9 @@ +PATH = %PATH%;c:\Program Files\Java\jdk1.8.0_181\bin + +javac -d . src\OSEC.java +jar cfe OSEC.jar OSEC *.class +del *.class +del "..\OSEC Parameter Configurator.jar" +move OSEC.jar "..\..\OSEC Parameter Configurator.jar" +pause +exit diff --git a/tools/JavaConfigurator/makeJar.sh b/tools/JavaConfigurator/makeJar.sh new file mode 100755 index 00000000..8d4680ae --- /dev/null +++ b/tools/JavaConfigurator/makeJar.sh @@ -0,0 +1,8 @@ +#!/bin/bash + +javac -d . src/OSEC.java +jar cfe OSEC.jar OSEC *.class +rm *.class +rm "../../OSEC Parameter Configurator.jar" +mv OSEC.jar "../../OSEC Parameter Configurator.jar" + diff --git a/tools/JavaConfigurator/manifest.mf b/tools/JavaConfigurator/manifest.mf new file mode 100644 index 00000000..1574df4a --- /dev/null +++ b/tools/JavaConfigurator/manifest.mf @@ -0,0 +1,3 @@ +Manifest-Version: 1.0 +X-COMMENT: Main-Class will be added automatically by build + diff --git a/tools/JavaConfigurator/nbproject/build-impl.xml b/tools/JavaConfigurator/nbproject/build-impl.xml new file mode 100644 index 00000000..7a98f379 --- /dev/null +++ b/tools/JavaConfigurator/nbproject/build-impl.xml @@ -0,0 +1,1420 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must set src.dir + Must set test.src.dir + Must set build.dir + Must set dist.dir + Must set build.classes.dir + Must set dist.javadoc.dir + Must set build.test.classes.dir + Must set build.test.results.dir + Must set build.classes.excludes + Must set dist.jar + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must set javac.includes + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + No tests executed. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must set JVM to use for profiling in profiler.info.jvm + Must set profiler agent JVM arguments in profiler.info.jvmargs.agent + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must select some files in the IDE or set javac.includes + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + To run this application from the command line without Ant, try: + + java -jar "${dist.jar.resolved}" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must select one file in the IDE or set run.class + + + + Must select one file in the IDE or set run.class + + + + + + + + + + + + + + + + + + + + + + + Must select one file in the IDE or set debug.class + + + + + Must select one file in the IDE or set debug.class + + + + + Must set fix.includes + + + + + + + + + + This target only works when run from inside the NetBeans IDE. + + + + + + + + + Must select one file in the IDE or set profile.class + This target only works when run from inside the NetBeans IDE. + + + + + + + + + This target only works when run from inside the NetBeans IDE. + + + + + + + + + + + + + This target only works when run from inside the NetBeans IDE. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must select one file in the IDE or set run.class + + + + + + Must select some files in the IDE or set test.includes + + + + + Must select one file in the IDE or set run.class + + + + + Must select one file in the IDE or set applet.url + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Must select some files in the IDE or set javac.includes + + + + + + + + + + + + + + + + + + + + Some tests failed; see details above. + + + + + + + + + Must select some files in the IDE or set test.includes + + + + Some tests failed; see details above. + + + + Must select some files in the IDE or set test.class + Must select some method in the IDE or set test.method + + + + Some tests failed; see details above. + + + + + Must select one file in the IDE or set test.class + + + + Must select one file in the IDE or set test.class + Must select some method in the IDE or set test.method + + + + + + + + + + + + + + Must select one file in the IDE or set applet.url + + + + + + + + + Must select one file in the IDE or set applet.url + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/JavaConfigurator/nbproject/genfiles.properties b/tools/JavaConfigurator/nbproject/genfiles.properties new file mode 100644 index 00000000..32e1726f --- /dev/null +++ b/tools/JavaConfigurator/nbproject/genfiles.properties @@ -0,0 +1,8 @@ +build.xml.data.CRC32=d21f578f +build.xml.script.CRC32=c094a861 +build.xml.stylesheet.CRC32=8064a381@1.80.1.48 +# This file is used by a NetBeans-based IDE to track changes in generated files such as build-impl.xml. +# Do not edit this file. You may delete it but then the IDE will never regenerate such files for you. +nbproject/build-impl.xml.data.CRC32=d21f578f +nbproject/build-impl.xml.script.CRC32=f6c2e4e6 +nbproject/build-impl.xml.stylesheet.CRC32=830a3534@1.80.1.48 diff --git a/tools/JavaConfigurator/nbproject/project.properties b/tools/JavaConfigurator/nbproject/project.properties new file mode 100644 index 00000000..eb24b9da --- /dev/null +++ b/tools/JavaConfigurator/nbproject/project.properties @@ -0,0 +1,74 @@ +annotation.processing.enabled=true +annotation.processing.enabled.in.editor=false +annotation.processing.processor.options= +annotation.processing.processors.list= +annotation.processing.run.all.processors=true +annotation.processing.source.output=${build.generated.sources.dir}/ap-source-output +build.classes.dir=${build.dir}/classes +build.classes.excludes=**/*.java,**/*.form +# This directory is removed when the project is cleaned: +build.dir=build +build.generated.dir=${build.dir}/generated +build.generated.sources.dir=${build.dir}/generated-sources +# Only compile against the classpath explicitly listed here: +build.sysclasspath=ignore +build.test.classes.dir=${build.dir}/test/classes +build.test.results.dir=${build.dir}/test/results +# Uncomment to specify the preferred debugger connection transport: +#debug.transport=dt_socket +debug.classpath=\ + ${run.classpath} +debug.test.classpath=\ + ${run.test.classpath} +# Files in build.classes.dir which should be excluded from distribution jar +dist.archive.excludes= +# This directory is removed when the project is cleaned: +dist.dir=dist +dist.jar=${dist.dir}/JavaConfigurator.jar +dist.javadoc.dir=${dist.dir}/javadoc +excludes= +includes=** +jar.compress=false +javac.classpath= +# Space-separated list of extra javac options +javac.compilerargs= +javac.deprecation=false +javac.external.vm=true +javac.processorpath=\ + ${javac.classpath} +javac.source=1.8 +javac.target=1.8 +javac.test.classpath=\ + ${javac.classpath}:\ + ${build.classes.dir} +javac.test.processorpath=\ + ${javac.test.classpath} +javadoc.additionalparam= +javadoc.author=false +javadoc.encoding=${source.encoding} +javadoc.noindex=false +javadoc.nonavbar=false +javadoc.notree=false +javadoc.private=false +javadoc.splitindex=true +javadoc.use=true +javadoc.version=false +javadoc.windowtitle= +main.class= +manifest.file=manifest.mf +meta.inf.dir=${src.dir}/META-INF +mkdist.disabled=false +platform.active=default_platform +run.classpath=\ + ${javac.classpath}:\ + ${build.classes.dir} +# Space-separated list of JVM arguments used when running the project. +# You may also define separate properties like run-sys-prop.name=value instead of -Dname=value. +# To set system properties for unit tests define test-sys-prop.name=value: +run.jvmargs= +run.test.classpath=\ + ${javac.test.classpath}:\ + ${build.test.classes.dir} +source.encoding=UTF-8 +src.dir=src +test.src.dir=test diff --git a/tools/JavaConfigurator/nbproject/project.xml b/tools/JavaConfigurator/nbproject/project.xml new file mode 100644 index 00000000..5f96edef --- /dev/null +++ b/tools/JavaConfigurator/nbproject/project.xml @@ -0,0 +1,15 @@ + + + org.netbeans.modules.java.j2seproject + + + JavaConfigurator + + + + + + + + + diff --git a/tools/JavaConfigurator/src/OSEC.java b/tools/JavaConfigurator/src/OSEC.java new file mode 100644 index 00000000..293d7b1b --- /dev/null +++ b/tools/JavaConfigurator/src/OSEC.java @@ -0,0 +1,1384 @@ + +import java.awt.BorderLayout; +import java.awt.EventQueue; +import java.io.*; +import java.awt.Desktop; +import java.net.URI; +import java.net.URISyntaxException; +import java.text.SimpleDateFormat; +import java.util.Date; +import javax.swing.JFrame; +import javax.swing.JPanel; +import javax.swing.border.EmptyBorder; +import javax.swing.text.JTextComponent; +import javax.swing.JTextField; +import javax.swing.JButton; +import javax.swing.ListSelectionModel; +import javax.swing.JLabel; +import javax.swing.AbstractAction; +import java.awt.event.ActionEvent; +import javax.swing.Action; +import java.awt.event.ActionListener; +import java.awt.GridLayout; +import java.awt.Font; +import java.awt.Color; +import java.io.BufferedWriter; +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import javax.swing.SwingConstants; +import javax.swing.JOptionPane; +import javax.swing.JRadioButton; +import javax.swing.ButtonGroup; +import javax.swing.JList; +import javax.swing.JScrollPane; +import java.awt.Dimension; +import java.awt.event.ItemEvent; +import java.awt.event.ItemListener; +import java.util.List; +import java.util.ArrayList; +import java.io.File; +import java.util.logging.Level; +import java.util.logging.Logger; +import javax.swing.event.ListSelectionEvent; +import javax.swing.event.ListSelectionListener; +import javax.swing.DefaultListModel; +import javax.swing.event.ListDataListener; +import java.awt.event.MouseAdapter; +import java.awt.event.MouseEvent; +import java.nio.file.Paths; +import java.util.Arrays; +import javax.swing.JCheckBox; + +public class OSEC extends JFrame { + + public class FileContainer { + + public FileContainer(File file) { + this.file = file; + } + public File file; + + @Override + public String toString() { + return file.getName(); + } + } + + private JPanel contentPane; + private JTextField txtPasTimeout; + private JTextField txtWheelCircumference; + private JTextField txtNumberOfPas; + private JLabel lblSpeedLimit; + private JTextField txtSpeedlimit; + private JTextField txtSpeedlimitWithoutPas; + private JTextField txtSpeedlimitWithThrottleOverride; + private JLabel lblThrottleMin; + private JTextField txtThrottlemin; + private JTextField batteryVoltageCalib; + private JLabel lblThrottleMax; + private JTextField txtThrottlemax; + private JButton btnWriteConfiguration; + private JTextField txtMaxbatterycurrent; + private JTextField txtUndervoltage; + private JTextField txtOvervoltage; + //private final ButtonGroup Ridingmode = new ButtonGroup(); + private JTextField txtMotor_specific_angle; + private JTextField txtBatteryCurcala; + private JLabel lblDiplayType; + private final ButtonGroup displayButtonGroup = new ButtonGroup(); + + private JTextField Assist_Level_1; + private JTextField Assist_Level_2; + private JTextField Assist_Level_3; + private JTextField Assist_Level_4; + private JLabel lblAssistLevel_3; + private JLabel lblAssistLevel_4; + private JTextField Assist_Level_5; + private JTextField Morse_Time_1; + private JTextField Morse_Time_2; + private JTextField Morse_Time_3; + private JTextField ramp_end; + private JTextField ramp_start; + private JTextField flt_tqCalibrationFactor; + private JTextField p_factor; + private JTextField i_factor; + private JTextField GearRatio; + private JTextField txtMaxregencurrent; + + private JTextField PAS_threshold; + private JTextField txtMaxphasecurrent; + private JRadioButton rdbtnBluOsecDisplay; + + private JRadioButton rdbtnKingmeterJlcd; + private JRadioButton rdbtnKtlcd; + private JRadioButton rdbtnDiganostics; + + private JCheckBox cbResetEeprom; + + private JCheckBox cbTorqueSensor; + private JCheckBox cbBypassLowSpeedRegenPiControl; + private JCheckBox cbAssistLevelInfluencesThrottle; + private JCheckBox cbOffroadEnabled; + private JCheckBox cbBrakeDisablesOffroad; + + private JCheckBox cbDigitalRegen; + private JCheckBox cbSpeedInfluencesRegen; + private JCheckBox cbSpeedInfluencesTqSensor; + private JCheckBox cbPasInverted; + private JCheckBox cbPwmOff; + private JCheckBox cbDcNull; + private JCheckBox cbHighSpeedMotor; + private JCheckBox cbAntiJitter; + private JCheckBox cbSwitch360; + private JCheckBox cbDisable60DegInterpolation; + private JCheckBox cbDisableInterpolation; + private JCheckBox cbCorrectionEnabled; + private JCheckBox cbExternalSpeedSensor; + private JCheckBox cbIdleDisablesOffroad; + private JCheckBox cbPowerBasedControlEnabled; + private JCheckBox cbDynAssist; + + private JTextField txtAngle4; + private JTextField txtAngle6; + private JTextField txtAngle2; + private JTextField txtAngle3; + private JTextField txtAngle1; + private JTextField txtAngle5; + private JTextField txtCorrectionAtAngle; + private JRadioButton rdbtnNodisplay; + + private File experimentalSettingsDir; + private File lastSettingsFile = null; + + /** + * Launch the application. + */ + public static void main(String[] args) { + + EventQueue.invokeLater(new Runnable() { + public void run() { + try { + OSEC frame = new OSEC(); + frame.setVisible(true); + } catch (Exception e) { + e.printStackTrace(); + } + } + }); + } + + public void loadSettings(File f) throws IOException { + + BufferedReader in = new BufferedReader(new FileReader(f)); + txtNumberOfPas.setText(in.readLine()); + txtSpeedlimit.setText(in.readLine()); + txtPasTimeout.setText(in.readLine()); + txtWheelCircumference.setText(in.readLine()); + txtSpeedlimitWithoutPas.setText(in.readLine()); + if (txtSpeedlimitWithoutPas.getText().trim().isEmpty()) { + txtSpeedlimitWithoutPas.setText("6"); + } + txtThrottlemin.setText(in.readLine()); + txtThrottlemax.setText(in.readLine()); + txtUndervoltage.setText(in.readLine()); + txtMaxbatterycurrent.setText(in.readLine()); + txtMaxphasecurrent.setText(in.readLine()); + txtMaxregencurrent.setText(in.readLine()); + txtMotor_specific_angle.setText(in.readLine()); + txtBatteryCurcala.setText(in.readLine()); + in.readLine(); + in.readLine(); + Assist_Level_1.setText(in.readLine()); + Assist_Level_2.setText(in.readLine()); + Assist_Level_3.setText(in.readLine()); + Assist_Level_4.setText(in.readLine()); + Assist_Level_5.setText(in.readLine()); + Morse_Time_1.setText(in.readLine()); + Morse_Time_2.setText(in.readLine()); + Morse_Time_3.setText(in.readLine()); + ramp_end.setText(in.readLine()); + p_factor.setText(in.readLine()); + i_factor.setText(in.readLine()); + GearRatio.setText(in.readLine()); + in.readLine(); + PAS_threshold.setText(in.readLine()); + + cbTorqueSensor.setSelected(Boolean.parseBoolean(in.readLine())); + ramp_start.setText(in.readLine()); + txtSpeedlimitWithThrottleOverride.setText(in.readLine()); + if (txtSpeedlimitWithThrottleOverride.getText().trim().isEmpty()) { + txtSpeedlimitWithThrottleOverride.setText("25"); + } + String tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtCorrectionAtAngle.setText(tmp); + } + + in.readLine(); + in.readLine(); + + rdbtnKtlcd.setSelected(Boolean.parseBoolean(in.readLine())); + rdbtnKingmeterJlcd.setSelected(Boolean.parseBoolean(in.readLine())); + in.readLine(); + in.readLine(); // old unused + rdbtnDiganostics.setSelected(Boolean.parseBoolean(in.readLine())); + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle4.setText(tmp); + } + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle6.setText(tmp); + } + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle2.setText(tmp); + } + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle3.setText(tmp); + } + rdbtnBluOsecDisplay.setSelected(Boolean.parseBoolean(in.readLine())); + rdbtnNodisplay.setSelected(Boolean.parseBoolean(in.readLine())); + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle1.setText(tmp); + } + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + txtAngle5.setText(tmp); + } + flt_tqCalibrationFactor.setText(in.readLine()); + + int acaFlags = Integer.parseInt(in.readLine()); + cbAssistLevelInfluencesThrottle.setSelected((acaFlags & 1) > 0); + cbOffroadEnabled.setSelected((acaFlags & 2) > 0); + cbBrakeDisablesOffroad.setSelected((acaFlags & 4) > 0); + + cbDigitalRegen.setSelected((acaFlags & 8) > 0); + cbSpeedInfluencesRegen.setSelected((acaFlags & 16) > 0); + cbSpeedInfluencesTqSensor.setSelected((acaFlags & 32) > 0); + cbPasInverted.setSelected((acaFlags & 64) > 0); + cbBypassLowSpeedRegenPiControl.setSelected((acaFlags & 256) > 0); + + cbDynAssist.setSelected((acaFlags & 512) > 0); + cbPowerBasedControlEnabled.setSelected((acaFlags & 1024) > 0); + cbTorqueSensor.setSelected((acaFlags & 2048) > 0); + cbCorrectionEnabled.setSelected((acaFlags & 4096) > 0); + cbIdleDisablesOffroad.setSelected((acaFlags & 16384) > 0); + cbExternalSpeedSensor.setSelected((acaFlags & 8192) > 0); + tmp = in.readLine(); + if (tmp.trim().length() > 0) { + batteryVoltageCalib.setText(tmp); + } + + int acaExperimentalFlags = Integer.parseInt(in.readLine()); + cbPwmOff.setSelected((acaExperimentalFlags & 1024) > 0); + cbDcNull.setSelected((acaExperimentalFlags & 1) > 0); + cbAntiJitter.setSelected((acaExperimentalFlags & 2) > 0); + cbHighSpeedMotor.setSelected((acaExperimentalFlags & 256) > 0); + cbSwitch360.setSelected((acaExperimentalFlags & 16) > 0); + cbDisableInterpolation.setSelected((acaExperimentalFlags & 4) > 0); + cbDisable60DegInterpolation.setSelected((acaExperimentalFlags & 8) > 0); + + txtOvervoltage.setText(in.readLine()); + in.close(); + } + + private void updateDependiencies(boolean resetActivatedValue) { + if (cbTorqueSensor.isSelected()) { + ramp_end.setText("0"); + ramp_end.setEditable(false); + ramp_start.setText("0"); + ramp_start.setEditable(false); + flt_tqCalibrationFactor.setEditable(true); + if (resetActivatedValue) { + flt_tqCalibrationFactor.setText("1000.0"); + } + } else { + flt_tqCalibrationFactor.setText("0.0"); + if (resetActivatedValue) { + ramp_end.setText("1500"); + ramp_start.setText("64000"); + } + ramp_end.setEditable(true); + ramp_start.setEditable(true); + flt_tqCalibrationFactor.setEditable(false); + + } + } + + /** + * Create the frame. + * + * @throws IOException + */ + public OSEC() throws IOException { + + setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + setBounds(100, 100, 960, 720); + contentPane = new JPanel(); + contentPane.setBorder(new EmptyBorder(5, 5, 5, 5)); + setContentPane(contentPane); + contentPane.setLayout(null); + + JLabel footnote = new JLabel("*) manual adjustments needed in config.h"); + footnote.setBounds(600, 650, 320, 25); + contentPane.add(footnote); + + JLabel lblTitle = new JLabel("OSEC Parameter Configurator"); + lblTitle.setHorizontalAlignment(SwingConstants.RIGHT); + lblTitle.setForeground(Color.black); + lblTitle.setFont(new Font("Tahoma", Font.PLAIN, 18)); + lblTitle.setBounds(100, 4, 300, 34); + contentPane.add(lblTitle); + + JLabel lblOpenSourceFirmware = new JLabel("Open Source Firmware for E-Bike Controller"); + lblOpenSourceFirmware.setHorizontalAlignment(SwingConstants.RIGHT); + lblOpenSourceFirmware.setFont(new Font("Tahoma", Font.PLAIN, 11)); + lblOpenSourceFirmware.setForeground(Color.BLUE); + lblOpenSourceFirmware.setBounds(100, 34, 300, 14); + contentPane.add(lblOpenSourceFirmware); + + JLabel lblRTFM = new JLabel("Read the friendly Wiki before using"); + lblRTFM.setHorizontalAlignment(SwingConstants.RIGHT); + lblRTFM.setFont(new Font("Tahoma", Font.PLAIN, 11)); + lblRTFM.setForeground(Color.RED); + lblRTFM.setBounds(100, 50, 300, 14); + contentPane.add(lblRTFM); + + JLabel lblgray = new JLabel("Gray settings can be changed @ runtime (via BluOsec App)"); + lblgray.setHorizontalAlignment(SwingConstants.RIGHT); + lblgray.setFont(new Font("Tahoma", Font.PLAIN, 11)); + lblgray.setForeground(Color.GRAY); + lblgray.setBounds(100, 66, 300, 14); + contentPane.add(lblgray); + + + experimentalSettingsDir = new File(Paths.get(".").toAbsolutePath().normalize().toString()); + while (!Arrays.asList(experimentalSettingsDir.list()).contains("experimental settings")) { + experimentalSettingsDir = experimentalSettingsDir.getParentFile(); + } + File provenSettingsDir = new File(experimentalSettingsDir.getAbsolutePath() + File.separator + "proven settings"); + experimentalSettingsDir = new File(experimentalSettingsDir.getAbsolutePath() + File.separator + "experimental settings"); + + DefaultListModel provenSettingsFilesModel = new DefaultListModel(); + DefaultListModel experimentalSettingsFilesModel = new DefaultListModel(); + for (File file : experimentalSettingsDir.listFiles()) { + experimentalSettingsFilesModel.addElement(new FileContainer(file)); + } + for (File file : provenSettingsDir.listFiles()) { + provenSettingsFilesModel.addElement(new FileContainer(file)); + + if (lastSettingsFile == null) { + lastSettingsFile = file; + } else { + if (file.getName().compareTo(lastSettingsFile.getName()) > 0) { + lastSettingsFile = file; + } + } + } + + JLabel lblES = new JLabel("Proven Settings"); + lblES.setBounds(600, 85, 320, 20); + contentPane.add(lblES); + JLabel lblPS = new JLabel("Experimental Settings"); + lblPS.setBounds(600, 235, 320, 20); + contentPane.add(lblPS); + + JList provenSettingsList = new JList(provenSettingsFilesModel); + provenSettingsList.setSelectionMode(ListSelectionModel.SINGLE_SELECTION); + provenSettingsList.setLayoutOrientation(JList.VERTICAL); + provenSettingsList.setVisibleRowCount(-1); + + JScrollPane provenListScroller = new JScrollPane(provenSettingsList); + provenListScroller.setPreferredSize(new Dimension(280, 120)); + provenListScroller.setBounds(600, 105, 320, 130); + contentPane.add(provenListScroller); + + JList experimentalSettingsList = new JList(experimentalSettingsFilesModel); + experimentalSettingsList.setSelectionMode(ListSelectionModel.SINGLE_SELECTION); + experimentalSettingsList.setLayoutOrientation(JList.VERTICAL); + experimentalSettingsList.setVisibleRowCount(-1); + + JScrollPane expListScroller = new JScrollPane(experimentalSettingsList); + expListScroller.setPreferredSize(new Dimension(280, 120)); + expListScroller.setBounds(600, 255, 320, 140); + contentPane.add(expListScroller); + + provenSettingsList.addMouseListener(new MouseAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + try { + provenSettingsList.getSelectedValue(); + loadSettings(((FileContainer) provenSettingsList.getSelectedValue()).file); + provenSettingsList.clearSelection(); + } catch (IOException ex) { + Logger.getLogger(OSEC.class.getName()).log(Level.SEVERE, null, ex); + } + provenSettingsList.clearSelection(); + updateDependiencies(false); + } + }); + experimentalSettingsList.addMouseListener(new MouseAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + try { + experimentalSettingsList.getSelectedValue(); + loadSettings(((FileContainer) experimentalSettingsList.getSelectedValue()).file); + experimentalSettingsList.clearSelection(); + } catch (IOException ex) { + Logger.getLogger(OSEC.class.getName()).log(Level.SEVERE, null, ex); + } + experimentalSettingsList.clearSelection(); + updateDependiencies(false); + } + }); + + JLabel lblNumberOfPas = new JLabel("No. of PAS Magnets"); + lblNumberOfPas.setBounds(15, 90, 141, 14); + contentPane.add(lblNumberOfPas); + + txtNumberOfPas = new JTextField(); + txtNumberOfPas.setText("16"); + txtNumberOfPas.setBounds(150, 90, 86, 20); + contentPane.add(txtNumberOfPas); + txtNumberOfPas.setColumns(10); + + JLabel lblWheelCircumference = new JLabel("Wheel circumference"); + lblWheelCircumference.setBounds(15, 110, 131, 14); + contentPane.add(lblWheelCircumference); + + txtWheelCircumference = new JTextField(); + txtWheelCircumference.setText("2000"); + txtWheelCircumference.setBounds(150, 110, 86, 20); + contentPane.add(txtWheelCircumference); + txtWheelCircumference.setColumns(10); + + JLabel lblPasTimeout = new JLabel("PAS Timeout"); + lblPasTimeout.setBounds(15, 130, 130, 14); + contentPane.add(lblPasTimeout); + + txtPasTimeout = new JTextField(); + txtPasTimeout.setText("3125"); + txtPasTimeout.setBounds(150, 130, 86, 20); + contentPane.add(txtPasTimeout); + txtPasTimeout.setColumns(10); + + lblThrottleMin = new JLabel("Throttle min"); + lblThrottleMin.setBounds(15, 150, 78, 14); + lblThrottleMin.setForeground(Color.GRAY); + contentPane.add(lblThrottleMin); + + txtThrottlemin = new JTextField(); + txtThrottlemin.setText("43"); + txtThrottlemin.setBounds(150, 150, 86, 20); + contentPane.add(txtThrottlemin); + txtThrottlemin.setColumns(10); + + lblThrottleMax = new JLabel("Throttle max"); + lblThrottleMax.setBounds(15, 170, 78, 14); + lblThrottleMax.setForeground(Color.GRAY); + contentPane.add(lblThrottleMax); + + txtThrottlemax = new JTextField(); + txtThrottlemax.setText("182"); + txtThrottlemax.setBounds(150, 170, 86, 20); + contentPane.add(txtThrottlemax); + txtThrottlemax.setColumns(10); + + JLabel lblBatteryCurrentMax = new JLabel("Battery Current max"); + lblBatteryCurrentMax.setForeground(Color.GRAY); + lblBatteryCurrentMax.setBounds(15, 190, 131, 14); + contentPane.add(lblBatteryCurrentMax); + + txtMaxbatterycurrent = new JTextField(); + txtMaxbatterycurrent.setText("372"); + txtMaxbatterycurrent.setBounds(150, 190, 86, 20); + contentPane.add(txtMaxbatterycurrent); + txtMaxbatterycurrent.setColumns(10); + + JLabel lblPhaseCurrentMax = new JLabel("Phase Current max"); + lblPhaseCurrentMax.setBounds(15, 210, 131, 14); + contentPane.add(lblPhaseCurrentMax); + + txtMaxphasecurrent = new JTextField(); + txtMaxphasecurrent.setText("372"); + txtMaxphasecurrent.setColumns(10); + txtMaxphasecurrent.setBounds(150, 210, 86, 20); + contentPane.add(txtMaxphasecurrent); + + JLabel lblRegenCurrentMax = new JLabel("Regen Current max"); + lblRegenCurrentMax.setForeground(Color.GRAY); + lblRegenCurrentMax.setBounds(15, 230, 131, 14); + contentPane.add(lblRegenCurrentMax); + + txtMaxregencurrent = new JTextField(); + txtMaxregencurrent.setText("282"); + txtMaxregencurrent.setColumns(10); + txtMaxregencurrent.setBounds(150, 230, 86, 20); + contentPane.add(txtMaxregencurrent); + + JLabel lblBatteryCurrentCal = new JLabel("Battery Current cal a"); + lblBatteryCurrentCal.setBounds(15, 250, 121, 14); + lblBatteryCurrentCal.setForeground(Color.GRAY); + contentPane.add(lblBatteryCurrentCal); + + txtBatteryCurcala = new JTextField(); + txtBatteryCurcala.setText("10"); + txtBatteryCurcala.setBounds(150, 250, 86, 20); + contentPane.add(txtBatteryCurcala); + txtBatteryCurcala.setColumns(10); + + JLabel lblGearRatio = new JLabel("Gear Ratio"); + lblGearRatio.setBounds(15, 270, 131, 14); + lblGearRatio.setForeground(Color.GRAY); + contentPane.add(lblGearRatio); + + + GearRatio = new JTextField(); + GearRatio.setText("64"); + GearRatio.setColumns(10); + GearRatio.setBounds(150, 270, 86, 20); + contentPane.add(GearRatio); + + JLabel lblUndervoltageLimit = new JLabel("Undervoltage"); + lblUndervoltageLimit.setBounds(15, 290, 121, 14); + lblUndervoltageLimit.setForeground(Color.GRAY); + contentPane.add(lblUndervoltageLimit); + + txtUndervoltage = new JTextField(); + txtUndervoltage.setText("127"); + txtUndervoltage.setBounds(150, 290, 86, 20); + contentPane.add(txtUndervoltage); + txtUndervoltage.setColumns(3); + + JLabel lblOvervoltageLimit = new JLabel("Overvoltage"); + lblOvervoltageLimit.setBounds(15, 310, 121, 14); + lblOvervoltageLimit.setForeground(Color.GRAY); + contentPane.add(lblOvervoltageLimit); + + txtOvervoltage = new JTextField(); + txtOvervoltage.setText("150"); + txtOvervoltage.setBounds(150, 310, 86, 20); + contentPane.add(txtOvervoltage); + txtOvervoltage.setColumns(3); + + JLabel lblBatVolCal = new JLabel("Voltage Calibration"); + lblBatVolCal.setBounds(15, 330, 121, 14); + lblBatVolCal.setForeground(Color.GRAY); + contentPane.add(lblBatVolCal); + + batteryVoltageCalib = new JTextField(); + batteryVoltageCalib.setText("70"); + batteryVoltageCalib.setColumns(2); + batteryVoltageCalib.setBounds(150, 330, 86, 20); + contentPane.add(batteryVoltageCalib); + + JLabel lblPfactor = new JLabel("Gain P"); + lblPfactor.setBounds(15, 350, 67, 14); + lblPfactor.setForeground(Color.GRAY); + contentPane.add(lblPfactor); + + p_factor = new JTextField(); + p_factor.setText("0.5"); + p_factor.setColumns(10); + p_factor.setBounds(150, 350, 86, 20); + contentPane.add(p_factor); + + JLabel lblIfactor = new JLabel("Gain I"); + lblIfactor.setForeground(Color.GRAY); + lblIfactor.setBounds(15, 370, 67, 14); + contentPane.add(lblIfactor); + + i_factor = new JTextField(); + i_factor.setText("0.2"); + i_factor.setColumns(10); + i_factor.setBounds(150, 370, 86, 20); + contentPane.add(i_factor); + + + JLabel lblPasThreshold = new JLabel("PAS threshold"); + lblPasThreshold.setBounds(15, 390, 86, 20); + lblPasThreshold.setForeground(Color.GRAY); + contentPane.add(lblPasThreshold); + + PAS_threshold = new JTextField(); + PAS_threshold.setText("1.7"); + PAS_threshold.setColumns(10); + PAS_threshold.setBounds(150, 390, 86, 20); + contentPane.add(PAS_threshold); + + JLabel lblAssistLevel = new JLabel("Assist Level 1"); + lblAssistLevel.setForeground(Color.GRAY); + lblAssistLevel.setBounds(250, 90, 80, 14); + contentPane.add(lblAssistLevel); + + Assist_Level_1 = new JTextField(); + Assist_Level_1.setText("20"); + Assist_Level_1.setColumns(10); + Assist_Level_1.setBounds(350, 90, 50, 20); + contentPane.add(Assist_Level_1); + + JLabel lblAssistLevel_1 = new JLabel("Assist Level 2"); + lblAssistLevel_1.setForeground(Color.GRAY); + lblAssistLevel_1.setBounds(250, 110, 80, 14); + contentPane.add(lblAssistLevel_1); + + Assist_Level_2 = new JTextField(); + Assist_Level_2.setText("40"); + Assist_Level_2.setColumns(10); + Assist_Level_2.setBounds(350, 110, 50, 20); + contentPane.add(Assist_Level_2); + + JLabel lblAssistLevel_2 = new JLabel("Assist Level 3"); + lblAssistLevel_2.setForeground(Color.GRAY); + lblAssistLevel_2.setBounds(250, 130, 80, 14); + contentPane.add(lblAssistLevel_2); + + Assist_Level_3 = new JTextField(); + Assist_Level_3.setText("60"); + Assist_Level_3.setColumns(10); + Assist_Level_3.setBounds(350, 130, 50, 20); + contentPane.add(Assist_Level_3); + + lblAssistLevel_3 = new JLabel("Assist Level 4"); + lblAssistLevel_3.setForeground(Color.GRAY); + lblAssistLevel_3.setBounds(250, 150, 80, 14); + contentPane.add(lblAssistLevel_3); + + Assist_Level_4 = new JTextField(); + Assist_Level_4.setText("80"); + Assist_Level_4.setColumns(10); + Assist_Level_4.setBounds(350, 150, 50, 20); + contentPane.add(Assist_Level_4); + + lblAssistLevel_4 = new JLabel("Assist Level 5"); + lblAssistLevel_4.setForeground(Color.GRAY); + lblAssistLevel_4.setBounds(250, 170, 80, 14); + contentPane.add(lblAssistLevel_4); + + Assist_Level_5 = new JTextField(); + Assist_Level_5.setText("100"); + Assist_Level_5.setColumns(10); + Assist_Level_5.setBounds(350, 170, 50, 20); + contentPane.add(Assist_Level_5); + + JLabel lblHallAngle4 = new JLabel("Hall angle 4"); + lblHallAngle4.setForeground(Color.GRAY); + lblHallAngle4.setBounds(415, 90, 100, 14); + contentPane.add(lblHallAngle4); + + txtAngle4 = new JTextField(); + txtAngle4.setText("0"); + txtAngle4.setColumns(10); + txtAngle4.setBounds(530, 90, 50, 20); + contentPane.add(txtAngle4); + + JLabel lblHallAngle6 = new JLabel("Hall angle 6"); + lblHallAngle6.setForeground(Color.GRAY); + lblHallAngle6.setBounds(415, 110, 100, 14); + contentPane.add(lblHallAngle6); + + txtAngle6 = new JTextField(); + txtAngle6.setText("42"); + txtAngle6.setColumns(10); + txtAngle6.setBounds(530, 110, 50, 20); + contentPane.add(txtAngle6); + + JLabel lblHallAngle2 = new JLabel("Hall angle 2"); + lblHallAngle2.setForeground(Color.GRAY); + lblHallAngle2.setBounds(415, 130, 100, 14); + contentPane.add(lblHallAngle2); + + txtAngle2 = new JTextField(); + txtAngle2.setText("85"); + txtAngle2.setColumns(10); + txtAngle2.setBounds(530, 130, 50, 20); + contentPane.add(txtAngle2); + + JLabel lblHallAngle3 = new JLabel("Hall angle 3"); + lblHallAngle3.setForeground(Color.GRAY); + lblHallAngle3.setBounds(415, 150, 100, 14); + contentPane.add(lblHallAngle3); + + txtAngle3 = new JTextField(); + txtAngle3.setText("127"); + txtAngle3.setColumns(10); + txtAngle3.setBounds(530, 150, 50, 20); + contentPane.add(txtAngle3); + + JLabel lblHallAngle1 = new JLabel("Hall angle 1"); + lblHallAngle1.setForeground(Color.GRAY); + lblHallAngle1.setBounds(415, 170, 100, 14); + contentPane.add(lblHallAngle1); + + txtAngle1 = new JTextField(); + txtAngle1.setText("170"); + txtAngle1.setColumns(10); + txtAngle1.setBounds(530, 170, 50, 20); + contentPane.add(txtAngle1); + + JLabel lblHallAngle5 = new JLabel("Hall angle 5"); + lblHallAngle5.setForeground(Color.GRAY); + lblHallAngle5.setBounds(415, 190, 100, 14); + contentPane.add(lblHallAngle5); + + txtAngle5 = new JTextField(); + txtAngle5.setText("212"); + txtAngle5.setColumns(10); + txtAngle5.setBounds(530, 190, 50, 20); + contentPane.add(txtAngle5); + + JLabel lblCorrectionAtAngle = new JLabel("Correction angle"); + lblCorrectionAtAngle.setBounds(415, 210, 100, 14); + lblCorrectionAtAngle.setForeground(Color.GRAY); + contentPane.add(lblCorrectionAtAngle); + + txtCorrectionAtAngle = new JTextField(); + txtCorrectionAtAngle.setText("127"); + txtCorrectionAtAngle.setColumns(10); + txtCorrectionAtAngle.setBounds(530, 210, 50, 20); + contentPane.add(txtCorrectionAtAngle); + + JLabel lblMotorSpecificAngle = new JLabel("Motor spec. angle"); + lblMotorSpecificAngle.setBounds(415, 230, 100, 14); + lblMotorSpecificAngle.setForeground(Color.GRAY); + contentPane.add(lblMotorSpecificAngle); + + txtMotor_specific_angle = new JTextField(); + txtMotor_specific_angle.setText("214"); + txtMotor_specific_angle.setBounds(530, 230, 50, 20); + contentPane.add(txtMotor_specific_angle); + txtMotor_specific_angle.setColumns(10); + + JList list = new JList(); + list.setBounds(441, 177, 1, 1); + contentPane.add(list); + + JLabel lblMorsetime = new JLabel("Morse-time 1"); + lblMorsetime.setBounds(250, 190, 80, 14); + contentPane.add(lblMorsetime); + + Morse_Time_1 = new JTextField(); + Morse_Time_1.setText("50"); + Morse_Time_1.setColumns(10); + Morse_Time_1.setBounds(350, 190, 50, 20); + contentPane.add(Morse_Time_1); + + JLabel lblMorsetime_1 = new JLabel("Morse-time 2"); + lblMorsetime_1.setBounds(250, 210, 80, 14); + contentPane.add(lblMorsetime_1); + + Morse_Time_2 = new JTextField(); + Morse_Time_2.setText("50"); + Morse_Time_2.setColumns(10); + Morse_Time_2.setBounds(350, 210, 50, 20); + contentPane.add(Morse_Time_2); + + JLabel lblMorsetime_2 = new JLabel("Morse-time 3"); + lblMorsetime_2.setBounds(250, 230, 80, 14); + contentPane.add(lblMorsetime_2); + + Morse_Time_3 = new JTextField(); + Morse_Time_3.setText("50"); + Morse_Time_3.setColumns(10); + Morse_Time_3.setBounds(350, 230, 50, 20); + contentPane.add(Morse_Time_3); + + JButton btnWiki = new JButton("Wiki Documentation"); + btnWiki.addActionListener(new ActionListener() { + public void actionPerformed(ActionEvent arg0) { + if (Desktop.isDesktopSupported()) { + try { + Desktop.getDesktop().browse(new URI("https://github.com/stancecoke/BMSBattery_S_controllers_firmware/wiki")); + } catch (Exception e) { + } + } + } + }); + btnWiki.setForeground(Color.RED); + btnWiki.setBounds(600, 20, 150, 29); + contentPane.add(btnWiki); + + JButton btnGit = new JButton("Git Repository"); + btnGit.addActionListener(new ActionListener() { + public void actionPerformed(ActionEvent arg0) { + if (Desktop.isDesktopSupported()) { + try { + Desktop.getDesktop().browse(new URI("https://github.com/stancecoke/BMSBattery_S_controllers_firmware")); + } catch (Exception e) { + } + } + } + }); + btnGit.setForeground(Color.BLUE); + btnGit.setBounds(770, 20, 150, 29); + contentPane.add(btnGit); + + JLabel lblTqCalibrationFactor = new JLabel("TQ Calib"); + lblTqCalibrationFactor.setBounds(250, 270, 80, 14); + lblTqCalibrationFactor.setForeground(Color.GRAY); + contentPane.add(lblTqCalibrationFactor); + + flt_tqCalibrationFactor = new JTextField(); + flt_tqCalibrationFactor.setText("1000.0"); + flt_tqCalibrationFactor.setColumns(10); + flt_tqCalibrationFactor.setBounds(350, 270, 50, 20); + contentPane.add(flt_tqCalibrationFactor); + + JLabel lblRampEnd = new JLabel("Ramp end"); + lblRampEnd.setBounds(250, 290, 80, 14); + lblRampEnd.setForeground(Color.GRAY); + contentPane.add(lblRampEnd); + + ramp_end = new JTextField(); + ramp_end.setText("2000"); + ramp_end.setColumns(10); + ramp_end.setBounds(350, 290, 50, 20); + contentPane.add(ramp_end); + + JLabel lblRampStart = new JLabel("Ramp start"); + lblRampStart.setBounds(250, 310, 80, 14); + lblRampStart.setForeground(Color.GRAY); + contentPane.add(lblRampStart); + + ramp_start = new JTextField(); + ramp_start.setText("7000"); + ramp_start.setColumns(10); + ramp_start.setBounds(350, 310, 50, 20); + contentPane.add(ramp_start); + + lblSpeedLimit = new JLabel("Limit (km/h)"); + lblSpeedLimit.setBounds(250, 350, 80, 14); + lblSpeedLimit.setForeground(Color.GRAY); + contentPane.add(lblSpeedLimit); + + txtSpeedlimit = new JTextField(); + txtSpeedlimit.setText("25"); + txtSpeedlimit.setBounds(350, 350, 50, 20); + contentPane.add(txtSpeedlimit); + txtSpeedlimit.setColumns(10); + + JLabel lblSpeedLimitwopas = new JLabel("Without PAS"); + lblSpeedLimitwopas.setBounds(250, 370, 80, 14); + lblSpeedLimitwopas.setForeground(Color.GRAY); + contentPane.add(lblSpeedLimitwopas); + + txtSpeedlimitWithoutPas = new JTextField(); + txtSpeedlimitWithoutPas.setText("6"); + txtSpeedlimitWithoutPas.setBounds(350, 370, 50, 20); + contentPane.add(txtSpeedlimitWithoutPas); + txtSpeedlimitWithoutPas.setColumns(10); + + JLabel lblSpeedLimitwto = new JLabel("Offroad"); + lblSpeedLimitwto.setBounds(250, 390, 80, 14); + lblSpeedLimitwto.setForeground(Color.GRAY); + contentPane.add(lblSpeedLimitwto); + + txtSpeedlimitWithThrottleOverride = new JTextField(); + txtSpeedlimitWithThrottleOverride.setText("25"); + txtSpeedlimitWithThrottleOverride.setBounds(350, 390, 50, 20); + contentPane.add(txtSpeedlimitWithThrottleOverride); + txtSpeedlimitWithThrottleOverride.setColumns(10); + + JLabel lblRideMode = new JLabel("Ride Options"); + lblRideMode.setFont(new Font("Tahoma", Font.BOLD, 12)); + lblRideMode.setBounds(15, 430, 86, 20); + lblRideMode.setForeground(Color.GRAY); + contentPane.add(lblRideMode); + + cbDynAssist = new JCheckBox("Dynamic Assist Level"); + cbDynAssist.setSelected(false); + cbDynAssist.setBounds(15, 455, 200, 20); + cbDynAssist.setForeground(Color.GRAY); + contentPane.add(cbDynAssist); + + cbCorrectionEnabled = new JCheckBox("Enable rotor angle correction"); + cbCorrectionEnabled.setSelected(false); + cbCorrectionEnabled.setBounds(15, 475, 200, 20); + cbCorrectionEnabled.setForeground(Color.GRAY); + contentPane.add(cbCorrectionEnabled); + + cbBypassLowSpeedRegenPiControl = new JCheckBox("Bypass PI control regen @low speed"); + cbBypassLowSpeedRegenPiControl.setSelected(false); + cbBypassLowSpeedRegenPiControl.setBounds(15, 495, 200, 20); + cbBypassLowSpeedRegenPiControl.setForeground(Color.GRAY); + contentPane.add(cbBypassLowSpeedRegenPiControl); + + cbTorqueSensor = new JCheckBox("Torquesensor"); + cbTorqueSensor.setSelected(false); + cbTorqueSensor.setForeground(Color.GRAY); + cbTorqueSensor.setBounds(15, 515, 200, 20); + contentPane.add(cbTorqueSensor); + + cbTorqueSensor.addItemListener(new ItemListener() { + @Override + public void itemStateChanged(ItemEvent e) { + updateDependiencies(true); + } + }); + + cbAssistLevelInfluencesThrottle = new JCheckBox("Assist Lvl affects Throttle"); + cbAssistLevelInfluencesThrottle.setSelected(false); + cbAssistLevelInfluencesThrottle.setBounds(15, 535, 200, 20); + cbAssistLevelInfluencesThrottle.setForeground(Color.GRAY); + contentPane.add(cbAssistLevelInfluencesThrottle); + + cbOffroadEnabled = new JCheckBox("Offroad Enabled"); + cbOffroadEnabled.setSelected(false); + cbOffroadEnabled.setBounds(15, 555, 200, 20); + cbOffroadEnabled.setForeground(Color.GRAY); + contentPane.add(cbOffroadEnabled); + + cbBrakeDisablesOffroad = new JCheckBox("Brake Disables Offroad"); + cbBrakeDisablesOffroad.setSelected(false); + cbBrakeDisablesOffroad.setBounds(15, 575, 200, 20); + cbBrakeDisablesOffroad.setForeground(Color.GRAY); + contentPane.add(cbBrakeDisablesOffroad); + + cbDigitalRegen = new JCheckBox("Regen Digital (no X4 throttle)"); + cbDigitalRegen.setSelected(false); + cbDigitalRegen.setBounds(15, 595, 200, 20); + cbDigitalRegen.setForeground(Color.GRAY); + contentPane.add(cbDigitalRegen); + + cbSpeedInfluencesRegen = new JCheckBox("Speed Influences Regen Rate"); + cbSpeedInfluencesRegen.setSelected(false); + cbSpeedInfluencesRegen.setBounds(15, 615, 200, 20); + cbSpeedInfluencesRegen.setForeground(Color.GRAY); + contentPane.add(cbSpeedInfluencesRegen); + + cbSpeedInfluencesTqSensor = new JCheckBox("Speed influences Tq Sensor"); + cbSpeedInfluencesTqSensor.setSelected(false); + cbSpeedInfluencesTqSensor.setBounds(15, 635, 200, 20); + cbSpeedInfluencesTqSensor.setForeground(Color.GRAY); + contentPane.add(cbSpeedInfluencesTqSensor); + + cbPasInverted = new JCheckBox("Pas inverted (right side)"); + cbPasInverted.setSelected(false); + cbPasInverted.setBounds(15, 655, 200, 20); + cbPasInverted.setForeground(Color.GRAY); + contentPane.add(cbPasInverted); + + cbPowerBasedControlEnabled = new JCheckBox("Power based control"); + cbPowerBasedControlEnabled.setSelected(false); + cbPowerBasedControlEnabled.setBounds(250, 455, 250, 20); + cbPowerBasedControlEnabled.setForeground(Color.GRAY); + contentPane.add(cbPowerBasedControlEnabled); + + cbIdleDisablesOffroad = new JCheckBox("Idle disables offroad"); + cbIdleDisablesOffroad.setSelected(false); + cbIdleDisablesOffroad.setBounds(250, 475, 250, 20); + cbIdleDisablesOffroad.setForeground(Color.GRAY); + contentPane.add(cbIdleDisablesOffroad); + + cbExternalSpeedSensor = new JCheckBox("External Speed-Sensor"); + cbExternalSpeedSensor.setSelected(false); + cbExternalSpeedSensor.setBounds(250, 495, 250, 20); + cbExternalSpeedSensor.setForeground(Color.GRAY); + contentPane.add(cbExternalSpeedSensor); + + cbSwitch360 = new JCheckBox("Switch to 360\u00B0 interpol. @ 75 ERPS"); + cbSwitch360.setSelected(false); + cbSwitch360.setBounds(250, 515, 250, 20); + cbSwitch360.setForeground(Color.GRAY); + contentPane.add(cbSwitch360); + + cbHighSpeedMotor = new JCheckBox("High Speed Motor"); + cbHighSpeedMotor.setSelected(false); + cbHighSpeedMotor.setBounds(250, 535, 250, 20); + cbHighSpeedMotor.setForeground(Color.GRAY); + contentPane.add(cbHighSpeedMotor); + + cbAntiJitter = new JCheckBox("Motor anti jitter (@60\u00B0 interpol.)"); + cbAntiJitter.setSelected(false); + cbAntiJitter.setBounds(250, 575, 250, 20); + cbAntiJitter.setForeground(Color.ORANGE); + contentPane.add(cbAntiJitter); + + cbDisableInterpolation = new JCheckBox("Disable interpolation"); + cbDisableInterpolation.setSelected(false); + cbDisableInterpolation.setBounds(250, 595, 250, 20); + cbDisableInterpolation.setForeground(Color.ORANGE); + contentPane.add(cbDisableInterpolation); + + cbDisable60DegInterpolation = new JCheckBox("Disable 60\u00B0 interpolation"); + cbDisable60DegInterpolation.setSelected(false); + cbDisable60DegInterpolation.setBounds(250, 615, 250, 20); + cbDisable60DegInterpolation.setForeground(Color.ORANGE); + contentPane.add(cbDisable60DegInterpolation); + + cbPwmOff = new JCheckBox("PWM off @coast (experimental)"); + cbPwmOff.setSelected(false); + cbPwmOff.setBounds(250, 635, 250, 20); + cbPwmOff.setForeground(Color.RED); + contentPane.add(cbPwmOff); + + cbDcNull = new JCheckBox("DC static zero (testing/experimental)"); + cbDcNull.setSelected(false); + cbDcNull.setBounds(250, 655, 250, 20); + cbDcNull.setForeground(Color.RED); + contentPane.add(cbDcNull); + + + lblDiplayType = new JLabel("Display Type"); + lblDiplayType.setFont(new Font("Tahoma", Font.BOLD, 12)); + lblDiplayType.setBounds(600, 400, 86, 20); + contentPane.add(lblDiplayType); + + rdbtnNodisplay = new JRadioButton("None"); + rdbtnNodisplay.setSelected(true); + displayButtonGroup.add(rdbtnNodisplay); + rdbtnNodisplay.setBounds(600, 425, 131, 20); + contentPane.add(rdbtnNodisplay); + + rdbtnKingmeterJlcd = new JRadioButton("Kingmeter J-LCD *"); + displayButtonGroup.add(rdbtnKingmeterJlcd); + rdbtnKingmeterJlcd.setBounds(600, 445, 131, 20); + contentPane.add(rdbtnKingmeterJlcd); + + rdbtnKtlcd = new JRadioButton("KT-LCD3"); + displayButtonGroup.add(rdbtnKtlcd); + rdbtnKtlcd.setBounds(750, 425, 131, 20); + contentPane.add(rdbtnKtlcd); + + rdbtnBluOsecDisplay = new JRadioButton("BluOsec App"); + displayButtonGroup.add(rdbtnBluOsecDisplay); + rdbtnBluOsecDisplay.setBounds(750, 445, 131, 20); + contentPane.add(rdbtnBluOsecDisplay); + + rdbtnDiganostics = new JRadioButton("Diagnostics"); + displayButtonGroup.add(rdbtnDiganostics); + rdbtnDiganostics.setBounds(750, 465, 131, 20); + contentPane.add(rdbtnDiganostics); + + cbResetEeprom = new JCheckBox("Write eeprom magic byte (will reset eeprom)"); + cbResetEeprom.setSelected(true); + cbResetEeprom.setBounds(600, 500, 300, 20); + contentPane.add(cbResetEeprom); + + JButton btnWriteoptionsbyte = new JButton("Unlock Controller"); + btnWriteoptionsbyte.setFont(new Font("Tahoma", Font.BOLD, 12)); + btnWriteoptionsbyte.setForeground(Color.RED); + btnWriteoptionsbyte.addActionListener(new ActionListener() { + public void actionPerformed(ActionEvent arg0) { + { + + int n = JOptionPane.showConfirmDialog( + null, + "If you run this function with a brand new controller, the original firmware will be erased. This can't be undone. Are you sure?", + "", + JOptionPane.YES_NO_OPTION); + + if (n == JOptionPane.YES_OPTION) { + try { + Process process = Runtime.getRuntime().exec("cmd /c start WriteOptionBytes"); + } catch (IOException e1) { + txtThrottlemin.setText("Error"); + e1.printStackTrace(); + } + } else { + JOptionPane.showMessageDialog(null, "Goodbye"); + } + + // Saving code here + } + } + }); + btnWriteoptionsbyte.setBounds(600, 580, 320, 40); + contentPane.add(btnWriteoptionsbyte); + + btnWriteConfiguration = new JButton("Write Configuration"); + btnWriteConfiguration.addActionListener(new ActionListener() { + + public void actionPerformed(ActionEvent arg0) { + PrintWriter pWriter = null; + PrintWriter iWriter = null; + try { + //FileWriter fw = new FileWriter("settings.ini"); + //BufferedWriter bw = new BufferedWriter(fw); + + File newFile = new File(experimentalSettingsDir + File.separator + new SimpleDateFormat("yyyyMMdd-HHmmssz").format(new Date()) + ".ini"); + experimentalSettingsFilesModel.add(0, new FileContainer(newFile)); + + iWriter = new PrintWriter(new BufferedWriter(new FileWriter(newFile))); + pWriter = new PrintWriter(new BufferedWriter(new FileWriter("config.h"))); + pWriter.println("/*\r\n" + + " * config.h\r\n" + + " *\r\n" + + " * Automatically created by OSEC Parameter Configurator\r\n" + + " * Author: stancecoke\r\n" + + " */\r\n" + + "\r\n" + + "#ifndef CONFIG_H_\r\n" + + "#define CONFIG_H_\r\n"); + + String text_to_save = "#define NUMBER_OF_PAS_MAGS " + txtNumberOfPas.getText(); + iWriter.println(txtNumberOfPas.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define limit " + txtSpeedlimit.getText(); + iWriter.println(txtSpeedlimit.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define timeout " + txtPasTimeout.getText(); + iWriter.println(txtPasTimeout.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define wheel_circumference " + txtWheelCircumference.getText() + "L"; + iWriter.println(txtWheelCircumference.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define limit_without_pas " + txtSpeedlimitWithoutPas.getText(); + iWriter.println(txtSpeedlimitWithoutPas.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define ADC_THROTTLE_MIN_VALUE " + txtThrottlemin.getText(); + iWriter.println(txtThrottlemin.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define ADC_THROTTLE_MAX_VALUE " + txtThrottlemax.getText(); + iWriter.println(txtThrottlemax.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define BATTERY_VOLTAGE_MIN_VALUE " + txtUndervoltage.getText(); + iWriter.println(txtUndervoltage.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define BATTERY_CURRENT_MAX_VALUE " + txtMaxbatterycurrent.getText() + "L"; + iWriter.println(txtMaxbatterycurrent.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define PHASE_CURRENT_MAX_VALUE " + txtMaxphasecurrent.getText() + "L"; + iWriter.println(txtMaxphasecurrent.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define REGEN_CURRENT_MAX_VALUE " + txtMaxregencurrent.getText() + "L"; + iWriter.println(txtMaxregencurrent.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT " + txtMotor_specific_angle.getText(); + iWriter.println(txtMotor_specific_angle.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define current_cal_a " + txtBatteryCurcala.getText(); + iWriter.println(txtBatteryCurcala.getText()); + pWriter.println(text_to_save); + + iWriter.println(""); + iWriter.println(""); + + text_to_save = "#define LEVEL_1 " + Assist_Level_1.getText(); + iWriter.println(Assist_Level_1.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define LEVEL_2 " + Assist_Level_2.getText(); + iWriter.println(Assist_Level_2.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define LEVEL_3 " + Assist_Level_3.getText(); + iWriter.println(Assist_Level_3.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define LEVEL_4 " + Assist_Level_4.getText(); + iWriter.println(Assist_Level_4.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define LEVEL_5 " + Assist_Level_5.getText(); + iWriter.println(Assist_Level_5.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define MORSE_TIME_1 " + Morse_Time_1.getText(); + iWriter.println(Morse_Time_1.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define MORSE_TIME_2 " + Morse_Time_2.getText(); + iWriter.println(Morse_Time_2.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define MORSE_TIME_3 " + Morse_Time_3.getText(); + iWriter.println(Morse_Time_3.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define RAMP_END " + ramp_end.getText(); + iWriter.println(ramp_end.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define P_FACTOR " + p_factor.getText(); + iWriter.println(p_factor.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define I_FACTOR " + i_factor.getText(); + iWriter.println(i_factor.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define GEAR_RATIO " + GearRatio.getText() + "L"; + iWriter.println(GearRatio.getText()); + pWriter.println(text_to_save); + + iWriter.println(""); + + text_to_save = "#define PAS_THRESHOLD " + PAS_threshold.getText(); + iWriter.println(PAS_threshold.getText()); + pWriter.println(text_to_save); + + iWriter.println(cbTorqueSensor.isSelected()); + + text_to_save = "#define RAMP_START " + ramp_start.getText(); + iWriter.println(ramp_start.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define limit_with_throttle_override " + txtSpeedlimitWithThrottleOverride.getText(); + iWriter.println(txtSpeedlimitWithThrottleOverride.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define CORRECTION_AT_ANGLE " + txtCorrectionAtAngle.getText(); + iWriter.println(txtCorrectionAtAngle.getText()); + pWriter.println(text_to_save); + + iWriter.println(""); + iWriter.println(""); + + if (rdbtnKtlcd.isSelected()) { + text_to_save = "#define DISPLAY_TYPE_KT_LCD3"; + pWriter.println(text_to_save); + } + iWriter.println(rdbtnKtlcd.isSelected()); + + if (rdbtnKingmeterJlcd.isSelected()) { + text_to_save = "#define DISPLAY_TYPE_KINGMETER_618U (1<<4) // King-Meter 618U protocol (KM5s, EBS-LCD2, J-LCD, SW-LCD)"; + pWriter.println(text_to_save); + text_to_save = "#define DISPLAY_TYPE_KINGMETER (DISPLAY_TYPE_KINGMETER_618U|DISPLAY_TYPE_KINGMETER_901U)"; + pWriter.println(text_to_save); + text_to_save = "#define DISPLAY_TYPE DISPLAY_TYPE_KINGMETER // Set your display type here. CHANGES ONLY HERE!"; + pWriter.println(text_to_save); + } + iWriter.println(rdbtnKingmeterJlcd.isSelected()); + + iWriter.println(""); + iWriter.println(""); + + if (rdbtnDiganostics.isSelected()) { + text_to_save = "#define DIAGNOSTICS"; + pWriter.println(text_to_save); + } + iWriter.println(rdbtnDiganostics.isSelected()); + + text_to_save = "#define ANGLE_4_0 " + txtAngle4.getText(); + iWriter.println(txtAngle4.getText()); + pWriter.println(text_to_save); + text_to_save = "#define ANGLE_6_60 " + txtAngle6.getText(); + iWriter.println(txtAngle6.getText()); + pWriter.println(text_to_save); + text_to_save = "#define ANGLE_2_120 " + txtAngle2.getText(); + iWriter.println(txtAngle2.getText()); + pWriter.println(text_to_save); + text_to_save = "#define ANGLE_3_180 " + txtAngle3.getText(); + iWriter.println(txtAngle3.getText()); + pWriter.println(text_to_save); + + if (rdbtnBluOsecDisplay.isSelected()) { + text_to_save = "#define BLUOSEC"; + pWriter.println(text_to_save); + } + iWriter.println(rdbtnBluOsecDisplay.isSelected()); + + iWriter.println(rdbtnNodisplay.isSelected()); + + text_to_save = "#define ANGLE_1_240 " + txtAngle1.getText(); + iWriter.println(txtAngle1.getText()); + pWriter.println(text_to_save); + text_to_save = "#define ANGLE_5_300 " + txtAngle5.getText(); + iWriter.println(txtAngle5.getText()); + pWriter.println(text_to_save); + + text_to_save = "#define TQS_CALIB " + flt_tqCalibrationFactor.getText(); + iWriter.println(flt_tqCalibrationFactor.getText()); + pWriter.println(text_to_save); + + int acaFlags = 128; + acaFlags |= (cbAssistLevelInfluencesThrottle.isSelected() ? 1 : 0); + acaFlags |= (cbOffroadEnabled.isSelected() ? 2 : 0); + acaFlags |= (cbBrakeDisablesOffroad.isSelected() ? 4 : 0); + acaFlags |= (cbDigitalRegen.isSelected() ? 8 : 0); + acaFlags |= (cbSpeedInfluencesRegen.isSelected() ? 16 : 0); + acaFlags |= (cbSpeedInfluencesTqSensor.isSelected() ? 32 : 0); + acaFlags |= (cbPasInverted.isSelected() ? 64 : 0); + + acaFlags |= (cbBypassLowSpeedRegenPiControl.isSelected() ? 256 : 0); + + acaFlags |= (cbDynAssist.isSelected() ? 512 : 0); + acaFlags |= (cbPowerBasedControlEnabled.isSelected() ? 1024 : 0); + acaFlags |= (cbTorqueSensor.isSelected() ? 2048 : 0); + acaFlags |= (cbCorrectionEnabled.isSelected() ? 4096 : 0); + acaFlags |= (cbIdleDisablesOffroad.isSelected() ? 16384 : 0); + acaFlags |= (cbExternalSpeedSensor.isSelected() ? 8192 : 0); + + iWriter.println(acaFlags); + + pWriter.println("#define ACA " + acaFlags); + + if (!cbResetEeprom.isSelected()) { + pWriter.println("#define EEPROM_NOINIT // eeprom will not be cleared"); + } + pWriter.println("#define EEPROM_INIT_MAGIC_BYTE " + (System.currentTimeMillis() % 256) + " // makes sure (chance of fail 1/255) eeprom is invalidated after flashing new config"); + + text_to_save = "#define ADC_BATTERY_VOLTAGE_K " + batteryVoltageCalib.getText(); + iWriter.println(batteryVoltageCalib.getText()); + pWriter.println(text_to_save); + + int acaExperimentalFlags = 128; + acaExperimentalFlags |= (cbDcNull.isSelected() ? 1 : 0); + acaExperimentalFlags |= (cbAntiJitter.isSelected() ? 2 : 0); + acaExperimentalFlags |= (cbHighSpeedMotor.isSelected() ? 256 : 0); + acaExperimentalFlags |= (cbSwitch360.isSelected() ? 16 : 0); + acaExperimentalFlags |= (cbDisableInterpolation.isSelected() ? 4 : 0); + acaExperimentalFlags |= (cbDisable60DegInterpolation.isSelected() ? 8 : 0); + acaExperimentalFlags |= (cbPwmOff.isSelected() ? 1024 : 0); + iWriter.println(acaExperimentalFlags); + pWriter.println("#define ACA_EXPERIMENTAL " + acaExperimentalFlags); + + text_to_save = "#define BATTERY_VOLTAGE_MAX_VALUE " + txtOvervoltage.getText(); + iWriter.println(txtOvervoltage.getText()); + pWriter.println(text_to_save); + + pWriter.println("\r\n#endif /* CONFIG_H_ */"); + + iWriter.close(); + + } catch (IOException ioe) { + ioe.printStackTrace(); + } finally { + if (pWriter != null) { + pWriter.flush(); + pWriter.close(); + + } + } + try { + Process process = Runtime.getRuntime().exec("cmd /c start Start_Compiling"); + } catch (IOException e1) { + txtThrottlemin.setText("Error"); + e1.printStackTrace(); + } + } + }); + btnWriteConfiguration.setFont(new Font("Tahoma", Font.BOLD, 12)); + btnWriteConfiguration.setForeground(Color.BLUE); + btnWriteConfiguration.setBounds(600, 520, 320, 40); + contentPane.add(btnWriteConfiguration); + + if (lastSettingsFile != null) { + try { + loadSettings(lastSettingsFile); + } catch (Exception ex) { + + } + provenSettingsList.clearSelection(); + experimentalSettingsList.clearSelection(); + updateDependiencies(false); + } + } +} diff --git a/tools/OSEC Parameter Configurator.java b/tools/OSEC Parameter Configurator.java deleted file mode 100644 index b247b3b9..00000000 --- a/tools/OSEC Parameter Configurator.java +++ /dev/null @@ -1,326 +0,0 @@ -import java.awt.BorderLayout; -import java.awt.EventQueue; -import java.io.*; -import java.awt.Desktop; -import java.net.URI; -import java.net.URISyntaxException; - -import javax.swing.JFrame; -import javax.swing.JPanel; -import javax.swing.border.EmptyBorder; -import javax.swing.text.JTextComponent; -import javax.swing.JTextField; -import javax.swing.JButton; -import javax.swing.JLabel; -import javax.swing.AbstractAction; -import java.awt.event.ActionEvent; -import javax.swing.Action; -import java.awt.event.ActionListener; -import java.awt.GridLayout; -import java.awt.Font; -import java.awt.Color; -import java.io.BufferedWriter; -import java.io.FileWriter; -import java.io.IOException; -import java.io.PrintWriter; -import com.jgoodies.forms.factories.DefaultComponentFactory; -import javax.swing.SwingConstants; -import javax.swing.JOptionPane; -import javax.swing.JRadioButton; -import javax.swing.ButtonGroup; - -public class ErsterVersuch extends JFrame { - - private JPanel contentPane; - private JTextField txtPasTimeout; - private JTextField txtWheelCircumference; - private JTextField txtNumberOfPas; - private JLabel lblSpeedLimit; - private JTextField txtSpeedlimit; - private JLabel lblSupportFactor; - private JTextField txtSupportfactor; - private JLabel lblThrottleMin; - private JTextField txtThrottlemin; - private JLabel lblThrottleMax; - private JTextField txtThrottlemax; - private JButton lblHttpsopensourceebikefirmwarebitbucketio; - private JLabel lblOpenSourceFirmware; - private JButton btnWriteConfiguration; - private JTextField txtMaxbatterycurrent; - private JTextField txtUndervoltage; - private final ButtonGroup Ridingmode = new ButtonGroup(); - - - - /** - * Launch the application. - */ - public static void main(String[] args) { - EventQueue.invokeLater(new Runnable() { - public void run() { - try { - ErsterVersuch frame = new ErsterVersuch(); - frame.setVisible(true); - } catch (Exception e) { - e.printStackTrace(); - } - } - }); - } - - /** - * Create the frame. - */ - public ErsterVersuch() { - setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - setBounds(100, 100, 452, 570); - contentPane = new JPanel(); - contentPane.setBorder(new EmptyBorder(5, 5, 5, 5)); - setContentPane(contentPane); - contentPane.setLayout(null); - - JLabel lblTollesProgramm = new JLabel("OSEC Parameter Configurator"); - lblTollesProgramm.setForeground(Color.BLUE); - lblTollesProgramm.setFont(new Font("Tahoma", Font.PLAIN, 18)); - lblTollesProgramm.setBounds(5, 16, 255, 34); - contentPane.add(lblTollesProgramm); - - JLabel lblPasTimeout = new JLabel("PAS Timeout"); - lblPasTimeout.setBounds(5, 167, 101, 14); - contentPane.add(lblPasTimeout); - - txtPasTimeout = new JTextField(); - txtPasTimeout.setText("3125"); - txtPasTimeout.setBounds(150, 165, 86, 20); - contentPane.add(txtPasTimeout); - txtPasTimeout.setColumns(10); - - JLabel lblWheelCircumference = new JLabel("Wheel circumference"); - lblWheelCircumference.setBounds(5, 130, 131, 14); - contentPane.add(lblWheelCircumference); - - txtWheelCircumference = new JTextField(); - txtWheelCircumference.setText("2000"); - txtWheelCircumference.setBounds(150, 128, 86, 20); - contentPane.add(txtWheelCircumference); - txtWheelCircumference.setColumns(10); - - JLabel lblNumberOfPas = new JLabel("Number of PAS Magnets"); - lblNumberOfPas.setBounds(5, 92, 141, 14); - contentPane.add(lblNumberOfPas); - - txtNumberOfPas = new JTextField(); - txtNumberOfPas.setText("16"); - txtNumberOfPas.setBounds(150, 87, 86, 20); - contentPane.add(txtNumberOfPas); - txtNumberOfPas.setColumns(10); - - lblSpeedLimit = new JLabel("Speed Limit (km/h)"); - lblSpeedLimit.setBounds(5, 209, 135, 14); - contentPane.add(lblSpeedLimit); - - txtSpeedlimit = new JTextField(); - txtSpeedlimit.setText("25"); - txtSpeedlimit.setBounds(150, 207, 86, 20); - contentPane.add(txtSpeedlimit); - txtSpeedlimit.setColumns(10); - - lblSupportFactor = new JLabel("Assist factor"); - lblSupportFactor.setBounds(5, 248, 101, 14); - contentPane.add(lblSupportFactor); - - txtSupportfactor = new JTextField(); - txtSupportfactor.setText("64"); - txtSupportfactor.setBounds(150, 245, 86, 20); - contentPane.add(txtSupportfactor); - txtSupportfactor.setColumns(10); - - lblThrottleMin = new JLabel("Throttle min"); - lblThrottleMin.setBounds(5, 291, 78, 14); - contentPane.add(lblThrottleMin); - - txtThrottlemin = new JTextField(); - txtThrottlemin.setText("43"); - txtThrottlemin.setBounds(150, 288, 86, 20); - contentPane.add(txtThrottlemin); - txtThrottlemin.setColumns(10); - - lblThrottleMax = new JLabel("Throttle max"); - lblThrottleMax.setBounds(5, 335, 78, 14); - contentPane.add(lblThrottleMax); - - txtThrottlemax = new JTextField(); - txtThrottlemax.setText("182"); - txtThrottlemax.setBounds(150, 332, 86, 20); - contentPane.add(txtThrottlemax); - txtThrottlemax.setColumns(10); - - lblHttpsopensourceebikefirmwarebitbucketio = new JButton("https://opensourceebikefirmware.bitbucket.io/"); - lblHttpsopensourceebikefirmwarebitbucketio.addActionListener(new ActionListener() { - public void actionPerformed(ActionEvent arg0) { - if (Desktop.isDesktopSupported()) { - try { - try { - Desktop.getDesktop().browse(new URI("https://opensourceebikefirmware.bitbucket.io/")); - } catch (URISyntaxException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } catch (IOException e) { /* TODO: error handling */ } - } else { /* TODO: error handling */ } - - } - }); - lblHttpsopensourceebikefirmwarebitbucketio.setForeground(Color.BLUE); - lblHttpsopensourceebikefirmwarebitbucketio.setBounds(65, 476, 309, 29); - contentPane.add(lblHttpsopensourceebikefirmwarebitbucketio); - - lblOpenSourceFirmware = new JLabel("Open Source Firmware for E-Bike Controller"); - lblOpenSourceFirmware.setFont(new Font("Tahoma", Font.PLAIN, 11)); - lblOpenSourceFirmware.setForeground(Color.BLUE); - lblOpenSourceFirmware.setBounds(5, 47, 255, 14); - contentPane.add(lblOpenSourceFirmware); - - txtMaxbatterycurrent = new JTextField(); - txtMaxbatterycurrent.setText("254"); - txtMaxbatterycurrent.setBounds(150, 371, 86, 20); - contentPane.add(txtMaxbatterycurrent); - txtMaxbatterycurrent.setColumns(10); - - JLabel lblBatteryCurrentMax = new JLabel("Battery Current max"); - lblBatteryCurrentMax.setBounds(5, 374, 131, 14); - contentPane.add(lblBatteryCurrentMax); - - txtUndervoltage = new JTextField(); - txtUndervoltage.setText("127"); - txtUndervoltage.setBounds(150, 404, 86, 20); - contentPane.add(txtUndervoltage); - txtUndervoltage.setColumns(10); - - JLabel lblUndervoltageLimit = new JLabel("Undervoltage limit"); - lblUndervoltageLimit.setBounds(5, 407, 121, 14); - contentPane.add(lblUndervoltageLimit); - - JRadioButton rdbtnThrottle = new JRadioButton("Throttle"); - Ridingmode.add(rdbtnThrottle); - rdbtnThrottle.setBounds(259, 220, 109, 23); - contentPane.add(rdbtnThrottle); - - JRadioButton rdbtnThrottlePas = new JRadioButton("Throttle and PAS"); - Ridingmode.add(rdbtnThrottlePas); - rdbtnThrottlePas.setBounds(259, 244, 149, 23); - contentPane.add(rdbtnThrottlePas); - - JRadioButton rdbtnTorqueSensor = new JRadioButton("Torquesensor"); - rdbtnTorqueSensor.setSelected(true); - Ridingmode.add(rdbtnTorqueSensor); - rdbtnTorqueSensor.setBounds(259, 270, 144, 23); - contentPane.add(rdbtnTorqueSensor); - - JButton btnWriteoptionsbyte = new JButton("Write Option Bytes"); - btnWriteoptionsbyte.setFont(new Font("Tahoma", Font.BOLD, 12)); - btnWriteoptionsbyte.setForeground(Color.BLUE); - btnWriteoptionsbyte.addActionListener(new ActionListener() { - public void actionPerformed(ActionEvent arg0) { - { - - int n = JOptionPane.showConfirmDialog( - null, - "If you run this function with a brand new controller, the original firmware will be erased. This can't be undone. Are you sure?" , - "", - JOptionPane.YES_NO_OPTION); - - if(n == JOptionPane.YES_OPTION) - { - try { - Process process = Runtime.getRuntime().exec("cmd /c start WriteOptionBytes"); - } catch (IOException e1) { - txtThrottlemin.setText("Error"); - e1.printStackTrace(); - } - } - else - { - JOptionPane.showMessageDialog(null, "Goodbye"); - } - - // Saving code here - } - } - }); - btnWriteoptionsbyte.setBounds(259, 94, 167, 51); - contentPane.add(btnWriteoptionsbyte); - - btnWriteConfiguration = new JButton("Write Configuration"); - btnWriteConfiguration.addActionListener(new ActionListener() { - - public void actionPerformed(ActionEvent arg0) { - PrintWriter pWriter = null; - try { - pWriter = new PrintWriter(new BufferedWriter(new FileWriter("config.h"))); - pWriter.println("/*\r\n" + - " * config.h\r\n" + - " *\r\n" + - " * Automatically created by OSEC Parameter Configurator\r\n" + - " * Author: stancecoke\r\n" + - " */\r\n" + - "\r\n" + - "#ifndef CONFIG_H_\r\n" + - "#define CONFIG_H_\r\n"); - String text_to_save = "#define limit " + txtSpeedlimit.getText(); - pWriter.println(text_to_save); - text_to_save = "#define timeout " + txtPasTimeout.getText(); - pWriter.println(text_to_save); - text_to_save = "#define wheel_circumference " + txtWheelCircumference.getText()+"L"; - pWriter.println(text_to_save); - text_to_save = "#define fummelfaktor " + txtSupportfactor.getText()+"L"; - pWriter.println(text_to_save); - text_to_save = "#define ADC_THROTTLE_MIN_VALUE " + txtThrottlemin.getText(); - pWriter.println(text_to_save); - text_to_save = "#define ADC_THROTTLE_MAX_VALUE " + txtThrottlemax.getText(); - pWriter.println(text_to_save); - text_to_save = "#define BATTERY_VOLTAGE_MIN_VALUE " + txtUndervoltage.getText(); - pWriter.println(text_to_save); - text_to_save = "#define BATTERY_CURRENT_MAX_VALUE " + txtMaxbatterycurrent.getText(); - pWriter.println(text_to_save); - if (rdbtnTorqueSensor.isSelected()){ - text_to_save = "#define TORQUESENSOR"; - pWriter.println(text_to_save); - } - if (rdbtnThrottlePas.isSelected()){ - text_to_save = "#define THROTTLEANDPAS"; - pWriter.println(text_to_save); - } - if (rdbtnThrottle.isSelected()){ - text_to_save = "#define THROTTLE"; - pWriter.println(text_to_save); - } - pWriter.println("\r\n#endif /* CONFIG_H_ */"); - - - } catch (IOException ioe) { - ioe.printStackTrace(); - } finally { - if (pWriter != null){ - pWriter.flush(); - pWriter.close(); - } - } - try { - Process process = Runtime.getRuntime().exec("cmd /c start Start_Compiling"); - } catch (IOException e1) { - txtThrottlemin.setText("Error"); - e1.printStackTrace(); - } - } - }); - btnWriteConfiguration.setFont(new Font("Tahoma", Font.BOLD, 12)); - btnWriteConfiguration.setForeground(Color.BLUE); - btnWriteConfiguration.setBounds(259, 25, 167, 58); - contentPane.add(btnWriteConfiguration); - - - - - } -} diff --git a/tools/cygwin/bin/cyggcc_s-seh-1.dll b/tools/cygwin/bin/cyggcc_s-seh-1.dll new file mode 100644 index 00000000..27721e2d Binary files /dev/null and b/tools/cygwin/bin/cyggcc_s-seh-1.dll differ diff --git a/tools/cygwin/bin/cygiconv-2.dll b/tools/cygwin/bin/cygiconv-2.dll new file mode 100644 index 00000000..f1de05dd Binary files /dev/null and b/tools/cygwin/bin/cygiconv-2.dll differ diff --git a/tools/cygwin/bin/cygintl-8.dll b/tools/cygwin/bin/cygintl-8.dll new file mode 100644 index 00000000..d8e8f510 Binary files /dev/null and b/tools/cygwin/bin/cygintl-8.dll differ diff --git a/tools/cygwin/bin/cygncursesw-10.dll b/tools/cygwin/bin/cygncursesw-10.dll new file mode 100644 index 00000000..16f44046 Binary files /dev/null and b/tools/cygwin/bin/cygncursesw-10.dll differ diff --git a/tools/cygwin/bin/cygreadline7.dll b/tools/cygwin/bin/cygreadline7.dll new file mode 100644 index 00000000..2503fd45 Binary files /dev/null and b/tools/cygwin/bin/cygreadline7.dll differ diff --git a/tools/cygwin/bin/cygstdc++-6.dll b/tools/cygwin/bin/cygstdc++-6.dll new file mode 100644 index 00000000..d0cac92e Binary files /dev/null and b/tools/cygwin/bin/cygstdc++-6.dll differ diff --git a/tools/cygwin/bin/cygwin1.dll b/tools/cygwin/bin/cygwin1.dll new file mode 100644 index 00000000..e2a5919a Binary files /dev/null and b/tools/cygwin/bin/cygwin1.dll differ diff --git a/tools/cygwin/bin/libiconv2.dll b/tools/cygwin/bin/libiconv2.dll new file mode 100644 index 00000000..747073f1 Binary files /dev/null and b/tools/cygwin/bin/libiconv2.dll differ diff --git a/tools/cygwin/bin/libintl3.dll b/tools/cygwin/bin/libintl3.dll new file mode 100644 index 00000000..ec11e6b1 Binary files /dev/null and b/tools/cygwin/bin/libintl3.dll differ diff --git a/tools/cygwin/bin/make.exe b/tools/cygwin/bin/make.exe new file mode 100644 index 00000000..58d49e3c Binary files /dev/null and b/tools/cygwin/bin/make.exe differ diff --git a/tools/cygwin/bin/sh.exe b/tools/cygwin/bin/sh.exe new file mode 100644 index 00000000..69688fc0 Binary files /dev/null and b/tools/cygwin/bin/sh.exe differ diff --git a/tools/cygwin/tmp/dummy_file.txt b/tools/cygwin/tmp/dummy_file.txt new file mode 100644 index 00000000..a8e7226c --- /dev/null +++ b/tools/cygwin/tmp/dummy_file.txt @@ -0,0 +1 @@ +This folder can't be empty \ No newline at end of file diff --git a/uart.c b/uart.c index 6a9f24b5..edbdbcef 100755 --- a/uart.c +++ b/uart.c @@ -8,80 +8,163 @@ #include #include - +#include "uart.h" #include "stm8s.h" #include "stm8s_uart2.h" -#include "motor.h" +#include "interrupts.h" +#include "ACAcontrollerState.h" + +static uint8_t ui8_uarx_buffer[UART_RINGBUFFER_SIZE]; +static uint8_t ui8_uatx_buffer[UART_RINGBUFFER_SIZE]; +static uint8_t ui8_rx_fillpointer = 0; +static uint8_t ui8_tx_fillpointer = 0; +static uint8_t ui8_rx_digestpointer = 0; +static uint8_t ui8_tx_digestpointer = 0; + +static uint8_t ui8_rx_packet_start_pos = 0; + +void uart_put_buffered(uint8_t c) { + ui8_uatx_buffer[ui8_tx_fillpointer++] = c; + if (ui8_tx_fillpointer >= UART_RINGBUFFER_SIZE) { + ui8_tx_fillpointer = 0; + } +} -void uart_init (void) -{ - UART2_DeInit(); - UART2_Init((uint32_t)115200, - UART2_WORDLENGTH_8D, - UART2_STOPBITS_1, - UART2_PARITY_NO, - UART2_SYNCMODE_CLOCK_DISABLE, - UART2_MODE_TXRX_ENABLE); +uint8_t uart_get_packet_start_pos(void) { + return ui8_rx_packet_start_pos; +} + +uint8_t byte_avail_at_position(void) { + if (ui8_rx_fillpointer != ui8_rx_digestpointer) { + return ui8_rx_digestpointer; + } + return UART_EMPTY_INDICATOR; +} + +uint8_t uart_get_buffered(void) { + uint8_t c = ui8_uarx_buffer[ui8_rx_digestpointer++]; + if (ui8_rx_digestpointer >= UART_RINGBUFFER_SIZE) { + ui8_rx_digestpointer = 0; + } + return c; +} +// FIXME only needed as long as the other displays do not use new functions +// and instead have their own irq handler + +void UART2_IRQHandler(void) __interrupt(UART2_IRQHANDLER) { + + // if there is something to read, read it into cyclic buffer + if (UART2_GetFlagStatus(UART2_FLAG_RXNE) == SET) { + // assume data packet timeout after 20ms / new packet starting + if (ui16_time_ticks_for_uart_timeout > UART_PACKET_TIMEOUT_TICKS) { + ui8_rx_packet_start_pos = ui8_rx_fillpointer; + } + ui16_time_ticks_for_uart_timeout=0; + + ui8_uarx_buffer[ui8_rx_fillpointer++] = UART2_ReceiveData8(); + if (ui8_rx_fillpointer >= UART_RINGBUFFER_SIZE) { + ui8_rx_fillpointer = 0; + } + + } else { + + // catch errors and process them in a reasonable manner which is always to ignore them :) + if (UART2_GetITStatus(UART2_IT_IDLE) == SET) { + UART2_ReceiveData8(); // -> clear! + } + if (UART2_GetITStatus(UART2_IT_LBDF) == SET) { + UART2_ReceiveData8(); // -> clear! + } + if (UART2_GetITStatus(UART2_IT_OR) == SET) { + UART2_ReceiveData8(); // -> clear! + } + if (UART2_GetITStatus(UART2_IT_PE) == SET) { + UART2_ReceiveData8(); // -> clear! + } + } +} + +void uart_send_if_avail() { + // if there is something to send and we are not already sending + if ((ui8_tx_fillpointer != ui8_tx_digestpointer)&&(UART2_GetFlagStatus(UART2_FLAG_TXE) != RESET)) { + UART2_SendData8(ui8_uatx_buffer[ui8_tx_digestpointer++]); + if (ui8_tx_digestpointer >= UART_RINGBUFFER_SIZE) { + ui8_tx_digestpointer = 0; + } + } +} + +void uart_fill_rx_packet_buffer(uint8_t *buffer, uint8_t bufferSize, uint8_t *bufferPos) { + while (byte_avail_at_position() != UART_EMPTY_INDICATOR) { + + uint8_t uart_packet_start_pos = uart_get_packet_start_pos(); + uint8_t byte_avail_at_pos = byte_avail_at_position(); + + if ((*bufferPos == 0) && (uart_packet_start_pos != byte_avail_at_pos)) { + uart_get_buffered(); //discard incomplete chunks + } else if (*bufferPos < bufferSize) { + *(buffer + (*bufferPos)) = uart_get_buffered(); + (*bufferPos)++; + } + } +} + +void uart_init(void) { + UART2_DeInit(); + UART2_Init((uint32_t) 9600, + UART2_WORDLENGTH_8D, + UART2_STOPBITS_1, + UART2_PARITY_NO, + UART2_SYNCMODE_CLOCK_DISABLE, + UART2_MODE_TXRX_ENABLE); + UART2_ITConfig(UART2_IT_RXNE_OR, ENABLE); } #if __SDCC_REVISION < 9624 -void putchar(char c) -{ - //Write a character to the UART2 - UART2_SendData8(c); - //Loop until the end of transmission - while (UART2_GetFlagStatus(UART2_FLAG_TXE) == RESET); +void putchar(char c) { + //Write a character to the UART2 + UART2_SendData8(c); + + //Loop until the end of transmission + while (UART2_GetFlagStatus(UART2_FLAG_TXE) == RESET); +} + +void putbyte(uint8_t c) { + //Write a character to the UART2 + UART2_SendData8(c); + + //Loop until the end of transmission + while (UART2_GetFlagStatus(UART2_FLAG_TXE) == RESET); } #else -int putchar(int c) -{ - //Write a character to the UART2 - UART2_SendData8(c); - //Loop until the end of transmission - while (UART2_GetFlagStatus(UART2_FLAG_TXE) == RESET); +int putchar(int c) { + //Write a character to the UART2 + UART2_SendData8(c); + + //Loop until the end of transmission + while (UART2_GetFlagStatus(UART2_FLAG_TXE) == RESET); - return((unsigned char)c); + return ((unsigned char) c); } #endif #if __SDCC_REVISION < 9989 char getchar(void) #else + int getchar(void) #endif { - uint8_t c = 0; - - /* Loop until the Read data register flag is SET */ - while (UART2_GetFlagStatus(UART2_FLAG_RXNE) == RESET) ; - - c = UART2_ReceiveData8(); - - return (c); -} - -char getchar1(void) -{ - uint8_t c = 0; + uint8_t c = 0; - if (UART2_GetFlagStatus(UART2_FLAG_RXNE) == RESET) - { - return 255; - } - c = UART2_ReceiveData8(); - if (c == '0') - { - ui8_position_correction_value--; - } + /* Loop until the Read data register flag is SET */ + while (UART2_GetFlagStatus(UART2_FLAG_RXNE) == RESET); - if (c == '1') - { - ui8_position_correction_value++; - } + c = UART2_ReceiveData8(); - return (c); + return (c); } diff --git a/uart.h b/uart.h index ebef4b8a..f8bab6e3 100755 --- a/uart.h +++ b/uart.h @@ -9,9 +9,20 @@ #ifndef _UART_H #define _UART_H -#include "main.h" +#include + +#define UART_RINGBUFFER_SIZE 128 +#define UART_EMPTY_INDICATOR 255 +#define UART_PACKET_TIMEOUT_TICKS 312 // 20000 uS / 64 uS + +uint8_t uart_get_packet_start_pos(void); +uint8_t byte_avail_at_position(void); +uint8_t uart_get_buffered(void); +void uart_put_buffered(uint8_t c); +void uart_send_if_avail(); +void uart_fill_rx_packet_buffer(uint8_t *buffer, uint8_t bufferSize, uint8_t *bufferPos); void uart_init (void); -char getchar1(void); +void putbyte(uint8_t c); #endif /* _UART_H */ diff --git a/update_setpoint.c b/update_setpoint.c deleted file mode 100644 index d0da1ff1..00000000 --- a/update_setpoint.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * OpenSource EBike firmware - * - * Copyright (C) Stancecoke, 2017. - * - * Released under the GPL License, Version 3 - */ - -#include -#include -#include "stm8s.h" -#include "stm8s_it.h" -#include "gpio.h" -#include "main.h" -#include "interrupts.h" -#include "PAS.h" -#include "cruise_control.h" -#include "motor.h" -#include "pwm.h" -#include "adc.h" -#include "update_setpoint.h" -#include "config.h" - - -static uint32_t ui32_setpoint; -static uint32_t ui32_SPEED_km_h; -static int16_t i16_delta; -int8_t uint_PWM_Enable=0; - -uint16_t update_setpoint (uint16_t speed, uint16_t PAS, uint16_t sumtorque, uint16_t setpoint_old) -{ - - if(ui8_BatteryVoltagetimeout){ - TIM1_CtrlPWMOutputs(DISABLE); - uint_PWM_Enable=0; - ui32_setpoint=0; // next priority: Stop motor if not pedaling - printf("you are not pedaling!"); -#endif - - }else if(ui8_BatteryCurrent>BATTERY_CURRENT_MAX_VALUE){ - ui32_setpoint=setpoint_old--; //next priority: reduce (old) setpoint if battery current is too high - printf("Battery Current too high!"); - - }else if (ui32_SPEED_km_h>limit && setpoint_old>(ui32_SPEED_km_h-limit)){ - ui32_setpoint=(uint32_t)setpoint_old-(ui32_SPEED_km_h-limit); //next priority: reduce (old) setpoint, if you are riding too fast - printf("Speed too high!"); - - }else { //if none of the overruling boundaries are concerned, calculate new setpoint -#ifdef TORQUESENSOR - ui32_setpoint=(fummelfaktor*sumtorque)/PAS; //calculate setpoint - //printf("vor: spd %d, pas %d, sumtor %d, setpoint %lu\n", speed, PAS, sumtorque, ui32_setpoint); - ui32_SPEED_km_h=(wheel_circumference*PWM_CYCLES_SECOND*36L)/(100000L*(uint32_t)speed); //calculate speed in km/h conversion fr om sec to hour --> *3600, conversion from mm to km --> /1000000, tic frequency 15625 Hz - i16_delta=(ui32_setpoint-setpoint_old)/p_factor; //simple p-controller to avoid big steps in setpoint value course - if (i16_delta>max_change){i16_delta=max_change;} //limit max change of setpoint to avoid motor stopping by fault - if (i16_deltaSETPOINT_MAX_VALUE){ui32_setpoint=SETPOINT_MAX_VALUE;} //cut setpoint values to the defined maximum -#endif - -#ifdef THROTTLE - if (sumtorque>10 && setpoint_old-10>sumtorque){ - TIM1_CtrlPWMOutputs(DISABLE); - uint_PWM_Enable=0; - printf("Floating!"); - } - else if (!uint_PWM_Enable && sumtorque>setpoint_old){ - TIM1_CtrlPWMOutputs(ENABLE); - uint_PWM_Enable=1; - printf("PWM enabled!"); - } - - ui32_setpoint=sumtorque; -#endif - -#ifdef THROTTLE_AND_PAS - ui32_setpoint=sumtorque; -#endif - - printf("Current %d, Voltage %d, sumtor %d, setpoint %lu, km/h %lu\n", ui8_BatteryCurrent, ui8_BatteryVoltage, sumtorque, ui32_setpoint, ui32_SPEED_km_h); - } - return ui32_setpoint; -} diff --git a/update_setpoint.h b/update_setpoint.h deleted file mode 100644 index 487f7d24..00000000 --- a/update_setpoint.h +++ /dev/null @@ -1,20 +0,0 @@ -/* - * OpenSource EBike firmware - * - * Copyright (C) Stancecoke 2017. - * - * Released under the GPL License, Version 3 - */ - -#ifndef _update_setpoint_H -#define _update_setpoint_H - -#define p_factor 4 //P-factor for control loop -#define max_change 8 //maximum sudden change of setpoint before the motor stops working - -#include "main.h" - -uint16_t update_setpoint (uint16_t speed, uint16_t PAS, uint16_t sumtorque, uint16_t setpoint_old ); - - -#endif /* _SPEED_H */ diff --git a/utils.c b/utils.c deleted file mode 100755 index a663a599..00000000 --- a/utils.c +++ /dev/null @@ -1,27 +0,0 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#include -#include "stm8s.h" - -int32_t map (int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) -{ - // if input is smaller/bigger than expected return the min/max out ranges value - if (x < in_min) - return out_min; - else if (x > in_max) - return out_max; - - // map the input to the output range. - // round up if mapping bigger ranges to smaller ranges - else if ((in_max - in_min) > (out_max - out_min)) - return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min; - // round down if mapping smaller ranges to bigger ranges - else - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} diff --git a/utils.h b/utils.h deleted file mode 100755 index 87bbb710..00000000 --- a/utils.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * EGG OpenSource EBike firmware - * - * Copyright (C) Casainho, 2015, 2106, 2017. - * - * Released under the GPL License, Version 3 - */ - -#ifndef _UTILS_H -#define _UTILS_H - -#include "main.h" - -int32_t map (int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max); - -#endif /* _UTILS_H */ diff --git a/wavetables/midpoint_clamp_255_gen.c b/wavetables/midpoint_clamp_255_gen.c new file mode 100644 index 00000000..7d5b38e6 --- /dev/null +++ b/wavetables/midpoint_clamp_255_gen.c @@ -0,0 +1,67 @@ +uint8_t midpoint_clamp_gen [65] = { + 127, + 133, + 138, + 144, + 149, + 155, + 160, + 165, + 171, + 176, + 181, + 186, + 192, + 197, + 202, + 207, + 212, + 217, + 222, + 227, + 232, + 236, + 239, + 240, + 242, + 243, + 244, + 246, + 247, + 248, + 249, + 250, + 251, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 253, + 253, + 252, + 252, + 251, + 250, + 249, + 248, + 247, + 246, + 245, + 244, + 242, + 241, + 239, + 238 +}; \ No newline at end of file diff --git a/wavetables/nip_tuck_255_gen.c b/wavetables/nip_tuck_255_gen.c new file mode 100644 index 00000000..8d302e67 --- /dev/null +++ b/wavetables/nip_tuck_255_gen.c @@ -0,0 +1,67 @@ +uint8_t nip_tuck_gen [65] = { + 127, + 133, + 138, + 143, + 149, + 154, + 159, + 163, + 168, + 173, + 177, + 182, + 186, + 190, + 194, + 198, + 202, + 206, + 210, + 213, + 217, + 220, + 223, + 226, + 229, + 231, + 234, + 236, + 239, + 241, + 243, + 245, + 246, + 248, + 249, + 251, + 252, + 253, + 253, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255 +}; \ No newline at end of file diff --git a/wavetables/pure_sine_255_gen.c b/wavetables/pure_sine_255_gen.c new file mode 100644 index 00000000..db19e465 --- /dev/null +++ b/wavetables/pure_sine_255_gen.c @@ -0,0 +1,67 @@ +uint8_t pure_sine_gen [65] = { + 127, + 131, + 134, + 137, + 140, + 143, + 146, + 149, + 152, + 155, + 158, + 162, + 165, + 167, + 170, + 173, + 176, + 179, + 182, + 185, + 188, + 190, + 193, + 196, + 198, + 201, + 203, + 206, + 208, + 211, + 213, + 215, + 218, + 220, + 222, + 224, + 226, + 228, + 230, + 232, + 234, + 235, + 237, + 238, + 240, + 241, + 243, + 244, + 245, + 246, + 248, + 249, + 250, + 250, + 251, + 252, + 253, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255 +}; \ No newline at end of file diff --git a/wavetables/six_step_255.c b/wavetables/six_step_255.c new file mode 100644 index 00000000..af600ab8 --- /dev/null +++ b/wavetables/six_step_255.c @@ -0,0 +1,67 @@ +uint8_t six_step [65] = { + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 191, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255 +}; \ No newline at end of file diff --git a/wavetables/third_harmonic_255_gen.c b/wavetables/third_harmonic_255_gen.c new file mode 100644 index 00000000..1149f75f --- /dev/null +++ b/wavetables/third_harmonic_255_gen.c @@ -0,0 +1,67 @@ +uint8_t third_harmonic_gen [65] = { + 127, + 133, + 138, + 143, + 149, + 154, + 159, + 164, + 169, + 174, + 179, + 184, + 188, + 193, + 197, + 201, + 205, + 209, + 213, + 217, + 220, + 223, + 227, + 229, + 232, + 235, + 237, + 239, + 242, + 243, + 245, + 247, + 248, + 249, + 251, + 251, + 252, + 253, + 254, + 254, + 254, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 254, + 254, + 254, + 254, + 253, + 253, + 253, + 252, + 252, + 252, + 252, + 252, + 252, + 251, + 251, + 251 +}; \ No newline at end of file diff --git a/wavetables/trapezodial_255.c b/wavetables/trapezodial_255.c new file mode 100644 index 00000000..e4fe2642 --- /dev/null +++ b/wavetables/trapezodial_255.c @@ -0,0 +1,67 @@ +uint8_t trapezodial [65] = { + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 127, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255 +}; \ No newline at end of file