void writeToFlashIfPending() { if (!getNeedToWriteConfiguration()) { return; } // todo: technically we need a lock here, realistically we should be fine. needToWriteConfiguration = false; scheduleMsg(&logger, "Writing pending configuration"); writeToFlash(); }
static void setEngineType(int value) { engineConfiguration->engineType = (engine_type_e) value; resetConfigurationExt((engine_type_e) value, engineConfiguration, engineConfiguration2, boardConfiguration); #if EFI_PROD_CODE writeToFlash(); // scheduleReset(); #endif /* EFI_PROD_CODE */ incrementGlobalConfigurationVersion(); doPrintConfiguration(); }
int main(void){ uart_init(); stdout = &out; while(1){ char option = uart_getchar(); switch(option){ case '\x00': printf("preparing to write to flash\n"); writeToFlash(); break; default: printf("%c is not a valid option\n", option); } } }
void initHardware(Logging *logger, Engine *engine) { engine_configuration_s *engineConfiguration = engine->engineConfiguration; efiAssertVoid(engineConfiguration!=NULL, "engineConfiguration"); board_configuration_s *boardConfiguration = &engineConfiguration->bc; printMsg(logger, "initHardware()"); // todo: enable protection. it's disabled because it takes // 10 extra seconds to re-flash the chip //flashProtect(); chMtxInit(&spiMtx); #if EFI_HISTOGRAMS /** * histograms is a data structure for CPU monitor, it does not depend on configuration */ initHistogramsModule(); #endif /* EFI_HISTOGRAMS */ /** * This is so early because we want to init logger * which would be used while finding trigger synch index * while config read */ initTriggerDecoder(); /** * We need the LED_ERROR pin even before we read configuration */ initPrimaryPins(); if (hasFirmwareError()) { return; } initDataStructures(PASS_ENGINE_PARAMETER_F); #if EFI_INTERNAL_FLASH palSetPadMode(CONFIG_RESET_SWITCH_PORT, CONFIG_RESET_SWITCH_PIN, PAL_MODE_INPUT_PULLUP); initFlash(engine); /** * this call reads configuration from flash memory or sets default configuration * if flash state does not look right. */ if (SHOULD_INGORE_FLASH()) { engineConfiguration->engineType = FORD_ASPIRE_1996; resetConfigurationExt(logger, engineConfiguration->engineType, engine); writeToFlash(); } else { readFromFlash(); } #else engineConfiguration->engineType = FORD_ASPIRE_1996; resetConfigurationExt(logger, engineConfiguration->engineType, engineConfiguration, engineConfiguration2, boardConfiguration); #endif /* EFI_INTERNAL_FLASH */ if (hasFirmwareError()) { return; } mySetPadMode2("board test", boardConfiguration->boardTestModeJumperPin, PAL_MODE_INPUT_PULLUP); bool isBoardTestMode_b = GET_BOARD_TEST_MODE_VALUE(); initAdcInputs(isBoardTestMode_b); if (isBoardTestMode_b) { // this method never returns initBoardTest(); } initRtc(); initOutputPins(); #if EFI_HIP_9011 initHip9011(); #endif /* EFI_HIP_9011 */ #if EFI_MAX_31855 initMax31855(boardConfiguration); #endif /* EFI_MAX_31855 */ #if EFI_CAN_SUPPORT initCan(); #endif /* EFI_CAN_SUPPORT */ // init_adc_mcp3208(&adcState, &SPID2); // requestAdcValue(&adcState, 0); // todo: figure out better startup logic initTriggerCentral(engine); #if EFI_SHAFT_POSITION_INPUT initShaftPositionInputCapture(); #endif /* EFI_SHAFT_POSITION_INPUT */ initSpiModules(boardConfiguration); #if EFI_FILE_LOGGING initMmcCard(); #endif /* EFI_FILE_LOGGING */ // initFixedLeds(); // initBooleanInputs(); #if EFI_UART_GPS initGps(); #endif #if ADC_SNIFFER initAdcDriver(); #endif #if EFI_HD44780_LCD // initI2Cmodule(); lcd_HD44780_init(); if (hasFirmwareError()) return; lcd_HD44780_print_string(VCS_VERSION); #endif /* EFI_HD44780_LCD */ addConsoleActionII("i2c", sendI2Cbyte); // while (true) { // for (int addr = 0x20; addr < 0x28; addr++) { // sendI2Cbyte(addr, 0); // int err = i2cGetErrors(&I2CD1); // print("I2C: err=%x from %d\r\n", err, addr); // chThdSleepMilliseconds(5); // sendI2Cbyte(addr, 255); // chThdSleepMilliseconds(5); // } // } printMsg(logger, "initHardware() OK!"); }
/*********************************************************************** This is the main controller loop. sequences of operations: - reads from CAN bus or serial port 1. - reads encoders (or ADC). - computes the control value (a PID in this version). - checks limits and other errors. - sets PWM - does extra functions (e.g. communicate with neighboring cards). ***********************************************************************/ void main(void) { Int32 PWMoutput [JN]; Int32 PWMoutput_old [JN]; byte i=0; byte wi=0; byte k=0; UInt16 *value=0; Int32 t1val=0; Int32 PID_R= 2; Int32 kpp=1; Int16 current_turn=0; Int16 print_number=0; Int16 real_pos=0; byte first_step=0; #if (VERSION == 0x0351) #define winSizeMax 32 #define initialWindowSize 4 #else #define winSizeMax 32 #define initialWindowSize 30 #endif byte divJntPos[JN]=INIT_ARRAY(initialWindowSize-1); byte divJntVel[JN]=INIT_ARRAY(initialWindowSize-1); byte divMotPos[JN]=INIT_ARRAY(initialWindowSize-1); byte divMotVel[JN]=INIT_ARRAY(initialWindowSize-1); byte headJntPos[JN]=INIT_ARRAY(0); //current joint pos byte tailJntPos[JN]=INIT_ARRAY(0); byte headJntVel[JN]=INIT_ARRAY(0); //current joint vel byte tailJntVel[JN]=INIT_ARRAY(0); byte headMotPos[JN]=INIT_ARRAY(0); //current motor pos byte tailMotPos[JN]=INIT_ARRAY(0); byte headMotVel[JN]=INIT_ARRAY(0); //current motor vel byte tailMotVel[JN]=INIT_ARRAY(0); Int32 jntPosWindow[winSizeMax][JN]; //max window size = winSizeMax Int32 jntVelWindow[winSizeMax][JN]; //max window size = winSizeMax Int32 motPosWindow[winSizeMax][JN]; //max window size = winSizeMax Int32 motVelWindow[winSizeMax][JN]; //max window size = winSizeMax Int16 _safeband[JN]; //it is a value for reducing the JOINT limit of 2*_safeband [tick encoder] #ifdef TEMPERATURE_SENSOR byte TempSensCount1 = 0; UInt32 TempSensCount2 = 0; byte temp_sens_status=0; overtemp[0]=0; overtemp[1]=0; errortemp[0]=0; errortemp[1]=0; #endif /* gets the address of flash memory from the linker */ _flash_addr = get_flash_addr(); /* enable interrupts */ setReg(SYS_CNTL, 0); // IPL channels from 0 to 6 enabled // external interrupts IRQA and IRQB disabled setRegBits(IPR, 0xFE00); // enable FAULT __ENIGROUP (61, 3); #if (VERSION == 0x0254) #else __ENIGROUP (60, 3); #endif // enable SCI __ENIGROUP (52, 4); __ENIGROUP (53, 4); __ENIGROUP (50, 4); __ENIGROUP (51, 4); // enable data flash __ENIGROUP (13, 4); // enable CAN __ENIGROUP (14, 6); __ENIGROUP (15, 6); __ENIGROUP (16, 6); __ENIGROUP (17, 6); // enable ADCA/ADCB __ENIGROUP (55, 6); __ENIGROUP (54, 6); //enable PWM reload __ENIGROUP (59, 7); // PMWA #if (VERSION == 0x0254) #else __ENIGROUP (58, 7); // PWMB #endif // enable timers // TIMER_A __ENIGROUP (45, 7); //Timer for the encoder commutation if used __ENIGROUP (44, 7); // __ENIGROUP (43, 7); // __ENIGROUP (42, 4); //TI1 1ms delay main loop // TIMER_B __ENIGROUP (41, 7); // __ENIGROUP (40, 7); // __ENIGROUP (39, 7); // __ENIGROUP (38, 7); // TIMER_C __ENIGROUP (37, 1); __ENIGROUP (36, 1); __ENIGROUP (35, 1); __ENIGROUP (34, 1); // TIMER_D __ENIGROUP (33, 7); //1ms delay duty cycle __ENIGROUP (32, 1); __ENIGROUP (31, 1); __ENIGROUP (30, 1); __EI(); flash_interface_init (JN); readFromFlash (_flash_addr); if (_version==_flash_version) { } else { writeToFlash(_flash_addr); } __DI(); #warning "debug"// ; __EI(); init_leds (); #if (VERSION == 0x0254) Init_Brushless_Comm (1,HALL); #else Init_Brushless_Comm (JN,HALL); #endif can_interface_init (JN); init_strain (); init_position_abs_ssi (); #if VERSION ==0x0257 init_relative_position_abs_ssi(); #endif init_faults (true,true,true); init_position_encoder (); TI1_init (); //variable init mainLoopOVF=0; _count=0; for(i=0;i<JN;i++) { _received_pid[i].rec_pid=0; } BUS_OFF=false; #warning "debug"// ; //__EI(); // print_version (); /* initialization */ for (i=0; i<JN; i++) _calibrated[i] = false; /* reset trajectory generation */ for (i=0; i<JN; i++) abort_trajectory (i, 0); /////////////////////////////////////// // reset of the ABS_SSI // this is needed because the AS5045 gives the first value wrong !!! for (i=0; i<JN; i++) _position[i]=(Int32) Filter_Bit(get_position_abs_ssi(i)); for (i=0; i<JN; i++) _max_real_position[i]=Filter_Bit(4095); ////////////////////////////////////// /* initialize speed and acceleration to zero (useful later on) */ for (i=0; i<JN; i++) _position_old[i] = 0; for (i=0; i<JN; i++) _speed[i] = 0; for (i=0; i<JN; i++) _accel[i] = 0; for (i=0; i<JN; i++) _safeband[i] =5; //5 ticks => 1 grado di AEA. for (i=0; i<JN; i++) PWMoutput [i] = PWMoutput_old[i] = 0; /* reset the recursive windows for storage of position and velocity data */ /* (for velocity and position estimates) */ for(i=0;i<JN;i++) { for(wi=0;wi<winSizeMax;wi++) { jntPosWindow[wi][i]=_position[i]; jntVelWindow[wi][i]=0; motPosWindow[wi][i]=0; motVelWindow[wi][i]=0; } } //set_relative_position_abs_ssi(1,get_absolute_real_position_abs_ssi(1)); /* main control loop */ for(_counter = 0;; _counter ++) { if (_counter >= CAN_SYNCHRO_STEPS) _counter = 0; led3_on while (_wait); _count=0; led3_off // BUS_OFF check if (getCanBusOffstatus() ) { #ifdef DEBUG_CAN_MSG can_printf("DISABLE BUS OFF"); #endif for (i=0; i<JN; i++) put_motor_in_fault(i); led1_off } else led1_on // READING CAN MESSAGES can_interface(); for (i=0; i<JN; i++) if (_pad_enabled[i]==false && _control_mode[i]!=MODE_HW_FAULT) _control_mode[i]=MODE_IDLE; //Position calculation // This is used to have a shift of the zero-cross out of the // joint workspace // // max_real_position is the limit of the joint starting from // 4095 and going to decrease this number without zero-cross // untill the joint limit is reached #if VERSION == 0x0257 _position_old[0]=_position[0]; if(get_error_abs_ssi(0)==ERR_OK) _position[0]=Filter_Bit (get_position_abs_ssi(0)); _position_old[1]=_position[1]; if(get_error_abs_ssi(1)==ERR_OK) _position[1]=Filter_Bit (get_position_abs_ssi(1)); #else for (i=0; i<JN; i++) { _position_old[i]=_position[i]; if(get_error_abs_ssi(i)==ERR_OK) _position[i]=Filter_Bit (get_position_abs_ssi(i)); } #endif // get_commutations() is used to read the incremental encoder of the motors. // the variable _motor_position is then used to estimate the rotor speed and // compensate the back-EMF of the motor. for (i=0; i<JN; i++) _motor_position[i]=get_position_encoder(i);//get_commutations(i); ///////////////////////////////////////////DEBUG//////////// #if (VERSION !=0x0254) for (i=0; i<JN; i++) { if (get_error_abs_ssi(i)==ERR_ABS_SSI) { put_motor_in_fault(i); #ifdef DEBUG_CAN_MSG can_printf("ABS error %d",i); #endif } } #endif #if (VERSION ==0x0254) if (get_error_abs_ssi(0)==ERR_ABS_SSI) { put_motor_in_fault(0); #ifdef DEBUG_CAN_MSG can_printf("ABS error %d",0); #endif } #endif //DO NOTHING // decoupling the position decouple_positions(); /* velocity and acceleration estimators */ { for (i=0; i<JN; i++) { //joint velocity estimator tailJntPos[i]=headJntPos[i]+(winSizeMax-divJntPos[i]); if(tailJntPos[i]>=winSizeMax) tailJntPos[i]=tailJntPos[i]%winSizeMax; _speed_old[i] = _speed[i]; jntPosWindow[headJntPos[i]][i]=_position[i]; _speed[i] = (Int32) (((jntPosWindow[headJntPos[i]][i] - jntPosWindow[tailJntPos[i]][i] ))<<_jntVel_est_shift[i]); // _speed[i] <<= _jntVel_est_shift[i]; _speed[i] = (Int32)(_speed[i]) / divJntPos[i]; headJntPos[i]=headJntPos[i]+1; if(headJntPos[i]>=winSizeMax) headJntPos[i]=0; /* //joint acceleration estimator tailJntVel[i]=headJntVel[i]+(winSizeMax-divJntVel[i]); if(tailJntVel[i]>=winSizeMax) tailJntVel[i]=tailJntVel[i]%winSizeMax; _accel_old[i] = _accel[i]; jntVelWindow[headJntVel[i]][i]=_speed[i]; _accel[i] = ((jntVelWindow[headJntVel[i]][i] - jntVelWindow[tailJntVel[i]][i] )); _accel[i] << _jntAcc_est_shift[i]; _accel[i] = (Int32)(_accel[i]) / divJntVel[i]; headJntVel[i]=headJntVel[i]+1; if(headJntVel[i]>=winSizeMax) headJntVel[i]=0; */ //motor velocity estimator tailMotPos[i]=headMotPos[i]+(winSizeMax-divMotPos[i]); if(tailMotPos[i]>=winSizeMax) tailMotPos[i]=tailMotPos[i]%winSizeMax; _motor_speed_old[i] = _motor_speed[i]; motPosWindow[headMotPos[i]][i]=_motor_position[i]; _motor_speed[i] = ((motPosWindow[headMotPos[i]][i] - motPosWindow[tailMotPos[i]][i] )); _motor_speed[i] <<= _motVel_est_shift[i]; _motor_speed[i] = (_motor_speed[i]) / divMotPos[i]; headMotPos[i]=headMotPos[i]+1; if(headMotPos[i]>=winSizeMax) headMotPos[i]=0; } } /* in position? */ #if (VERSION != 0x0254) for (i=0; i<JN; i++) _in_position[i] = check_in_position(i); #else _in_position[0] = check_in_position(0); #endif /* in reference configuration for calibration? */ //for (i=0; i<JN; i++) check_in_position_calib(i); //******************************************* POSITION LIMIT CHECK ***************************/ for (i=0; i<JN; i++) check_range(i, _safeband[i], PWMoutput); //******************************************* COMPUTES CONTROLS *****************************/ //FT sensor watchdog update for (i=0; i<STRAIN_MAX; i++) if (_strain_wtd[i]>0) _strain_wtd[i]--; for (i=0; i<JN; i++) { //computing the PWM value (PID) PWMoutput[i] = compute_pwm(i); // PWM filtering in torque control if there is no bemf compensation #if (VERSION != 0x0351) if (_control_mode[i] == MODE_TORQUE || _control_mode[i] == MODE_IMPEDANCE_POS || _control_mode[i] == MODE_IMPEDANCE_VEL) { if (_useFilter[i] == 3) PWMoutput[i] = lpf_ord1_3hz (PWMoutput[i], i); } // saving the PWM value before the decoupling _bfc_PWMoutput[i] = PWMoutput_old[i] = PWMoutput[i]; // applying the saturation to the PWM if (_bfc_PWMoutput[i] < -MAX_DUTY) _bfc_PWMoutput[i]=-MAX_DUTY; else if (_bfc_PWMoutput[i] > MAX_DUTY) _bfc_PWMoutput[i]= MAX_DUTY; #endif //(VERSION != 0x0351) } //decouple PWM decouple_dutycycle(PWMoutput); //******************************************* SATURATES CONTROLS ***************************/ /* back emf compensation + controls saturation (if necessary) */ for (i=0; i<JN; i++) { if (_control_mode[i] == MODE_TORQUE || _control_mode[i] == MODE_IMPEDANCE_POS || _control_mode[i] == MODE_IMPEDANCE_VEL) { #if (VERSION != 0x0351) // Back emf compensation //PWMoutput[i]+=compensate_bemf(i, _comm_speed[i]); //use the motor speed PWMoutput[i]+=compensate_bemf(i, _speed[i]); //use the joint speed //add the coulomb friction compensation term if (_kstp_torque[i] != 0 || _kstn_torque[i] != 0) //PWMoutput[i]+=compensate_friction(i, _comm_speed[i]); //use the motor speed PWMoutput[i]+=compensate_friction(i, _speed[i]); //use the joint speed // Protection for joints out of the admissible range during force control check_range_torque(i, _safeband[i], PWMoutput); // PWM saturation ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit_torque[i] ); #else //(VERSION != 0x0351) ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] ); #endif //(VERSION != 0x0351) } else { ENFORCE_LIMITS (i,PWMoutput[i], _pid_limit[i] ); } if (_pid[i] < -MAX_DUTY) _pid[i]=-MAX_DUTY; else if (_pid[i] > MAX_DUTY) _pid[i]= MAX_DUTY; } /* generate PWM */ for (i=0; i<JN; i++) { if (!mode_is_idle(i)) {PWM_generate(i,_pid[i]);} } /* Check Current done in T1 */ /* do extra functions, communicate, etc. */ //send broadcast data can_send_broadcast(); //send additional debug information //can_send_broadcast_debug(1,1); /*********************************************************************** // Check Current is made here /***********************************************************************/ #if (VERSION != 0x0254) for (i=0; i<JN; i++) #else for (i=0; i<1; i++) #endif { if ((get_current(i)>=25000) || (get_current(i)<=-25000)) { put_motor_in_fault(i); highcurrent[i]=true; #ifdef DEBUG_CAN_MSG can_printf("j%d curr %f",i,get_current(i)); #endif } check_current(i, (_pid[i] > 0)); compute_i2t(i); if (_filt_current[i] > MAX_I2T_CURRENT) { put_motor_in_fault(i); highcurrent[i]=true; #ifdef DEBUG_CAN_MSG can_printf("j%d filtcurr %f",i,_filt_current[i]); #endif } } // Check for the MAIN LOOP duration // t1val= (UInt16) TI1_getCounter(); if ( _count>0) { mainLoopOVF=1; _count=0; } /* tells that the control cycle is completed */ _wait = true; } /* end for(;;) */