#include "Arduino.h" #include "stm32h7xx.h" #include "globals.h" #include "ardustim.h" #include "enums.h" #include "comms.h" #include "storage.h" #include "wheel_defs.h" #include "Wire.h" #include "HardwareTimer.h" #include "STModbus.h" //******************************************************** //Ethernet Pins #define W5500_CS PA4 #define W5500_MOSI PA7 #define W5500_MISO PA6 #define W5500_SCK PA5 //Modbus Registers #define PATTERN_REG 100 #define RPM_REG 101 #define KNOCK_AMP_REG 102 #define KNOCK_FREQ_REG 103 #define KNOCK_ALPHA_REG 104 #define KNOCK_DELTA1_REG 105 #define KNOCK_DELTA2_REG 106 #define VIB_AMP_REG 107 #define VIB_FREQ_REG 108 STModbus mb(W5500_CS, W5500_MOSI, W5500_MISO, W5500_SCK); byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; IPAddress ip(192, 168, 20, 22); IPAddress gateway(192, 168, 20, 1); IPAddress subnet(255, 255, 255, 0); //******************************************************** uint16_t angle[1000]; uint16_t temp_index = 0; float A = 4000; float f = 50; float a1 = 5; float a2 = 50; float t = 0; float t0 = 0.25; uint16_t knock_signal_value; bool flag_update_knock_signal = LOW; uint16_t count = 0; volatile float time_per_degree; volatile uint32_t knock_start_time; volatile uint32_t knock_stop_time; volatile uint8_t knock_start_angle = 5; volatile uint8_t flag_generate_knock_signal = 0; volatile uint8_t flag_first_time = 0; volatile uint8_t knock_duration = 30; uint16_t last_pattern_idx = 0xFFFF; uint16_t last_rpm = 0xFFFF; float last_knock_amp = -1; float last_knock_freq = -1; float last_alpha = -1; uint8_t last_delta1 = 0xFF; uint8_t last_delta2 = 0xFF; float last_vib_amp = -1; float last_vib_freq = -1; uint32_t apb1_freq; uint32_t apb2_freq; #define SAMPLES_PER_KNOCK_SIGNAL 200 volatile uint32_t timer6_overflow_freq; ADC_HandleTypeDef hadc1; // ADC Handle volatile uint16_t adcValues[2]; // Store ADC results struct configTable config; struct status currentStatus; /* Sensistive stuff used in ISR's */ volatile uint16_t adc0; /* POT RPM */ volatile uint16_t adc1; /* Pot Wheel select */ /* Setting rpm to any value over 0 will enabled sweeping by default */ /* Stuff for handling prescaler changes (small tooth wheels are low RPM) */ volatile uint8_t analog_port = 0; volatile bool adc0_read_complete = false; volatile bool adc1_read_complete = false; volatile bool reset_prescaler = false; volatile uint8_t output_invert_mask = 0x00; /* Don't invert anything */ volatile uint8_t prescaler_bits = 0; volatile uint8_t last_prescaler_bits = 0; volatile uint16_t new_OCR1A = 5000; /* sane default */ volatile uint16_t edge_counter = 0; volatile uint32_t cycleStartTime = micros(); volatile uint32_t cycleDuration = 0; volatile uint32_t cycleTime; uint32_t sweep_time_counter = 0; uint8_t sweep_direction = ASCENDING; /* Less sensitive globals */ uint8_t bitshift = 0; HardwareTimer *MyTim = new HardwareTimer(TIM2); // Use TIM2 HardwareTimer *MyTim6 = new HardwareTimer(TIM6); //also use Timer6 (for knock signal) // Handle Modbus Update void handleUpdate() { uint16_t pattern_idx = mb.hreg(PATTERN_REG); //Serial.println("Pattern Selected : " + String(pattern_idx)); if (pattern_idx != last_pattern_idx && pattern_idx < MAX_WHEELS) { config.wheel = pattern_idx; last_pattern_idx = pattern_idx; Serial.println("Pattern Selected : " + String(pattern_idx)); } uint16_t new_rpm = mb.hreg(RPM_REG); if (new_rpm != last_rpm && config.mode == FIXED_RPM) { currentStatus.base_rpm = new_rpm; setRPM(new_rpm); last_rpm = new_rpm; } float knock_amp = mb.hreg(KNOCK_AMP_REG) * 400; float knock_freq = mb.hreg(KNOCK_FREQ_REG) * 5; float alpha = mb.hreg(KNOCK_ALPHA_REG) * 0.0625; uint8_t delta1 = mb.hreg(KNOCK_DELTA1_REG); uint8_t delta2 = mb.hreg(KNOCK_DELTA2_REG); float vib_amplitude = mb.hreg(VIB_AMP_REG) * 0.1; float vib_frequency = mb.hreg(VIB_FREQ_REG); bool knock_params_changed = (knock_amp != last_knock_amp || knock_freq != last_knock_freq || alpha != last_alpha || delta1 != last_delta1 || delta2 != last_delta2 || vib_amplitude != last_vib_amp || vib_frequency != last_vib_freq); if (knock_params_changed) { A = knock_amp; f = knock_freq; a1 = alpha; knock_start_angle = delta1; knock_duration = delta2; knock_stop_time = knock_start_time + (knock_duration * time_per_degree); last_knock_amp = knock_amp; last_knock_freq = knock_freq; last_alpha = alpha; last_delta1 = delta1; last_delta2 = delta2; last_vib_amp = vib_amplitude; last_vib_freq = vib_frequency; t = 0; for (uint16_t i = 0; i < 200; i++) { t += 0.005; knock_signal_value = (A + vib_amplitude) * sin(2 * PI * (f + vib_frequency) * t); if (t < t0) knock_signal_value = knock_signal_value * exp(-a1 * ((t0 - t) * (t0 - t))); else knock_signal_value = knock_signal_value * exp(-a2 * ((t - t0) * (t - t0))); knock_signal_value = fabs(knock_signal_value); angle[i] = (uint16_t)knock_signal_value; } } } // **Step 1: Configure ADC** void ADC_Init() { /* __HAL_RCC_ADC1_CLK_ENABLE(); // Enable ADC1 clock hadc1.Instance = ADC1; hadc1.Init.ScanConvMode = ENABLE; // Enable multi-channel hadc1.Init.ContinuousConvMode = DISABLE; // Single conversion mode hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc1.Init.NbrOfConversion = 2; // Two channels HAL_ADC_Init(&hadc1); // **Configure Channel 1 (PA0)** ADC_ChannelConfTypeDef sConfig = {0}; sConfig.Channel = ADC_CHANNEL_0; sConfig.Rank = 1; // First conversion sConfig.SamplingTime = ADC_SAMPLETIME_112CYCLES; HAL_ADC_ConfigChannel(&hadc1, &sConfig); // **Configure Channel 2 (PA1)** sConfig.Channel = ADC_CHANNEL_1; sConfig.Rank = 2; // Second conversion HAL_ADC_ConfigChannel(&hadc1, &sConfig); // **Enable ADC Interrupt** HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC_IRQn); // **Start ADC with Interrupt** //HAL_ADC_Start_IT(&hadc1); } // **Step 2: ADC Interrupt Handler** void ADC1_2_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc1); */ } // **Step 3: Callback Function for ADC Conversion** void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) { if (hadc->Instance == ADC1) { adcValues[0] = HAL_ADC_GetValue(&hadc1); // Read first channel (PA0) //HAL_ADC_Start_IT(&hadc1); // Start next conversion adc0_read_complete = true; } } void init_timer2(void) { // Configure TIM2 with a period of 1 second (1000000 µs) MyTim->setOverflow(8000, HERTZ_FORMAT); MyTim->attachInterrupt(Timer2Callback); // Attach the interrupt handler MyTim->resume(); // Start the timer } void init_timer6(void) { // Configure TIM2 with a period of 1 second (1000000 µs) MyTim6->setOverflow(200000, HERTZ_FORMAT); MyTim6->attachInterrupt(Timer6Callback); // Attach the interrupt handler MyTim6->resume(); // Start the timer } wheels Wheels[MAX_WHEELS] = { /* Pointer to friendly name string, pointer to edge array, RPM Scaler, Numodbuser of edges in the array, whether the numodbuser of edges covers 360 or 720 degrees */ { dizzy_four_cylinder_friendly_name, dizzy_four_cylinder, 0.03333, 4, 360 }, { dizzy_six_cylinder_friendly_name, dizzy_six_cylinder, 0.05, 6, 360 }, { dizzy_eight_cylinder_friendly_name, dizzy_eight_cylinder, 0.06667, 8, 360 }, { sixty_minus_two_friendly_name, sixty_minus_two, 1.0, 120, 360 }, { sixty_minus_two_with_cam_friendly_name, sixty_minus_two_with_cam, 1.0, 240, 720 }, { sixty_minus_two_with_halfmoon_cam_friendly_name, sixty_minus_two_with_halfmoon_cam, 1.0, 240, 720 }, { thirty_six_minus_one_friendly_name, thirty_six_minus_one, 0.6, 72, 360 }, { twenty_four_minus_one_friendly_name, twenty_four_minus_one, 0.5, 48, 360 }, { four_minus_one_with_cam_friendly_name, four_minus_one_with_cam, 0.06667, 16, 720 }, { eight_minus_one_friendly_name, eight_minus_one, 0.13333, 16, 360 }, { six_minus_one_with_cam_friendly_name, six_minus_one_with_cam, 0.15, 36, 720 }, { twelve_minus_one_with_cam_friendly_name, twelve_minus_one_with_cam, 0.6, 144, 720 }, { fourty_minus_one_friendly_name, fourty_minus_one, 0.66667, 80, 360 }, { dizzy_four_trigger_return_friendly_name, dizzy_four_trigger_return, 0.15, 9, 720 }, { oddfire_vr_friendly_name, oddfire_vr, 0.2, 24, 360 }, { optispark_lt1_friendly_name, optispark_lt1, 3.0, 720, 720 }, { twelve_minus_three_friendly_name, twelve_minus_three, 0.4, 48, 360 }, { thirty_six_minus_two_two_two_friendly_name, thirty_six_minus_two_two_two, 0.6, 72, 360 }, { thirty_six_minus_two_two_two_h6_friendly_name, thirty_six_minus_two_two_two_h6, 0.6, 72, 360 }, { thirty_six_minus_two_two_two_with_cam_friendly_name, thirty_six_minus_two_two_two_with_cam, 0.6, 144, 720 }, { fourty_two_hundred_wheel_friendly_name, fourty_two_hundred_wheel, 0.6, 72, 360 }, { thirty_six_minus_one_with_cam_fe3_friendly_name, thirty_six_minus_one_with_cam_fe3, 0.6, 144, 720 }, { six_g_seventy_two_with_cam_friendly_name, six_g_seventy_two_with_cam, 0.6, 144, 720 }, { buell_oddfire_cam_friendly_name, buell_oddfire_cam, 0.33333, 80, 720 }, { gm_ls1_crank_and_cam_friendly_name, gm_ls1_crank_and_cam, 6.0, 720, 720 }, { gm_ls_58X_crank_and_4x_cam_friendly_name, GM_LS_58X_crank_and_4x_cam, 1.0, 240, 720 }, { lotus_thirty_six_minus_one_one_one_one_friendly_name, lotus_thirty_six_minus_one_one_one_one, 0.6, 72, 360 }, { honda_rc51_with_cam_friendly_name, honda_rc51_with_cam, 0.2, 48, 720 }, { thirty_six_minus_one_with_second_trigger_friendly_name, thirty_six_minus_one_with_second_trigger, 0.6, 144, 720 }, { chrysler_ngc_thirty_six_plus_two_minus_two_with_ngc4_cam_friendly_name, chrysler_ngc_thirty_six_plus_two_minus_two_with_ngc4_cam, 3.0, 720, 720 }, { chrysler_ngc_thirty_six_minus_two_plus_two_with_ngc6_cam_friendly_name, chrysler_ngc_thirty_six_minus_two_plus_two_with_ngc6_cam, 3.0, 720, 720 }, { chrysler_ngc_thirty_six_minus_two_plus_two_with_ngc8_cam_friendly_name, chrysler_ngc_thirty_six_minus_two_plus_two_with_ngc8_cam, 3.0, 720, 720 }, { weber_iaw_with_cam_friendly_name, weber_iaw_with_cam, 1.2, 144, 720 }, { fiat_one_point_eight_sixteen_valve_with_cam_friendly_name, fiat_one_point_eight_sixteen_valve_with_cam, 3.0, 720, 720 }, { three_sixty_nissan_cas_friendly_name, three_sixty_nissan_cas, 3.0, 720, 720 }, { twenty_four_minus_two_with_second_trigger_friendly_name, twenty_four_minus_two_with_second_trigger, 0.3, 72, 720 }, { yamaha_eight_tooth_with_cam_friendly_name, yamaha_eight_tooth_with_cam, 0.26667, 64, 720 }, { gm_four_tooth_with_cam_friendly_name, gm_four_tooth_with_cam, 0.06666, 8, 720 }, { gm_six_tooth_with_cam_friendly_name, gm_six_tooth_with_cam, 0.1, 12, 720 }, { gm_eight_tooth_with_cam_friendly_name, gm_eight_tooth_with_cam, 0.13333, 16, 720 }, { volvo_d12acd_with_cam_friendly_name, volvo_d12acd_with_cam, 4.0, 480, 720 }, { mazda_thirty_six_minus_two_two_two_with_six_tooth_cam_friendly_name, mazda_thirty_six_minus_two_two_two_with_six_tooth_cam, 1.5, 360, 720 }, { mitsubishi_4g63_4_2_friendly_name, mitsubishi_4g63_4_2, 0.6, 144, 720 }, { audi_135_with_cam_friendly_name, audi_135_with_cam, 1.5, 1080, 720 }, { honda_d17_no_cam_friendly_name, honda_d17_no_cam, 0.6, 144, 720 }, { mazda_323_au_friendly_name, mazda_323_au, 1, 30, 720 }, { daihatsu_3cyl_friendly_name, daihatsu_3cyl, 0.8, 144, 360 }, { miata_9905_friendly_name, miata_9905, 0.6, 144, 720 }, { twelve_with_cam_friendly_name, twelve_with_cam, 0.6, 144, 720 }, { twenty_four_with_cam_friendly_name, twenty_four_with_cam, 0.6, 144, 720 }, { subaru_six_seven_name_friendly_name, subaru_six_seven, 3.0, 720, 720 }, { gm_seven_x_friendly_name, gm_seven_x, 1.502, 180, 720 }, { four_twenty_a_friendly_name, four_twenty_a, 0.6, 144, 720 }, { ford_st170_friendly_name, ford_st170, 3.0, 720, 720 }, { mitsubishi_3A92_friendly_name, mitsubishi_3A92, 0.6, 144, 720 }, { Toyota_4AGE_CAS_friendly_name, toyota_4AGE_CAS, 0.333, 144, 720 }, { Toyota_4AGZE_friendly_name, toyota_4AGZE, 0.333, 144, 720 }, { Suzuki_DRZ400_friendly_name, suzuki_DRZ400, 0.6, 72, 360 }, { Jeep_2000_friendly_name, jeep_2000, 1.5, 360, 720 }, { BMW_N20_friendly_name, bmw_n20, 1.0, 240, 720 }, { VIPER9602_friendly_name, viper9602wheel, 1.0, 240, 720 }, { thirty_six_minus_two_with_second_trigger_friendly_name, thirty_six_minus_two_with_second_trigger, 0.6, 144, 720 }, }; /* Initialization */ void setup() { HAL_Init(); // Initialize HAL initi2c(); loadConfig(); ADC_Init(); // Initialize ADC //analogWriteResolution(12); serialSetup(); //set the DAC resolution to 12-bit analogWriteResolution(12); apb1_freq = HAL_RCC_GetPCLK1Freq(); // APB1 clock apb2_freq = HAL_RCC_GetPCLK2Freq(); // APB2 clock t = 0; for (uint16_t i = 0; i < 200; i++) { t += 0.005; knock_signal_value = A * sin(2 * PI * f * t); if (t < t0) knock_signal_value = knock_signal_value * exp(-a1 * ((t0 - t) * (t0 - t))); else knock_signal_value = knock_signal_value * exp(-a2 * ((t - t0) * (t - t0))); knock_signal_value = fabs(knock_signal_value); angle[i] = (uint16_t)knock_signal_value; } mb.setDebug(Serial); if (!mb.start(mac, ip, gateway, subnet)) { Serial.println("Failed to start Modbus server."); while (true) delay(1000); } mb.addHreg(PATTERN_REG, 0); mb.addHreg(RPM_REG, 0); mb.addHreg(KNOCK_AMP_REG, 0); mb.addHreg(KNOCK_FREQ_REG, 0); mb.addHreg(KNOCK_ALPHA_REG, 0); mb.addHreg(KNOCK_DELTA1_REG, 0); mb.addHreg(KNOCK_DELTA2_REG, 0); mb.addHreg(VIB_AMP_REG, 0); mb.addHreg(VIB_FREQ_REG, 0); //Modbus portion end init_timer2(); init_timer6(); //Serial.println(config.version); // pinMode(7, OUTPUT); /* Debug pin for Saleae to track sweep ISR execution speed */ pinMode(PB0, OUTPUT); /* Primary (crank usually) output */ pinMode(PB1, OUTPUT); /* Secondary (cam1 usually) output */ pinMode(PB5, OUTPUT); /* Tertiary (cam2 usually) output */ pinMode(PB3, OUTPUT); /* Knock signal for seank, ony on LS1 pattern, NOT IMPL YET */ //DAC output for knock signal generation pinMode(PA4, OUTPUT); // Set DAC1 pin pinMode(PC0, OUTPUT); //for test purposes /* Make sure we are using the DEFAULT RPM on startup */ reset_new_OCR1A(currentStatus.rpm); } // End setup void Timer6Callback(void) { //digitalWrite(PC0, HIGH); //DAC1->DHR12R1 = (uint32_t)knock_signal_value; //HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_0); flag_update_knock_signal = HIGH; //digitalWrite(PC0, LOW); } /* Pumps the pattern out of flash to the port * The rate at which this runs is dependent on what OCR1A is set to */ void Timer2Callback() { /* This is VERY simple, just walk the array and wrap when we hit the limit */ GPIOB->ODR = output_invert_mask ^ Wheels[config.wheel].edge_states_ptr[edge_counter]; /* Write it to the port */ edge_counter++; if (edge_counter == Wheels[config.wheel].wheel_max_edges) { edge_counter = 0; cycleDuration = micros() - cycleStartTime; flag_first_time = 1; cycleStartTime = micros(); } /* The tables are in flash so we need pgm_read_byte() */ /* Reset Prescaler only if flag is set */ if (reset_prescaler) { TIM2->PSC = prescaler_bits - 1; reset_prescaler = false; } /* Reset next compare value for RPM changes */ TIM2->ARR = new_OCR1A - 1; /* Apply new "RPM" from Timer2 ISR, i.e. speed up/down the virtual "wheel" */ calculateCurrentCrankAngle(); } void loop() { //Modbus portion //Call once inside loop() - all magic here mb.task(); handleUpdate(); uint16_t tmp_rpm = currentStatus.base_rpm; /*code for knock signal generation*/ if (flag_generate_knock_signal) { if (flag_update_knock_signal) { HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_0); //digitalWrite(PC0, HIGH); analogWrite(PA4, angle[count++]); if (count > 199) { count = 0; flag_generate_knock_signal = 0; } //digitalWrite(PC0, HIGH); /* t += 0.002; if(t>1) t=0; knock_signal_value = A * sin(2 * PI * f * t); if(t 0) { commandParser(); } if (config.mode == POT_RPM) { //HAL_ADC_Start_IT(&hadc1); if (adc0_read_complete == true) { adc0_read_complete = false; tmp_rpm = adc0 << TMP_RPM_SHIFT; if (tmp_rpm > TMP_RPM_CAP) { tmp_rpm = TMP_RPM_CAP; } } } else if (config.mode == LINEAR_SWEPT_RPM) { if (micros() > (sweep_time_counter + config.sweep_interval)) { sweep_time_counter = micros(); if (sweep_direction == ASCENDING) { tmp_rpm = currentStatus.base_rpm + 1; if (tmp_rpm >= config.sweep_high_rpm) { sweep_direction = DESCENDING; } } else { tmp_rpm = currentStatus.base_rpm - 1; if (tmp_rpm <= config.sweep_low_rpm) { sweep_direction = ASCENDING; } } } } else if (config.mode == FIXED_RPM) { tmp_rpm = config.fixed_rpm; } currentStatus.base_rpm = tmp_rpm; currentStatus.compressionModifier = calculateCompressionModifier(); if (currentStatus.compressionModifier >= currentStatus.base_rpm) { currentStatus.compressionModifier = 0; } setRPM((currentStatus.base_rpm - currentStatus.compressionModifier)); } uint16_t calculateCompressionModifier() { if ((currentStatus.base_rpm > config.compressionRPM) || (config.useCompression != true)) { return 0; } //if( currentStatus.base_rpm > 400 ) { return 0;} uint16_t crankAngle = calculateCurrentCrankAngle(); uint16_t modAngle = crankAngle; uint16_t compressionModifier = 0; switch (config.compressionType) { case COMPRESSION_TYPE_2CYL_4STROKE: modAngle = crankAngle / 2; compressionModifier = sin_100_180[modAngle]; break; case COMPRESSION_TYPE_4CYL_4STROKE: modAngle = (crankAngle % 180); compressionModifier = sin_100_180[modAngle]; break; case COMPRESSION_TYPE_6CYL_4STROKE: modAngle = crankAngle % 120; compressionModifier = sin_100_120[modAngle]; break; case COMPRESSION_TYPE_8CYL_4STROKE: modAngle = crankAngle % 90; compressionModifier = sin_100_90[modAngle]; break; default: modAngle = (crankAngle % 180); compressionModifier = sin_100_180[modAngle]; break; } //RPM scaler - Varies the amplitude of the compression modifier based on how far below the compression RPM point we are. Eg: //If the compression RPM value is 400 //At 300rpm the amplitude will be 75% //At 200rpm the amplitude will be 50% //At 100rpm the amplitude will be 25% etc //Base RPM must be below 650 to prevent overflow if (config.compressionDynamic && (currentStatus.base_rpm < 655U)) { compressionModifier = (compressionModifier * currentStatus.base_rpm) / config.compressionRPM; } return compressionModifier; } uint16_t calculateCurrentCrankAngle() { /*changes made to incorporate the generation of knock signal at a specified start angle*/ if (cycleDuration == 0) { return 0; } /*changes made to incorporate the generation of knock signal at a specified start angle*/ if (flag_first_time) { flag_first_time = 0; //time_per_degree = (cycleDuration/2)/Wheels[config.wheel].wheel_degrees ; time_per_degree = cycleDuration / Wheels[config.wheel].wheel_degrees; knock_start_time = knock_start_angle * time_per_degree; knock_stop_time = (knock_start_time + (30 * time_per_degree)); timer6_overflow_freq = ((SAMPLES_PER_KNOCK_SIGNAL * 1000000) / (30 * time_per_degree)); MyTim6->setOverflow(timer6_overflow_freq, HERTZ_FORMAT); } //digitalWrite(PC0, HIGH); cycleTime = micros() - cycleStartTime; /* if(!flag_generate_knock_signal){ if(knock_start_time < cycleTime && knock_stop_time > cycleTime) { digitalWrite(PC0, HIGH); flag_generate_knock_signal = 1; } else { digitalWrite(PC0, LOW); } } */ if (Wheels[config.wheel].wheel_degrees == 720) { cycleTime = cycleTime * 2; } uint16_t tmpCrankAngle = ((cycleTime * 360U) / cycleDuration); tmpCrankAngle += config.compressionOffset; while (tmpCrankAngle > 360) { tmpCrankAngle -= 360; } if (tmpCrankAngle > knock_start_angle && tmpCrankAngle < (knock_start_angle + 30)) { flag_generate_knock_signal = 1; //digitalWrite(PC0, HIGH); } //else //digitalWrite(PC0, LOW); //temproatry code for testing //HAL_DAC_SetValue(&hdac, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dac_value); //analogWrite(PA4, (tmpCrankAngle * 8)); //angle[temp_index++] = tmpCrankAngle; //if(temp_index>999) //temp_index=0; //digitalWrite(PC0, LOW); return tmpCrankAngle; } /*! * Validates the new user requested RPM and sets it if valid. */ void setRPM(uint16_t newRPM) { if (newRPM < 10) { return; } if (currentStatus.rpm != newRPM) { reset_new_OCR1A(newRPM); } currentStatus.rpm = newRPM; } void reset_new_OCR1A(uint32_t new_rpm) { uint32_t tmp; uint32_t test; uint8_t bitshift; uint8_t tmp_prescaler_bits = 1; tmp = (uint32_t)(120000000.0 / (Wheels[config.wheel].rpm_scaler * (float)(new_rpm < 10 ? 10 : new_rpm))); tmp = tmp / 2; //tmp = (uint32_t)(8000000/(Wheels[config.wheel].rpm_scaler * (new_rpm < 10 ? 10:new_rpm))); //uint64_t x = 800000000000ULL; //get_prescaler_bits(&tmp,&tmp_prescaler_bits,&bitshift); do { test = tmp / tmp_prescaler_bits; tmp_prescaler_bits++; } while (test > 60000); new_OCR1A = test; prescaler_bits = tmp_prescaler_bits - 1; reset_prescaler = true; } /* uint8_t get_bitshift_from_prescaler(uint8_t *prescaler_bits) { switch (*prescaler_bits) { case PRESCALE_1024: return 10; case PRESCALE_256: return 8; case PRESCALE_64: return 6; case PRESCALE_8: return 3; case PRESCALE_1: return 0; } return 0; } //! Gets prescaler enum and bitshift based on OC value void get_prescaler_bits(uint32_t *potential_oc_value, uint8_t *prescaler, uint8_t *bitshift) { if (*potential_oc_value >= 16777216) { *prescaler = PRESCALE_1024; *bitshift = 10; } else if (*potential_oc_value >= 4194304) { *prescaler = PRESCALE_256; *bitshift = 8; } else if (*potential_oc_value >= 524288) { *prescaler = PRESCALE_64; *bitshift = 6; } else if (*potential_oc_value >= 65536) { *prescaler = PRESCALE_8; *bitshift = 3; } else { *prescaler = PRESCALE_1; *bitshift = 0; } } */