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;
}
}
}
/*
** 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 );
}
}
}
void advertise_cancel(void)
{
if (!IS_ADVERTISING)
{
return;
}
status_clear(STATUS_ADVERTISING);
sd_ble_gap_adv_stop();
}
/*
* 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);
}
/*
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);
}
}