Esempio n. 1
0
void advertise_on_ble_evt(ble_evt_t const * p_adv_data_evt)
{
    switch(p_adv_data_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
        {
            if (IS_ADVERTISING)
            {
                advertise_cancel();
            }

            break;
        }
        case BLE_GAP_EVT_DISCONNECTED:
        {
            break;
        }
        case BLE_GAP_EVT_TIMEOUT:
        {
            if (BLE_GAP_TIMEOUT_SRC_ADVERTISING == p_adv_data_evt->evt.gap_evt.params.timeout.src)
            {
                status_clear(STATUS_ADVERTISING);
            }

            break;
        }
    }
}
Esempio n. 2
0
/*
** close_output
**
** close output file, if it not stdout
*/
VOID close_output( VOID )
{
    if ( outfile ) {

        if ( (return_code == RC_OK)
             || (return_code == RC_WARN)
             || debug )
        {

            status_msg( "Writting output" );
            errno = 0;
            outfclose( outfile );
            if ( errno )
                err_write( outfile );
            status_clear();

        } else {

            status_msg( "No output written" );
            status_lf();
            outfclear( outfile );

        }
    }
}
Esempio n. 3
0
void advertise_cancel(void)
{
    if (!IS_ADVERTISING)
    {
        return;
    }

    status_clear(STATUS_ADVERTISING);
    sd_ble_gap_adv_stop();
}
Esempio n. 4
0
File: output.c Progetto: mbethke/hsc
/*
 * close_output
 *
 * close output file, if it not stdout
 */
BOOL write_output(HSCPRC * hp)
{
#define MAX_ERRORLEN 500
    STRPTR outfilenm = NULL;
    BOOL written = FALSE;

    if (outfilename)
        outfilenm = estr2str(outfilename);

    if ((return_code == RC_OK)
        || (return_code == RC_WARN)
        || hp->debug) {
        FILE *outfile = NULL;   /* output file */
        char buf[MAX_ERRORLEN + 2];     /* buffer for error string */

        /*
         * try to open output file
         */
        if (outfilenm) {
            errno = 0;
            outfile = fopen(outfilenm, "w");
            if (!outfile) {
                strncpy(buf, "unable to open `", MAX_ERRORLEN);
                strncat(buf, estr2str(outfilename),
                        MAX_ERRORLEN - strlen(buf));
                strncat(buf, "' for output: ", MAX_ERRORLEN - strlen(buf));
                strncat(buf, strerror(errno), MAX_ERRORLEN - strlen(buf));
                status_error(buf);
            }
        } else {
            outfile = stdout;
            outfilenm = STDOUT_NAME;
        }

        /*
         * write output
         */
        if (outfile) {

            DLNODE *nd = dll_first(outlist);

            status_msg("writing output...");
            errno = 0;

            /* write whole list of output-strings */
            while (nd && !errno) {

                EXPSTR *outstring = (EXPSTR *) dln_data(nd);

                nd = dln_next(nd);
                fwrite(estr2str(outstring), sizeof(char),
                       estrlen(outstring), outfile);
            }

            /* handle write-error, display message */
            if (errno) {
                strncpy(buf, "error writing `", MAX_ERRORLEN);
                strncat(buf, outfilenm, MAX_ERRORLEN - strlen(buf));
                strncat(buf, "': ", MAX_ERRORLEN - strlen(buf));
                strncat(buf, strerror(errno), MAX_ERRORLEN - strlen(buf));
                status_error(buf);
            } else
                written = TRUE;

            status_clear();

            /* close output file */
            if (outfile != stdout)
            {
                fclose(outfile);

#ifdef RISCOS
                {
                  /* set the filetype to HTML (&FAF) */
                  char *riscos_filename=unixname_to_riscos(outfilenm);
                  _swix(OS_File,_IN(0)|_IN(1)|_IN(2),18,riscos_filename,0xFAF);
                  free(riscos_filename);
                }
#endif
            }
        }
    } else {

        status_msg("no output written");
        status_lf();

    }

    return (written);
}
Esempio n. 5
0
/*
void read_calibration(void){	
	//Get the factory temperature value, at 85 Deg. C.		
	ad_temp_ref = SP_ReadCalibrationByte(PRODSIGNATURES_ADCACAL0) ;
	ad_temp_ref |= SP_ReadCalibrationByte(PRODSIGNATURES_ADCACAL1) <<8;
}
*/
int main(void){
	clock_switch32M();
	
	init_pwm();
	uart_init();	
	bus_init(&bus);	
	
	
	
	//Clear the AD settings.
	PORTA.DIR = 0;
	PORTA.OUT = 0;
	ADCA.CTRLA  = ADC_FLUSH_bm;
	ADCA.CTRLB  = 0;
	ADCA.REFCTRL  = 0;
	ADCA.EVCTRL  = 0;
	ADCA.PRESCALER  = 0;	

	//Set PORTB as AD input
	PORTB.DIR = 0;
	PORTB.OUT = 0;
		
	//Setup a timer, just for counting. Used for bus message and timeouts.
	TCC1_CTRLA = TC_CLKSEL_DIV1024_gc;
	
	//Set up the RTC for speed timing.
	CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm;	
	RTC.CTRL = RTC_PRESCALER_DIV1_gc;
	RTC.COMP = 32768;
	
	//A counter for 100% PWM timing.
	TCD0.CTRLA = TC_CLKSEL_DIV1024_gc;
	TCD0.PER = 320;
	TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
	
	
	//Make sure to move the reset vectors to application flash.
	//Set the interrupts we want to listen to.
	CCP = CCP_IOREG_gc;
	PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm |PMIC_HILVLEN_bm | PMIC_RREN_bm;
	
	//Enable interrupts.
	sei();
	
	//Set defaults:
	eepsettings.straincal = 200;
	
	eemem_read_block(EEMEM_MAGIC_HEADER_SETTINGS,(uint8_t*)&eepsettings, sizeof(eepsettings), EEBLOCK_SETTINGS1);
	
	
	//Apply settings...
	strain_threshhold = eepsettings.straincal;

	//Startup hall state.
	hall_state = 0x04;
	get_hall();

	//Startup PWM value.
	pwm = PWM_SET_MIN;
	
	uint8_t cnt_tm = 0;		//Used by TCC1
	uint8_t poll_cnt = 0;	//Used for polling the display.	
	uint16_t pwm_lim = 0;	//PWM limit may change at higher speed.
	
	display.strain_th = strain_threshhold;	
	
	//Two flags that can only be set.
	bool should_freewheel = true;
	bool force_commute = false;
	while(1){		
		
		
		//Double check the PWM hasn't gone crazy:
		if (pwm < pwm_set_min){
			pwm = pwm_set_min;
		}
		if (pwm > pwm_set_max){
			pwm = pwm_set_max;
		}
		
		//Testing
		if (testvalue > pwm_set_max){
			testvalue = pwm_set_max;
		}		
		if (testvalue < pwm_set_min){
			testvalue = pwm_set_min;
		}

		//Apply PWM settings.
		TCC0.CCA = pwm;
		TCC0.CCB = pwm;
		TCC0.CCC = pwm;
		
		//Get halls sensors.		
		if (!get_hall()){
			should_freewheel = true;			
			status_set(STAT_HALL_FAULT);
		}		
				
		//Commute or freewheel:
		if (should_freewheel){
			pwm_freewheel();
		}else if (force_commute){			
			if (direction == FORWARD){				
				commute_forward();
			}else{
				commute_backward();
			}	
			
		}else if (flag_hall_changed){
			if (direction == FORWARD){
				commute_forward();
			}else{
				commute_backward();
			}
		}else{			
			//pwm_freewheel();
		}
				
		//Remeber...
		pwm_prev = pwm;
		
		
		//Reset 
		should_freewheel = false;
		//force_commute = false;
				
		if (startup == 10){ //Just started
			//Setup PWM
			commute_start();
			pwm = estimate_pwm_byspeed();
			//Apply PWM settings.
			TCC0.CCA = pwm;
			TCC0.CCB = pwm;
			TCC0.CCC = pwm;
		}
		
		if (display.online == false){
			should_freewheel = true;			
		}		
		if (motor.mode == 0){
			should_freewheel = true;
		}
		
		//Get voltage/current.
		get_measurements();
		
		//Hard under and over voltage.
		if (ad_voltage > 3700){ //3900
			//Freewheel as fast as possible.
			pwm_freewheel();
			should_freewheel = true;
			status_set(STAT_OVER_VOLTAGE);
		}else if (ad_voltage < 1800){ //About 19 volts.
			//Freewheel as fast as possible.
			pwm_freewheel();
			should_freewheel = true;
			status_set(STAT_UNDER_VOLTAGE);
			pwm_inc(200);pwm_inc(200);
		}

		//Regenning too hard:
		if (ad_current_regav < -1300){
			//Freewheel as fast as possible.
			//pwm_freewheel();
			//should_freewheel = true;
			//status_set(STAT_REGEN_HIGH);
			//startup = 3;
		}
		
		#if (HARDWARE_HAS_THROTTLE)
		if (use_throttle){
			//This gas pedal is low on disconnect.					
			if (!get_throttle()){
				status_set(STAT_THROTTLE_FAULT);
				should_freewheel = true;
			}
			
			//Use the brake too:
			if (get_brake()){
				if (use_brake){
					throttle_cruise = false;
					throttle_tick = 0;
				}else{
					if (throttle == 0){ //< 5
						should_freewheel = true;
					}
				}
			}else{
				status_set(STAT_BRAKE_FAULT);				
				should_freewheel = true;
			}
		}
		#endif
		
		if (usehall){
			//Timer:
			if (flag_hall_changed){
				//Toggle blue led:
				PORTC.OUTTGL = PIN7_bm;			
			
				//compute time:
				time_now = RTC.CNT;			
				time_comm = time_now;
				//Running average.			
				time_comm_av += time_comm;
				time_comm_av >>= 1;
				
				commtime_sum+= time_comm;
				commtime_cnt++;				
				cnt_commutations++;				
			
				//Reset timer.
				RTC.CNT = 0;

				//Computer average current in this time:
				if (ad_current_regcnt > 0){
					ad_current_regav = ad_current_regsum / (int32_t)ad_current_regcnt;
				}else{
					ad_current_regav = ad_current;
				}

				//Values reset in next if-statement.
			}
			
			//Allowed to go faster if the commutation time is guaranteed to switch fast enough.
			//TCD0 is used in case the motor suddenly stops. Capacitors are 100uF/16V
			if (time_comm_av < 250){
				//Round REV0 does not support 100% duty cycle.
				pwm_set_min = 0;
			}
			
			//Limit for braking
			pwm_lim = pwm_set_max -  (display.function_val3 * 10);
			if (pwm_lim < pwm_set_min){
				pwm_lim = pwm_set_min;
			}else if (pwm_lim > pwm_set_max){
				pwm_lim = pwm_set_max;
			}
			
			//That's really slow.
			if (RTC.CNT> 5000){			
				speed = 0;
			}
		
			//Minimum response time
			if (flag_hall_changed) {
					

				//Clear current sum:
				ad_current_regcnt = 0;
				ad_current_regsum = 0;				
				
				RTC.CNT = 0;	
			}							
			
			//Control loop
			if ((flag_hall_changed) || (TCC1.CNT > 450)){
				//Clear hall flag if it was set.
				flag_hall_changed = false;
				if (!off){
					//If brake is enabled:
					if (use_brake){
						if (brake > 5){
							//Regenning is a bit harder. The amount of strength decreases if PWM is too high.
							//Brake is current limited.
							//At low speed, ovverride PWM settings
							if (speed < 150){
								pwm_lim = pwm_set_max;
							}
																
							if ((ad_current_regav + 169) > (((int16_t)brake)*-12)){  //100 * 10 -> current of about -8 Amp
								//More power: inc PWM
									
								//Use the speed average as a voltage estimate for the motor.
								//24V will give an unloaded speed of 350 (35.0kmh)
								//If we're doing 10km/h, the motor is at 7V approx, 30% duty cycle.
									
								if (pwm < pwm_lim){
									pwm_inc(slope_brake);
								}									
									
							}else{
								//Back off
								pwm_dec(slope_brake);
							}
							//Voltage limits
							if (ad_voltage > 3600){
								pwm_dec(slope_brake/2);
							}
							if (ad_voltage > 3700){
								pwm_dec(slope_brake/2);
							}
						}
					}
					if (use_throttle){
						//Undo brake :
						if (flag_brake_lowrpm){
							off = false;
							tie = false;
							status_clear();
							flag_brake_lowrpm = false;
						}


						if (throttle > 5){
							/*
							//Throttle is current limited.
							if ((ad_current_regav + 169) < (((int16_t)throttle)*8)){
								//More power
								pwm_dec(slope_throttle*2);
							}else if ((ad_current_regav + 169) < (((int16_t)throttle)*16)){  //100 * 16 -> current of about 12 Amp									
								//More power
								pwm_dec(slope_throttle);
							}else{
								//Back off
								pwm_inc(slope_throttle);
							}*/
							
							//Voltage limits
							if (ad_voltage < 1950){
								pwm_inc(slope_throttle*2);
							}	
							if (ad_voltage < 1900){
								pwm_inc(slope_throttle*2);
							}
							
							//Cal current reg:
							//Current calibration:
							#if(HARDWARE_VER == HW_CTRL_REV0)
							int32_t regualtion = ad_current_regav; 
							regualtion += 1749;
							regualtion *= 1000;
							regualtion /= 286;
							regualtion -= 34;
							#endif
							
							//Current regulation
							if (speed){
								if (regualtion < ((int16_t)throttle*(int16_t)motor.mode)){ //200 * 0-5
									//More power
									pwm_dec(slope_throttle*2);
								}else if (regualtion < ((int16_t)throttle*(int16_t)motor.mode)){ 
									//More power
									pwm_dec(slope_throttle);
								}else{
									//Back off
									pwm_inc(slope_throttle);
								}
							}							
						}
					}
					
					if (use_pedalassist) {						
						if (ad_strain_av > highestforce){
							highestforce  = ad_strain_av;
						}					
						
						if (ad_strain_av > strain_threshhold){
							strain_cnt = 75UL + (2UL*speed);
							if (ad_strain_av-strain_threshhold < 100){
								pedal_signal = ad_strain_av-strain_threshhold;
							}else{
								pedal_signal = 100;
							}							
							
						}
						
						if (strain_cnt){
							strain_cnt--;
						}else{
							pedal_signal = 0;
						}
						
						if (pedal_signal && strain_cnt){
							//Same control loop as in throttle:
						
							
							//Voltage limits
							if (ad_voltage < 1950){
								pwm_inc(slope_throttle*2);
							}	
							if (ad_voltage < 1900){
								pwm_inc(slope_throttle*2);
							}
							
							//Cal current reg:
							//Current calibration:
							#if(HARDWARE_VER == HW_CTRL_REV0)
							int32_t regualtion = ad_current_regav; 
							regualtion += 1749;
							regualtion *= 1000;
							regualtion /= 286;
							regualtion -= 34;
							#endif
							
							//Current regulation
							if (speed){
								if (regualtion < ((int16_t)pedal_signal*2*(int16_t)motor.mode)){ //200 * 0-5
									//More power
									pwm_dec(slope_throttle*2);
								}else if (regualtion < ((int16_t)pedal_signal*2*(int16_t)motor.mode)){ 
									//More power
									pwm_dec(slope_throttle);
								}else{
									//Back off
									pwm_inc(slope_throttle);
								}
							}			
						}
						
					}
					
					

				}else{ //if (off)
					//Undo braking at low RPM
					if (flag_brake_lowrpm){
						off = false;
						tie = false;
						status_clear();
						flag_brake_lowrpm = false;
					}
					
					
				}
			}			
		}else{ //Hall hasn't changed.
			//Don't use hall.
			if (RTC.CNT > time_comm_av_last){
				//Reset timer.
				RTC.CNT = 0;				
				
			}					
		}		
		
		//Act on a fault?
		if (isfaultset()){
			if (tie){	
				tie_ground();
			}else{			
				should_freewheel = true;
			}			
		}
		
		if (use_pedalassist && (!use_throttle)){
			if (!pedal_signal){
				should_freewheel = true;
			}
		}else if ((!use_pedalassist) && (use_throttle && (throttle < 5))){
			should_freewheel = true;
		}else if ((use_throttle)&& (use_pedalassist)){
			if ((throttle < 5) && (!pedal_signal)){
				should_freewheel = true;
			}
		}
		
		//Current min/max
		if (ad_current < ad_currentmin){
			ad_currentmin = ad_current;
		}
		if (ad_current > ad_currentmax){
			ad_currentmax = ad_current;
		}
		
		if (flag_keep_pwm){
			pwm = pwm_prev;
		}

		cnt_rotations = cnt_commutations / 48;
		
		
				
		//Set display data	
		display.voltage = ad_voltage_av;
		display.current = ad_current_av;
		display.power = power_av;
		display.error = status;
		display.speed = speed% 1000; //999 max
		motor.status = status;
		//display.speed = pwm;
		//display.current = pwm_lim;
		if (use_brake){
			display.throttle = brake;	
		}else{
			if (use_pedalassist){
				if (pedal_signal > throttle){
					display.throttle = pedal_signal;
				}else{
					display.throttle = throttle;
				}
			}else{
				display.throttle = throttle;
			}
			
		}
		display.cruise = throttle_cruise;
		
		//Throttle test
		//display.speed = pwm;
		//display.current = status;
		
		if (TCC1.CNT > 450){
			cnt_tm++;
			TCC1.CNT = 0;
			
			//Testing
			/*
			if (display.button_state & BUTT_MASK_FRONT){
				if (testvalue < (PWM_SET_MAX -5)){
					testvalue+=5;
				}
			}else if (display.button_state & BUTT_MASK_TOP){
				if (testvalue > (PWM_SET_MIN+5)){
					testvalue-=5;
				}
			}*/
			
			//Apply speed limit:
			pwm_set_max = (display.function_val2 +1) * 50;
			if (pwm_set_max > PWM_SET_MAX){
				pwm_set_max = PWM_SET_MAX;
			}
			
			
			//Startup from standstill
			uint8_t throttle_was = throttle;
			throttle = motor.throttle;
			if ((throttle_was == 0) && throttle && (speed == 0)){
				force_commute = true;
				pwm_inc(20);
			}else{
				force_commute = false;
			}
			
			
			if (!motor.mode){
				pwm_inc(200);
			}
			//Testing 
			
					
			
			
			//Last character in message
			if (wait_for_last_char){
				wait_for_last_char = false;
			}else{
				bus_endmessage(&bus);
			}
			
			
			
			//Estimate speed  /pwm. Only when there is no throttle signal or brake.
			if (!use_brake){
				if ((use_throttle && (throttle < 5)) && ((use_pedalassist)&&(!pedal_signal))){
					pwm = estimate_pwm_byspeed();					
				}
			}			
			
			//Send timer tick
			bus_tick(&bus);
			
			if (bus_check_for_message()){
				green_led_on();				
			}else{
				green_led_off();
				//Increase offline count.
				display.offline_cnt++;
				if (display.offline_cnt > 10){
					uart_rate_find();
					PORTC.OUTSET = PIN6_bm;
				}else{
					PORTC.OUTCLR = PIN6_bm;
				}					
				
				//If offline too long:
				if (display.offline_cnt > 50){
					//You need to make it unsafe again.
					display.road_legal = true;
					display.online = false;
					throttle_cruise = false;
					should_freewheel = true;
					throttle = 0;
					use_brake = false;
					brake = 0;
					pedal_signal = 0;
					strain_cnt = 0;
					motor.mode = 0;
				}
				
				poll_cnt++;				
				if (poll_cnt == 2){
					#if (OPT_DISPLAY== FW_DISPLAY_MASTER)
					bus_display_poll(&bus);
					#endif
				}else if (poll_cnt == 4){
					//Update the display, and it's variables:
					
					//Speed is now the amount of commutations per seconds.
					if (commtime_sum){
						speed = (commtime_cnt * 32768UL) / commtime_sum;
						//With a 26" wheel:
						speed = (speed * 10000UL) / 6487UL;
					}else{
						speed=0;
					}						
					
					commtime_sum = 0;
					commtime_cnt = 0 ;
						
					//Disaplay average voltage:
					ad_voltage_av = ad_voltage_sum / meas_cnt;
					ad_voltage_sum = 0;
						
					//And current.
					ad_current_av = ad_current_sum / meas_cnt;
					ad_current_sum = 0;

					//Strain, if we have it:
					#ifdef HARDWARE_HAS_STRAIN
					//Running average:
					int32_t strain_val = ad_strain_sum / meas_cnt;
					strain_val /= 100;
					
					ad_strain_av += strain_val;
					ad_strain_av /= 2;
					
					//ad_strain_av = meas_cnt;
					ad_strain_sum = 0;
					
					//Strain calibration in Menu F8/F3:
					if ((display.func == 8) && (display.menu_downcnt > 100)){	
						strain_threshhold = 200;
						display.strain_th = strain_threshhold;
					}						
					if ((display.func == 3) && (display.menu_downcnt > 100)){						
						if (ad_strain_av > strain_threshhold){
							strain_threshhold++;
						}
						display.strain_th = strain_threshhold;
						flagsave = true;
					}
					
					if (display.menu_timeout == 0){
						if (flagsave){
							flagsave = false;
							//Save
							eepsettings.straincal = strain_threshhold;
							eemem_write_block(EEMEM_MAGIC_HEADER_SETTINGS,(uint8_t*)&eepsettings, sizeof(eepsettings), EEBLOCK_SETTINGS1);
							
						}
						
					}
					
					
					//Set test value
					//uint16_t s; 
					//adc_init_single_ended(REF_3V3,ADC_CH_MUXPOS_PIN8_gc); //Strain Sensor.
					//s = adc_getsample();
					
					//2180 average, over if you help.
					/*
					if (s > 2183){
						display.value1 = 200;
						testvalue+=1;
					}else if (s < 2177){
						display.value1 = 000;
						testvalue-=1;
					}else{
						display.value1 = 100;
					}*/
					
					
					
					
					//motor.current = ad_strain_av;
					display.value1 = ad_strain_av;// ad_strain_av - 2000;
					display.value2 = highestforce;// ad_strain_av - 2000;
					display.value3 = pwm;
					
					display.value4 = should_freewheel;
					#endif
					
					//Temperature
					ad_temp_av = ad_temp_sum / meas_cnt;
					ad_temp_sum = 0;
					
					//Set test value
					//display.value2 = ad_temp_av / 2;
					
					//Temperature about 20 Deg.C is 1866 counts. CKDIV16 2020 CKDIV512
					
										
					//Counts per kelvin.
					/*
					uint32_t cpk = ad_temp_ref*10 / 358;					
					ad_temp_av  = (ad_temp_av * 10) / cpk - 273;
					ad_temp_av = ad_temp_ref;
					*/
												
					meas_cnt = 0;
						
					//Calibration:
					ad_voltage_av -= 175;
					ad_voltage_av *= 1000;
					ad_voltage_av /= 886;
					
					//Current calibration:	
					#if(HARDWARE_VER == HW_CTRL_REV0)
					ad_current_av += 1749;
					ad_current_av *= 1000;
					ad_current_av /= 286;
					ad_current_av -= 34;
					#else					
					ad_current_av += 169;
					ad_current_av *= 100;
					ad_current_av /= 75;					
					#endif
					
					power_av = (ad_current_av * ad_voltage_av) / 10000;
									
					poll_cnt = 0;
					#if(OPT_DISPLAY == FW_DISPLAY_MASTER)
					bus_display_update(&bus);
					#endif
					//motor.mode = 1;
				}
			}
		}
	
		//Counter from TCC1
		if (cnt_tm > 15){
			cnt_tm = 0;
			
			//Increase throttle tick, for cruise control.
			if ((use_throttle)&& (!use_brake)){
				if ((display.online)&&(throttle > 10)){
					if (throttle_tick < 20){
						throttle_tick++;
					}else{
						throttle_cruise = true;
					}
				}				
			}
			
			//test
			//pwm_dec(5);
			testvalue-=5;
			
			ad_currentmin = 9999;
			ad_currentmax = -9999;
			
			
			
			
			if (startup){
				startup--;
				
				if (startup == 0){
					//Clear any status flag:
					//status_clear();
					//Give it a nudge.
					//commute_allowed = true;
				}
			}else{
				status_clear();
			}
			
		}
		
		//Set fault flags:
		if (off){
			status_set(STAT_STOPPED);
		}
		
		if (use_throttle){
			//status_set(STAT_USETHROTTLE);
		}		
	}