为了便于阅读，偿试把BLHeli的汇编源程序改成C语言格式

zlf66778899 · 发表于 2017-3-5 21:49:35

只是为了方便阅读和理解，所以未做详细的正确性检查。
因为能力所限，程度中还是保留了很多跳转（goto）语句。
主循环已经完成，发上来供大家参考
int main(void)
{
if(0 == Prepare_Lock_Or_Fuse_Read() & 0x80)
{
while(1);
}

Disable_Watchdog();

Initialize stack();

switch_power_off();

Ports_initialization();

Clear_r0_r25();

Clear_ram();

Random = 1;

// Set default programmed parameters
set_default_parameters();

// Read all programmed parameters
read_all_eeprom_parameters();

// Initialize ADC
Initialize_Adc();

// Set beep strength
Beep_Strength = Pgm_Beep_Strength;

//Set initial arm variable
Initial_Arm = 1;

//关中断
cli();

//???蜂鸣123
wait200ms();
beep_f1();
wait30ms();
beep_f2();
wait30ms();
beep_f3();
wait30ms();

#if MODE <= 1 ; Main or tail
//Wait for receiver to initialize
wait1s();
wait200ms();
wait200ms();
wait100ms();
#endif

init_no_signal:
while(1)
{
cli();
temp1 = 250;
do
{
temp2 = 250;
do
{
if(0 == (Read_Rcp_Int() & (1 << Rcp_In)))
{
goto bootloader_done;
}
}while(--temp2 != 0);
}while(--temp1 != 0);

if(Be_Bootloader())
{
Booterloder();
while(1);
}

bootloader_done:

decode_parameters();

decode_settings();

set_bec_voltage();

find_throttle_gain();

Beep_Strength = Pgm_Beep_Strength;

switch_power_off();

//Timer0: clk/8 for regular interrupts
TCCR0 = (1 << CS01);
//Timer1: clk/8 for commutation control and RC pulse measurement
TCCR1B = (1 << CS11);
//Timer2: clk/8 for pwm
TCCR2 = (1 << CS21);
//Initialize interrupts and registers
TIFR = (1<<TOIE0)+(1<<OCIE1A)+(1<<TOIE2);
TIMSK = (1<<TOIE0)+(1<<OCIE1A)+(1<<TOIE2);

//Initialize comparator
Comp_Init();

wait1ms();

//Enable all interrupts
sei();

//Measure number of lipo cells
Measure_Lipo_Cells();

//Reset stall count
sts Stall_Cnt = 0;

//Initialize RC pulse
Rcp_Int_First(); //Enable interrupt and set to first edge
Rcp_Int_Enable();// Enable interrupt
Rcp_Clear_Int_Flag();// Clear interrupt flag

Flags2 &= ~(1<<RCP_EDGE_NO);// Set first edge flag

wait200ms();

Flags0 |= (1<<RCP_MEAS_PWM_FREQ);// ; Set measure pwm frequency flag
Temp4 = 3;// ; Number of attempts before going back to detect input signal
measure_pwm_freq_start:
Temp3 = 12// ; Number of pulses to measure
measure_pwm_freq_loop:
//Check if period diff was accepted
if(Rcp_Period_Diff_Accepted == 0)
{
Temp3 = 12; // ; Reset number of pulses to measure
if(0 == --Temp4)
{
continue;
};
}
wait30ms(); // ; Wait 30ms for new pulse
if(0 == (Flags2 & (1 << RCP_UPDATED)))//; Is there an updated RC pulse available - proceed
{
continue;
}
Flags2 &= ~(1<<RCP_UPDATED);//      ; Flag that pulse has been evaluated

if(New_Rcp < RCP_VALIDATE) goto measure_pwm_freq_start; //New_Rcp太小

Temp1 = Curr_Rcp_Pwm_Freq;// ; Store as previous flags for next pulse
Prev_Rcp_Pwm_Freq = Temp1;//

// Store current flags for next pulse
Curr_Rcp_Pwm_Freq = (Flags3 & ((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ)));

if(Curr_Rcp_Pwm_Freq != Temp1) goto measure_pwm_freq_start;// ; Go back if new flags not same as previous

if(--Temp3 != 0) goto measure_pwm_freq_loop;// ; Go back if not required number of pulses seen

//Clear measure pwm frequency flag
Flags0 &= ~(1<<RCP_MEAS_PWM_FREQ);

//Set up RC pulse interrupts after pwm frequency measurement
Rcp_Int_First();// ; Enable interrupt and set to first edge
Rcp_Clear_Int_Flag();// ; Clear interrupt flag
Flags2 &= ~(1<<RCP_EDGE_NO);// ; Set first edge flag

if(Pgm_Enable_PWM_Input == 0)// ; Check if PWM input is enabled
{
Flags2 |= (1<<RCP_PPM);// ; Set PPM flag

//__zlf__原程序此处是否正确？？？
Flags3 &= (!((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ)));
}
//Test whether signal is OnShot125
Flags2 &= ~(1 << RCP_PPM_ONESHOT125);// ; Clear OneShot125 flag
Rcp_Outside_Range_Cnt = 0;// ; Reset out of range counter
wait100ms();// ; Wait for new RC pulses

if((0 != (Flags2 & (1 << RCP_PPM))) &&// If flag is not set (PWM) - branch
(Rcp_Outside_Range_Cnt >= 10))// Check how many pulses were outside normal PPM range (800-2160us)
{
Flags2 |= (1<<RCP_PPM_ONESHOT125);// ; Set OneShot125 flag
}
//; Validate RC pulse
validate_rcp_start:
do
{
wait3ms();// ; Wait for next pulse (NB: Uses Temp1/2!)
Temp1 = RCP_VALIDATE;// ; Set validate level as default
if(0 != (Flags2 & ( 1<< RCP_PPM)))
{
Temp1 = 0;// ; Set level to zero for PPM (any level will be accepted)
}
}while (New_Rcp < Temp1);//New_Rcp太小
//; Beep arm sequence start signal
cli();// ; Disable all interrupts
beep_f1();// ; Signal that RC pulse is ready
beep_f1();// ; Signal that RC pulse is ready
beep_f1();// ; Signal that RC pulse is ready
sei();// ; Enable all interrupts
wait200ms();

// Arming sequence start
Gov_Arm_Target = 0;// ; Clear governor arm target

arming_start:
do
{

#if MODE >= 1// ; Tail or multi
if(Pgm_Direction != 3) // ; Check if bidirectional operation
{
#endif
wait3ms();
if((Pgm_Enable_TX_Program >= 1)|| //; Start programming mode entry if enabled
(Initial_Arm >= 1))// ; Yes - check if it is initial arm sequence
{
do
{
if(0 == (Flags2 & (1 << RCP_PPM)))
{
//PWM tx program entry
if(New_Rcp < RCP_MAX)// ; Is RC pulse max?
{
break;// ; No - branch
}

while(New_Rcp >= RCP_STOP)//New_Rcp >= RCP_STOP
{
cli();// ; Disable all interrupts
beep_f4();
sei();// ; Enable all interrupts
wait100ms();
}

while(New_Rcp < RCP_MAX);
{
cli();// ; Disable all interrupts
beep_f1();
wait10ms();
beep_f1();
sei();// ; Enable all interrupts
wait100ms();
}
program_by_tx();// ; Yes - enter programming mode
}

//; PPM throttle calibration and tx program entry
#if MODE <= 1 //; Main or tail
temp8 = 8;// ; Set 3 seconds wait time
#else
temp8 = 3;// ; Set 1 second wait time
#endif
for(;temp8 != 0; tmp8--)
{
Flags3 |= (1<<FULL_THROTTLE_RANGE);// ; Set range to 1000-2020us
cli();
find_throttle_gain();// ; Set throttle gain
sei();
wait100ms();// ; Wait for new throttle value
cli();// ; Disable interrupts (freeze New_Rcp value)
Flags3 &= ~(1<<FULL_THROTTLE_RANGE);// Set programmed range
find_throttle_gain();// ; Set throttle gain
Temp7 = New_Rcp;// ; Store new RC pulse value
sei(); //; Enable interrupts
if(New_Rcp < (RCP_MAX/2))// Is RC pulse above midstick?
{
goto arm_target_updated;// ; No - branch
}
wait1ms();
cli();// ; Disable all interrupts
beep_f4();
sei(); ; Enable all interrupts
}

average_throttle();
#if MODE <= 1 //; Main or tail
Pgm_Ppm_Max_Throttle = Temp7 - 5;// ; Subtract about 2% and ensure that it is 250 or lower
#else
Pgm_Ppm_Max_Throttle = Temp7;
#endif
wait200ms();
cli();
success_beep();
sei();

for(Temp8 = 10; Temp8 != 0; Temp8--)// ; Set 3 seconds wait time
{
Flags3 |= (1<<FULL_THROTTLE_RANGE);// ; Set range to 1000-2020us
cli();
find_throttle_gain();// ; Set throttle gain
sei();
wait100ms();
cli(); //; Disable interrupts (freeze New_Rcp value)
Flags3 &= ~(1<<FULL_THROTTLE_RANGE);// ; Set programmed range
find_throttle_gain();// ; Set throttle gain
Temp7 = New_Rcp;// ; Store new RC pulse value
sei(); ; Enable interrupts

if(New_Rcp >= (RCP_MAX/2))// ; Below midstick?
{
Temp8 = 10;// ; Set 3 seconds wait time
continue;// ; No - start over
}

wait1ms();
cli();// ; Disable all interrupts
beep_f1();
wait10ms();
beep_f1();
sei(); ; Enable all interrupts
}

average_throttle();
Temp1 = Pgm_Ppm_Min_Throttle = Temp7 + 5;// ; Add about 2% (subtract negative number), ; Min throttle in Temp1
Temp2 = Pgm_Ppm_Max_Throttle - 130;// ; Subtract 130 (520us) from max throttle
if((Pgm_Ppm_Max_Throttle < 130) ||
(Pgm_Ppm_Max_Throttle - 130 < Temp1)
{
Pgm_Ppm_Max_Throttle = Temp1 + 130;//Make max 520us higher than min
}
wait200ms();
cli();
store_all_in_eeprom();
success_beep_inverted();
sei();
wait100ms();
cli();
find_throttle_gain();// ; Set throttle gain
sei();
}while(New_Rcp < RCP_MAX);
program_by_tx();// ; Yes - enter programming mode
}

if(New_Rcp >= Gov_Arm_Target)// ; Is RC pulse larger than arm target?
{
Gov_Arm_Target = Temp1;// Yes - update arm target
}

arm_target_updated:
wait100ms();// ; Wait for new throttle value
Temp1 = RCP_STOP;// ; Default stop value
if(Pgm_Direction == 3)// ; Check if bidirectional operation
{
Temp1 = (RCP_STOP+4);// ; Higher stop value for bidirectional
}
#if MODE >= 1// ; Tail or multi
}
#endif
}while(New_Rcp >= Temp1);//No below stop

//; Beep arm sequence end signal
cli();// ; Disable all interrupts
beep_f4();// ; Signal that rcpulse is ready
beep_f4();// ; Signal that rcpulse is ready
beep_f4();// ; Signal that rcpulse is ready
sei();// ; Enable all interrupts
wait200ms();

//; Clear initial arm variable
Initial_Arm = 0;

//; Armed and waiting for power on
wait_for_power_on:
while(1)
{
Power_On_Wait_Cnt_L = 0;// Clear wait counter
Power_On_Wait_Cnt_H = 0;// Zero
do
{
Power_On_Wait_Cnt_L++;// ; Increment low wait counter
if(Power_On_Wait_Cnt_L == 0xFF)// Counter wrapping (about 3 sec)?
{
Power_On_Wait_Cnt_H++;// ; Increment high wait counter
Temp1 = 25;// ; Approximately 1 min
if(Pgm_Beacon_Delay = 1)
else if(Pgm_Beacon_Delay = 2)
{
Temp1 = 50;// ; Approximately 2 min
}
else if(Pgm_Beacon_Delay = 3)
{
Temp1 = 125;// ; Approximately 5 min
}
else if(Pgm_Beacon_Delay = 4)
{
Temp1 = 250;// ; Approximately 10 min
}
else
{
Power_On_Wait_Cnt_H = 0;//Reset counter for infinite delay
}

if(Power_On_Wait_Cnt_H >= Temp1)// ; Check against chosen delay
{
switch_power_off();// ; Switch power off in case braking is set
wait1ms();
Power_On_Wait_Cnt_H--;// Decrement high wait counter
Power_On_Wait_Cnt_L = 0; Set low wait counter
Beep_Strength = Pgm_Beacon_Strength;

cli();// ; Disable all interrupts
beep_f();// ; Signal that there is no signal
sei();// ; Enable all interrupts

Beep_Strength = Pgm_Beep_Strength;
wait100ms();// ; Wait for new RC pulse to be measured
}
}
wait10ms();
if(Rcp_Timeout_Cntd == 0)
{
if(0 != (Flags2 & (1 << RCP_PPM)))//; If ppm and pulses missing - go back to detect input signal
{
goto init_no_signal;
}
}

Temp1 = RCP_STOP;
if(0 == (Flags2 & (1 << RCP_PPM)))
{
Temp1 = (RCP_STOP+5);// ; Higher than stop (for pwm)
}
}while(New_Rcp < Temp1);

#if MODE >= 1 //; Tail or multi
if(Pgm_Direction != 3)
#endif
{
wait100ms();// ; Wait to see if start pulse was only a glitch
}
if(0 == Rcp_Timeout_Cntd)// ; If it is not zero - proceed
{
goto init_no_signal;// ; If it is zero (pulses missing) - go back to detect input signal
}

//Start entry point
cli();
switch_power_off();
Requested_Pwm = 0;//Set requested pwm to zero
Governor_Req_Pwm = 0;// Set governor requested pwm to zero
Current_Pwm = 0;//Set current pwm to zero
Current_Pwm_Limited = 0;//Set limited current pwm to zero
Current_Pwm_Lim_Dith = 0;
Pwm_Dither_Excess_Power = 0;
sei();

//; Set idle pwm to programmed value
Pwm_Motor_Idle = (Pgm_Motor_Idle << 1);

Gov_Target_L = 0;// ; Set target to zero
Gov_Target_H = 0;//
Gov_Integral_L = 0;// ; Set integral to zero
Gov_Integral_H = 0;//
Gov_Integral_X = 0;//
Adc_Conversion_Cnt = 0;//

Flags0 = 0;// ; Clear flags0
Flags1 = 0;// ; Clear flags1
Demag_Detected_Metric = 0;//Clear demag metric

//Motor start beginning
Adc_Conversion_Cnt = TEMP_CHECK_RATE; ; Make sure a temp reading is done
Set_Adc_Ip_Temp();
wait1ms();
Start_Adc();

//read_initial_temp
while(0 != (Get_Adc_Status() & (1 << ADSC))) {};

Temp1 = ADCL;
      Temp2 = ADCH;

Stop_Adc();
if(0 != Temp2)//; Is reading below 256?
{
Temp1 = 0xFF; ; No - set average temperature value to 255
}
Current_Average_Temp_Adc = Temp1;// ; Set initial average temp ADC reading

check_temp_voltage_and_limit_power();

Adc_Conversion_Cnt = TEMP_CHECK_RATE;// ; Make sure a temp reading is done next time
Set_Adc_Ip_Temp();

//; Set up start operating conditions
decode_parameters();// ; (Decode_parameters uses Temp1 and Temp8)
//; Set max allowed power
cli();// ; Disable interrupts to avoid that Requested_Pwm is overwritten
Pwm_Limit = 0xFF;// ; Set pwm limit to max
set_startup_pwm();
Pwm_Limit = Requested_Pwm;
Pwm_Limit_Spoolup = Requested_Pwm;
Pwm_Limit_By_Rpm = Requested_Pwm;
sei();

//Set low pwm again after calling set_startup_pwm
Requested_Pwm = 1;
Current_Pwm = 1;
Current_Pwm_Limited = 1;
Current_Pwm_Lim_Dith = 1;
Spoolup_Limit_Skip = 1;

Spoolup_Limit_Cnt = Auto_Bailout_Armed;

//Begin startup sequence
if(Pgm_Direction == 3)// ; Check if bidirectional operation
{
Flags3 &= ~(1<<PGM_DIR_REV);// ; Set spinning direction. Default fwd
if(0 != (Flags2 & (1 << RCP_DIR_REV)))// ; Check force direction
{
Flags3 |= (1<<PGM_DIR_REV);// ; Set spinning direction
}
}

init_start_bidir_done:
Flags1 |= (1<<MOTOR_SPINNING);// ; Set motor spinning flag
Flags1 |= (1<<STARTUP_PHASE);// ; Set startup phase flag
Startup_Cnt = Zero;// ; Reset counter
comm5comm6();// ; Initialize commutation
comm6comm1();//
initialize_timing();// ; Initialize timing
calc_next_comm_timing();// ; Set virtual commutation point
initialize_timing();// ; Initialize timing
calc_next_comm_timing();//
initialize_timing();// ; Initialize timing

//; Run entry point
//; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated
//; Out_cA changes from low to high
run1:
wait_for_comp_out_high ; Wait zero cross wait and wait for high
//;    setup_comm_wait();// ; Setup wait time from zero cross to commutation
//;    evaluate_comparator_integrity();// ; Check whether comparator reading has been normal
calc_governor_target();// ; Calculate governor target
wait_for_comm();// ; Wait from zero cross to commutation
comm1comm2();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

//; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated
//; Out_cB changes from high to low
run2:
wait_for_comp_out_low();
//;    setup_comm_wait();
//;    evaluate_comparator_integrity();
if(0 != (Flags0 & (1 << GOV_ACTIVE)))
{
calc_governor_prop_error();
}
if(0 == (Flags1 & (1 << HIGH_RPM)))// ; Skip if high rpm
{
set_pwm_limit_low_rpm();
}
if(0 != (Flags1 & (1 << HIGH_RPM)))// ; Do if high rpm
{
set_pwm_limit_high_rpm();
}
wait_for_comm();// ; Wait from zero cross to commutation
comm2comm3();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

//; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated
//; Out_cC changes from low to high
run3:
wait_for_comp_out_high();
//;    setup_comm_wait
//;    evaluate_comparator_integrity
if(0 != (Flags0 & (1 << GOV_ACTIVE)))
{
calc_governor_prop_error();
}
wait_for_comm();// ; Wait from zero cross to commutation
comm3comm4();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

//; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated
//; Out_cA changes from high to low
run4:
wait_for_comp_out_low();
//;    setup_comm_wait();
//;    evaluate_comparator_integrity();
if(0 != (Flags0 & (1 << GOV_ACTIVE)))
{
calc_governor_prop_error();
}
wait_for_comm();// ; Wait from zero cross to commutation
comm4comm5();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

//; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated
//; Out_cB changes from low to high
run5:
wait_for_comp_out_high();
//;    setup_comm_wait
//;    evaluate_comparator_integrity
if(0 != (Flags0 & (1 << GOV_ACTIVE)))
{
calc_governor_prop_error();
}
wait_for_comm();// ; Wait from zero cross to commutation
comm5comm6();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

//; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated
//; Out_cC changes from high to low
run6:

wait_for_comp_out_low();
//;    setup_comm_wait();
//;    evaluate_comparator_integrity();
Start_Adc();
wait_for_comm();// ; Wait from zero cross to commutation
comm6comm1();// ; Commutate
calc_next_comm_timing();// ; Calculate next timing and start advance timing wait
//;    wait_advance_timing();// ; Wait advance timing and start zero cross wait
//;    calc_new_wait_times();//
//;    set_comparator_phase();// ; Set comparator phase
//;    wait_before_zc_scan();// ; Wait zero cross wait and start zero cross timeout

check_temp_voltage_and_limit_power();

//; Check if it is startup
if(0 == (Flags1 & (1 << STARTUP_PHASE)))
{
     goto normal_run_checks;
}

//Set spoolup power variables
Pwm_Limit = Pwm_Spoolup_Beg;// ; Set initial max power
Pwm_Limit_Spoolup = Pwm_Spoolup_Beg;// ; Set initial slow spoolup power
Spoolup_Limit_Cnt = Auto_Bailout_Armed;
Spoolup_Limit_Skip = 1;

//; Check startup counter
Temp2 = 24;// ; Set nominal startup parameters
Temp3 = 12;

if(Startup_Cnt < Temp2)// ; Is counter above requirement?
{
if(New_Rcp < RCP_STOP)// ; Check if pulse is below stop value
{
goto run_to_wait_for_power_on;
}
else
{
goto run1;// ; Continue to run
}
}

Flags1 &= ~(1<<STARTUP_PHASE);// ; Clear startup phase flag
Flags1 |= (1<<INITIAL_RUN_PHASE);// Set initial run phase flag
Initial_Run_Rot_Cntd = Temp3;// ; Set initial run rotation count
#if MODE == 1 //; Tail
Pwm_Limit = 0xff;// ; Allow full power
#elif MODE == 2 // ; Multi
Pwm_Limit = Pwm_Spoolup_Beg;
Pwm_Limit_By_Rpm = Pwm_Spoolup_Beg;
#endif

normal_run_checks:
//; Check if it is initial run phase
if(0 == (Flags1 & (1 << INITIAL_RUN_PHASE)))// ; If not initial run phase - branch
{
goto initial_run_phase_done;
}
if(1 == (Flags0 & (1 << DIR_CHANGE_BRAKE)))// ; If a direction change - branch
{
goto initial_run_phase_done;
}

//; Decrement startup rotation count
if(Initial_Run_Rot_Cntd - 1) == 0) //; Check number of nondamped rotations
{
Flags1 &= ~(1<<INITIAL_RUN_PHASE);//; Clear initial run phase flag
Flags1 |= (1<<MOTOR_STARTED);// ; Set motor started
goto run1
} ; Continue with normal run

normal_run_check_startup_rot:
Initial_Run_Rot_Cntd = Initial_Run_Rot_Cntd - 1;// ; Not zero - store counter
if(New_Rcp >= RCP_STOP)// ; Check if pulse is below stop value
{
goto run1();// ; Continue to run
}

if(Pgm_Direction != 3)// ; Check if bidirectional operation
{
goto run_to_wait_for_power_on;
}

initial_run_phase_done:
//; Reset stall count
Stall_Cnt = 0;
#if MODE == 0 //; Main
//; Check if throttle is zeroed
if(Rcp_Stop_Cnt >= 1)// ; Is number of stop RC pulses above limit?
{
Pwm_Limit_Spoolup = Pwm_Spoolup_Beg;// ; If yes - set initial max powers
Spoolup_Limit_Cnt = Auto_Bailout_Armed;// ; And set spoolup parameters
Spoolup_Limit_Skip = 1;
}
#endif
//; Exit run loop after a given time
Temp1 = RCP_STOP_LIMIT;
if(0 != Pgm_Brake_On_Stop)
{
Temp1 = 3;// ; About 100ms before stopping when brake is set
}
if(Rcp_Stop_Cnt > Temp1)// ; Is number of stop RC pulses above limit?
{
goto run_to_wait_for_power_on;// ; Yes, go back to wait for poweron
}
if(0 != (Flags2 & (1 << RCP_PPM)))
{
if(Rcp_Timeout_Cntd == 0)
{
goto run_to_wait_for_power_on;// ; If it is zero - go back to wait for poweron
}
}
run6_check_dir:
#if MODE >= 1 // ; Tail or multi
if((Pgm_Direction == 3) && //Check if bidirectional operation
((0 == (Flags3 & (1 << PGM_DIR_REV))) != //Check if actual rotation direction
(0 == (Flags2 & (1 << RCP_DIR_REV)))) && //Matches force direction
(0 == (Flags0 & (1 << DIR_CHANGE_BRAKE))))
{
Flags0 |= (1<<DIR_CHANGE_BRAKE);// ; Set brake flag
Pwm_Limit = Pwm_Spoolup_Beg;// ; Set max power while braking
goto run4;// ; Go back to run 4, thereby changing force direction
}
#endif

Temp1 = 0xF0;// ; Default minimum speed

if((0 != (Flags0 & (1 <<DIR_CHANGE_BRAKE)))// ; Is it a direction change?
{
Pwm_Limit = Pwm_Spoolup_Beg;// ; Set max power while braking
Temp1 = 0x20;// ; Bidirectional braking termination speed
}

run6_brake_done:
if(Comm_Period4x_H < Temp1)// ; Is Comm_Period4x more than 32ms (~1220 eRPM)?
{
goto run1;// ; No - go back to run 1
}

//; Yes - stop or turn direction

#if MODE >= 1 //; Tail or multi
if(0 == (Flags0 & (1 << DIR_CHANGE_BRAKE)))// ; If it is not a direction change - stop
{
goto run_to_wait_for_power_on;
}

Flags0 &= ~(1<<DIR_CHANGE_BRAKE);// ; Clear brake flag
Flags3 &= (1<<PGM_DIR_REV);// ; Set spinning direction. Default fwd
if(0 != (Flags2 & (1 << RCP_DIR_REV)))// ; Check force direction
{
Flags3 += (1<<PGM_DIR_REV);// ; Set spinning direction
}
Flags1 |= (1<<INITIAL_RUN_PHASE);
Initial_Run_Rot_Cntd = 18;
Pwm_Limit = Pwm_Spoolup_Beg;// ; Set initial max power
goto run1;// ; Go back to run 1
.ENDIF

run_to_wait_for_power_on_fail:
Stall_Cnt++;// ; Increment stall count
if(0 != New_Rcp)// ; Check if RCP is zero, then it is a normal stop
{
goto run_to_wait_for_power_on_stall_done;
}

run_to_wait_for_power_on:
Stall_Cnt = Zero;

run_to_wait_for_power_on_stall_done:
cli();
switch_power_off();
Temp7 = Pgm_Pwm_Freq;// ; Store setting in Temp7
Pgm_Pwm_Freq = 2;

decode_parameters();// ; (Decode_parameters uses Temp1 and Temp8)

Pgm_Pwm_Freq = Temp7;// ; Restore settings
     Requested_Pwm = 0;// ; Set requested pwm to zero
     Governor_Req_Pwm = 0;// ; Set governor requested pwm to zero
     Current_Pwm = 0;// ; Set current pwm to zero
Current_Pwm_Limited = 0;// ; Set limited current pwm to zero
     Current_Pwm_Lim_Dith = 0;//
     Pwm_Motor_Idle = 0;// ; Set motor idle to zero
Flags0 = 0;// ; Clear flags0
Flags1 = 0;// ; Clear flags1
sei();
wait100ms();// ; Wait for pwm to be stopped
switch_power_off();

if(0 != Pgm_Brake_On_Stop)
{
Brake_FETs_On();
}

run_to_wait_for_power_on_brake_done:
Initialize_Adc();// ; Initialize ADC, to keep reference on for selected ESCs
#if MODE == 0 // ; Main
if((0 != (Flags2 & (1 << RCP_PPM)))
&& (0 == Rcp_Timeout_Cntd))// ; Load RC pulse timeout counter value
{
goto init_no_signal;// ; If it is zero (pulses missing) - go back to detect input signal
}
run_to_next_state_main:
if(Pgm_Main_Rearm_Start >= 1)// Is re-armed start enabled?
{
goto validate_rcp_start;// ; Yes - go back to validate RC pulse
}
//No - do like tail and start immediately

#elif MODE >= 1 //; Tail or multi
if((0 != (Flags2 & (1 << RCP_PPM)))
&& (Stall_Cnt >= 4))//
{
goto init_no_signal;// ; If it is zero (pulses missing) - go back to detect input signal
}
#endif
}
}
}

zlf66778899 · 发表于 2017-3-5 23:08:08

本帖最后由 zlf66778899 于 2017-3-5 23:10 编辑

//; Program by TX routine
void program_by_tx(void)
{
//; Programming mode entry beeps
cli();
success_beep();
sei();

wait1s();
wait1s();

//; Start at function 1, parameter value 1
#if MODE == 0
XH = 12;// ; Main has 11 functions
#elif MODE == 1
XH = 10;// ; Tail has 9 functions
#elif MODE == 2
XH = 12;// ; Multi has 11 functions
#endif
for(Tx_Pgm_Func_No = 1; Tx_Pgm_Func_No < XH; Tx_Pgm_Func_No++)
{
cli();
#if MODE == 0
Temp1 = TX_PGM_PARAMS_MAIN[Tx_Pgm_Func_No - 1];
#elif MODE == 1
Temp1 = TX_PGM_PARAMS_TAIL[Tx_Pgm_Func_No - 1];
#elif MODE == 2
Temp1 = TX_PGM_PARAMS_MULTI[Tx_Pgm_Func_No - 1];
#endif
sei();
Temp1++;
for(Tx_Pgm_Paraval_No = 1; Tx_Pgm_Paraval_No < Temp1; Tx_Pgm_Paraval_No++)
{
for(Tx_Pgm_Beep_No = 0; Tx_Pgm_Beep_No < 3; Tx_Pgm_Beep_No++)
{
cli();
function_paraval_beep();
sei();
Temp5 = 5;// ; Wait is 5x 200ms
for(Temp5 = 5; Temp5 != 0; )
{
Temp6 = New_Rcp// ; Load RC pulse
wait200ms();

if(Temp6 != New_Rcp)// ; Is RC pulse stable? (Avoid issues from 3in1 interference)
{
continue;
}

if(New_Rcp < RCP_STOP)// ; Below stop?
{
store_new_value_in_ram();// ; Yes - store new value in RAM
cli();// ; Disable all interrupts
store_all_in_eeprom();// ; Store all values in EEPROM
success_beep();// ; Beep success
Enable_Watchdog();// ; Generate hardware reset from watchdog
wait1s();
}

if(New_Rcp < RCP_MAX)// ; Below max?
{
goto function_next;// ; Yes - branch
}

Temp5--;
}
}
wait1s();
}
function_next：
wait1s();
wait1s();
}
set_default_parameters();// ; Load all defaults
cli();// ; Disable all interrupts
store_all_in_eeprom();// ; Erase flash and program
Enable_Watchdog();// ; Generate hardware reset from watchdog
wait1s();
}

cpholr1 · 发表于 2017-3-6 01:04:04

厉害了楼主

higeo · 发表于 2017-3-6 08:09:12

楼主好样的，所有程序都转完了吗

zqbing · 发表于 2017-3-6 08:49:10

楼主厉害了！编译都通过了吧，运行有没有问题呢

zlf66778899 · 发表于 2017-3-6 09:23:22

zqbing 发表于 2017-3-6 08:49
楼主厉害了！编译都通过了吧，运行有没有问题呢

只是为了参考，所以没进行细致的校对，更没有进行编译

su33691 · 发表于 2017-3-6 10:25:15

不管怎样，都要顶楼主一下。

zlf66778899 · 发表于 2017-3-6 17:28:08

;**** **** **** **** **** **** **** **** **** **** **** **** ****
;
; RC pulse processing interrupt routine
;
; No assumptions
; Can come from int0 or icp
;
;**** **** **** **** **** **** **** **** **** **** **** **** ****
void rcp_int(void):// ; Used for RC pulse timing
{
I_Sreg = SREG;

//; Get the timer counter values
Get_Rcp_Capture_Values();// I_Temp1, I_Temp2

//; Disable RCP interrupts
Flags2 &= ~(1<<RCP_INT_NESTED_ENABLED);// ; Set flag default to disabled
if (0 != Get_Rcp_Int_Enable_State())// XL ; Get rcp interrupt state
{
Flags2 |= (1<<RCP_INT_NESTED_ENABLED);// ; Set flag to enabled
}

Rcp_Int_Disable();// XL ; Disable rcp interrupts
T0_Int_Disable();// XL ; Disable timer0 interrupts
sei();// ; Enable interrupts
//; Check which edge it is

if(0 == (Flags2 & (1 << RCP_EDGE_NO)))// ; Is it a first edge trig?
{
Rcp_Int_Second();// XL ; Yes - set second edge trig
Flags2 |= (1<<RCP_EDGE_NO);// ; Set second edge flag

//; Read RC signal level
//Test RC signal level
if(0 == (Read_Rcp_Int() & (1 << Rcp_In)))// ; Is it high?
{
//; Prepare for next interrupt
Rcp_Int_First();// XL ; Set interrupt trig to first again
Rcp_Clear_Int_Flag();// XL ; Clear interrupt flag
Flags2 &= ~(1<<RCP_EDGE_NO);// ; Set first edge flag

if(0 == (Flags2 & (1 << RCP_PPM)))
{
I_Temp1 = RCP_MIN;// ; Set RC pulse value to minimum
if(0 == (Read_Rcp_Int() & (1 << Rcp_In)))// ; Is it high?
{
//; RC pulse value accepted
New_Rcp = I_Temp1;// ; Store new pulse length
Flags2 |= (1<<RCP_UPDATED);//      ; Set updated flag

if(0 != (Flags0 & (1 << RCP_MEAS_PWM_FREQ)))// ; Is measure RCP pwm frequency flag set?
{
//Clear all pwm frequency flags
Flags3 &= ~((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ));
Flags3 |= I_Temp4;// ; Store pwm frequency value in flags

Flags2 &= ~(1<<RCP_PPM);// ; Default, flag is not set (PWM)

if(0 == I_Temp4)// ; Check if all flags are cleared
{
Flags2 |= (1<<RCP_PPM);// ; Flag is set (PPM)
}
}
}
}
Rcp_Timeout_Cntd = RCP_TIMEOUT;// ; Set timeout count to start value
if(0 != (Flags2 & (1 << RCP_PPM)))
{
Rcp_Timeout_Cntd = RCP_TIMEOUT_PPM;// ; No flag set means PPM. Set timeout count
}
if((0 == (Flags0 & (1 << RCP_MEAS_PWM_FREQ))) && // ; Is measure RCP pwm frequency flag set?
(0 == (Flags2 & (1 << RCP_PPM))))
{
Flags2 &= ~(1<<RCP_INT_NESTED_ENABLED);// ; Set flag to disabled
}
}
else
{
//; RC pulse was high, store RC pulse start timestamp
Rcp_Prev_Edge_L = I_Temp1;
Rcp_Prev_Edge_H = I_Temp2;
}
goto rcp_int_exit;// ; Exit
}

rcp_int_second_meas_pwm_freq:
//; Prepare for next interrupt
Rcp_Int_First();// XL ; Set first edge trig
Flags2 &= ~(1<<RCP_EDGE_NO);// ; Set first edge flag
//; Check if pwm frequency shall be measured
if(0 != (Flags0 & (1 << RCP_MEAS_PWM_FREQ)))// ; Is measure RCP pwm frequency flag set?
{
//; Set second edge trig only during pwm frequency measurement
Rcp_Int_Second();// XL ; Set second edge trig
Rcp_Clear_Int_Flag();// XL ; Clear interrupt flag
Flags2 |= (1<<RCP_EDGE_NO);// ; Set second edge flag

//; Store edge data to RAM
Rcp_Edge_L = I_Temp1;
Rcp_Edge_H = I_Temp2;

//; Calculate pwm frequency
*((uint16_t *)&I_Temp1) -= ((uint16_t)Rcp_PrePrev_Edge_H * 256 + Rcp_PrePrev_Edge_L);
I_Temp3 = 0;
I_Temp4 = 0;

//; Check if pulse is too short
if(*((uint16_t *)&I_Temp1) < 140) //If pulse below 70us, not accepted
{
Rcp_Period_Diff_Accepted = 0;// ; Set not accepted
}
else if(0 == Pgm_Enable_PWM_Input)// ; Check if PWM input is enabled
{
}
//; Check if pwm frequency is 12kHz
else if(*((uint16_t *)&I_Temp1) < 200)// ; If below 100us, 12kHz pwm is assumed
{
I_Temp4 = (1<<RCP_PWM_FREQ_12KHZ);
I_Temp3 = 10;// ; Set period tolerance requirement (LSB)
}
//; Check if pwm frequency is 8kHz
else if(*((uint16_t *)&I_Temp1) < 360)// ; If below 180us, 8kHz pwm is assumed
{
I_Temp4 = (1<<RCP_PWM_FREQ_8KHZ);
I_Temp3 = 15;// ; Set period tolerance requirement (LSB)
}
//; Check if pwm frequency is 4kHz
else if(*((uint16_t *)&I_Temp1) < 720)// ; If below 360us, 4kHz pwm is assumed
{
I_Temp4 = (1<<RCP_PWM_FREQ_4KHZ);
I_Temp3 = 30;// ; Set period tolerance requirement (LSB)
}
//; Check if pwm frequency is 2kHz
else if(*((uint16_t *)&I_Temp1) < 1440)// ; If below 1440us, 2kHz pwm is assumed
{
I_Temp4 = (1<<RCP_PWM_FREQ_2KHZ);
I_Temp3 = 60;// ; Set period tolerance requirement (LSB)
}
//; Check if pwm frequency is 1kHz
else if(*((uint16_t *)&I_Temp1) < 2200)// ; If below 1100us, 1kHz pwm is assumed __zlf__???
{
I_Temp4 = (1<<RCP_PWM_FREQ_1KHZ);
I_Temp3 = 120;// ; Set period tolerance requirement (LSB)
}

//; Calculate difference between this period and previous period
if(*((uint16_t *)&I_Temp1) >= ((uint16_t)Rcp_PrePrev_Period_H * 256 + Rcp_PrePrev_Period_L))
{
*((uint16_t *)&I_Temp5) = *((uint16_t *)&I_Temp1) - ((uint16_t)Rcp_PrePrev_Period_H * 256 + Rcp_PrePrev_Period_L);
}
else
{
*((uint16_t *)&I_Temp5) = ((uint16_t)Rcp_PrePrev_Period_H * 256 + Rcp_PrePrev_Period_L) - *((uint16_t *)&I_Temp1);
}

//; Check difference
Rcp_Period_Diff_Accepted = 0;// ; Set not accepted as default

if(0 == I_Temp3)//; Check if no tolerance requirement is set
{
XL = 8;
}
else
{
XL = 0;
}

if(*((uint16_t *)&I_Temp5) < ((uint16_t)XL * 256 + I_Temp3))
{
Rcp_Period_Diff_Accepted = 1;// ; Set accepted
}

//; Store previous period
Rcp_Prev_Period_L = I_Temp1;
Rcp_Prev_Period_H = I_Temp2;

//; Restore edge data from RAM
I_Temp1 = Rcp_Edge_L;
I_Temp2 = Rcp_Edge_H;

//; Store pre previous edge
Rcp_PrePrev_Edge_L = I_Temp1;
Rcp_PrePrev_Edge_H = I_Temp2;
I_Temp1 = RCP_VALIDATE
}
else
{
//; RC pulse edge was second, calculate new pulse length
*((uint16_t *)&I_Temp1) -= ((uint16_t)Rcp_PrePrev_Edge_H * 256 + Rcp_PrePrev_Edge_L);

if((0 == (Flags3 & (1 << RCP_PWM_FREQ_12KHZ))) &&// ; Is RC input pwm frequency 12kHz?
(0 == (Flags3 & (1 << RCP_PWM_FREQ_8KHZ))))// ; Is RC input pwm frequency 8kHz?
{
if(0 != (Flags3 & (1 << RCP_PWM_FREQ_2KHZ)))// ; Is RC input pwm frequency 2kHz?
{
(*((uint16_t *)&I_Temp1)) >>= 1;//; 2kHz. Divide by 2
}
if(0 != (Flags3 & (1 << RCP_PWM_FREQ_1KHZ)))// ; Is RC input pwm frequency 1kHz?
{
(*((uint16_t *)&I_Temp1)) >>= 2;//; 1kHz. Divide by 2 again
}

if((0 == (Flags3 & (1 << RCP_PWM_FREQ_4KHZ))) &&// ; Is RC input pwm frequency 4kHz?
(0 == (Flags3 & (1 << RCP_PWM_FREQ_2KHZ))) &&// ; Is RC input pwm frequency 2kHz?
(0 == (Flags3 & (1 << RCP_PWM_FREQ_1KHZ))))// ; Is RC input pwm frequency 1kHz?
{

if(0 != (Flags3 & (1 << RCP_PPM_ONESHOT125)))
{
(*((uint16_t *)&I_Temp5)) = (*((uint16_t *)&I_Temp1));// ; Oneshot125 - move to I_Temp5/6
}
else
{
(*((uint16_t *)&I_Temp5)) = ((*((uint16_t *)&I_Temp1)) >> 3);//PPM. Divide by 2 (to bring range to 256) and move to I_Temp5/6
}

//; Skip range limitation if pwm frequency measurement
if((0 == (Flags0 & (1 << RCP_MEAS_PWM_FREQ))) &&
//; Check if 2160us or above (in order to ignore false pulses)
((((*((uint16_t *)&I_Temp5)) >= 540) ||// ; Is pulse 2160us or higher?
//; Check if below 800us (in order to ignore false pulses)
((*((uint16_t *)&I_Temp5)) < 200)))) //Is pulse below 800us?
{
//__zlf__????? 255的处理是否合理
XL = Rcp_Outside_Range_Cnt + 1;
if(XL != 0)
{
Rcp_Outside_Range_Cnt++;
}

if(XL >= 10)// ; Allow a given number of outside pulses
{
New_Rcp = Zero;// ; Set pulse length to zero
Flags2 |= (1<<RCP_UPDATED);//      ; Set updated flag
}
goto rcp_int_set_timeout;
}
//; Decrement outside range counter
if(0 != Rcp_Outside_Range_Cnt)
{
Rcp_Outside_Range_Cnt--;
}

//; Calculate "1000us" plus throttle minimum
I_Temp3 = 0;// ; Set 1000us as default minimum
I_Temp2 = Pgm_Direction;// ; Check if bidirectional operation (store in I_Temp2)

if(0 == (Flags3 & (1 << FULL_THROTTLE_RANGE)))// ; Check if full range is chosen
{
I_Temp3 = Pgm_Ppm_Min_Throttle;// ; Min throttle value is in 4us units

#if MODE >= 1 //; Tail or multi
if(I_Temp2 == 3)
{
I_Temp3 = Pgm_Ppm_Center_Throttle;// ; Center throttle value is in 4us units
}
#endif
}

(*((uint16_t *)&I_Temp5)) -= ((uint16_t)I_Temp3 + 250);// Subtract minimum// Add 1000us to minimum

I_Temp1 = SREG & (1<<SREG_C);

#if MODE >= 1 //; Tail or multi
if(I_Temp2 == 3)// ; Check if bidirectional operation
{
if(I_Temp1 != 0)
{
if(0 == (Flags2 & (1 << RCP_DIR_REV)))
{
Flags2 |= (1<<RCP_DIR_REV);
}
}
else
{
if(0 != (Flags & (1 << RCP_DIR_REV)))
{
Flags2 &= ~(1<<RCP_DIR_REV);
}
}
}
#endif

if(0 != I_Temp1)
{
#if MODE >= 1 // ; Tail or multi
if(I_Temp2 == 3)// ; Check if bidirectional operation
{
//; Change sign - invert and subtract minus one
(*((uint16_t *)&I_Temp5)) = 0x10000 - (*((uint16_t *)&I_Temp5));
}
else
#endif
{
*((uint16_t *)&I_Temp1) = RCP_MIN;
goto rcp_int_pwm_divide_done
}
}
#if MODE >= 1 //; Tail or multi
if(I_Temp2 == 3)// ; Check if bidirectional operation
{
(*((uint16_t *)&I_Temp5)) <<= 1;// ; Multiply value by 2
if(*((uint16_t *)&I_Temp5) < 10)
{
*((uint16_t *)&I_Temp5) = 10;
}
}
#endif
if(*((uint16_t *)&I_Temp5)) < RCP_MAX)// ; Check that RC pulse is within legal range (max 255)
{
I_Temp1 = (uint16_t)I_Temp5 * Ppm_Throttle_Gain / 256;// ; Multiply throttle value by gain
Mul_Res_L = (uint16_t)I_Temp5 * Ppm_Throttle_Gain % 256;//

if(0 != (I_Temp1 & 0x80))
{
*((uint16_t *)&I_Temp1) = RCP_MAX;
}
else
{
*((uint16_t *)&I_Temp1) = (I_Temp1 << 1) + (Mul_Res_L >> 7);
}
goto rcp_int_limited;
}
*((uint16_t *)&I_Temp1) = RCP_MAX;
}
else
{
*((uint16_t *)&I_Temp1) >>= 1;
}
}

if(0 != (Flags3 & (1 << RCP_PWM_FREQ_12KHZ)))// ; Is RC input pwm frequency 12kHz?
{
if(0 != I_Temp2)// ; Yes - check that value is not more than 255
{
I_Temp1 = RCP_MAX;
}
*((uint16_t *)&I_Temp1) += (I_Temp1 >> 1);// ; Multiply by 1.5
}
//; Check that RC pulse is within legal range
if(*((uint16_t *)&I_Temp1)) >= RCP_MAX)
{
I_Temp1 = RCP_MAX;
}
}
rcp_int_limited:
//; RC pulse value accepted
New_Rcp = I_Temp1;// ; Store new pulse length
Flags2 |= (1<<RCP_UPDATED);//      ; Set updated flag

if(0 != (Flags0 & (1 << RCP_MEAS_PWM_FREQ)))// ; Is measure RCP pwm frequency flag set?
{
//Clear all pwm frequency flags
Flags3 &= ~((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ));
Flags3 |= I_Temp4;// ; Store pwm frequency value in flags

Flags2 &= ~(1<<RCP_PPM);// ; Default, flag is not set (PWM)

if(0 == I_Temp4)// ; Check if all flags are cleared
{
Flags2 |= (1<<RCP_PPM);// ; Flag is set (PPM)
}
}
rcp_int_set_timeout:
Rcp_Timeout_Cntd = RCP_TIMEOUT;// ; Set timeout count to start value
if(0 != (Flags2 & (1 << RCP_PPM)))
{
Rcp_Timeout_Cntd = RCP_TIMEOUT_PPM;// ; No flag set means PPM. Set timeout count
}
rcp_int_ppm_timeout_set:
if((0 == (Flags0 & (1 << RCP_MEAS_PWM_FREQ))) && // ; Is measure RCP pwm frequency flag set?
(0 == (Flags2 & (1 << RCP_PPM))))
{
Flags2 &= ~(1<<RCP_INT_NESTED_ENABLED);// ; Set flag to disabled
}
rcp_int_exit:// ; Exit interrupt routine
if(0 == (Flags2 & (1 << RCP_PPM)))// ; If flag is set (PPM) - branch
{
Rcp_Skip_Cntd = RCP_SKIP_RATE;// ; Load number of skips
}
cli(); ; Disable interrupts
T0_Int_Enable();// XL ; Enable timer0 interrupts
if(0 != (Flags2 & (1 << RCP_INT_NESTED_ENABLED)))// Restore rcp interrupt state
{
Rcp_Int_Enable();
}
rcp_int_ret:
SREG = I_Sreg;
}

zenghui · 发表于 2017-3-6 20:12:45

楼主的钻研精神让我汗颜啊

zlf66778899 · 发表于 2017-3-6 22:21:53

//// Timer0 interrupt routine
void t0_int(void)// // Happens every 128us
{
I_Sreg = SREG;//

//// Disable RCP interrupts
Flags2 &= ~(1<<RCP_INT_NESTED_ENABLED); // Set flag default to disabled

if(0 != Get_Rcp_Int_Enable_State()) // Get rcp interrupt state
{
Flags2 |= (1<<RCP_INT_NESTED_ENABLED); // Set flag to enabled
}
Rcp_Int_Disable(); // Disable rcp interrupts
T0_Int_Disable(); // Disable timer0 interrupts
sei(); // Enable interrupts
// Check RC pulse timeout counter
if(0 != Rcp_Timeout_Cntd) // RC pulse timeout count zero?
{
// Decrement timeout counter (if PWM)
if(0 == (Flags2 & (1 << RCP_PPM)))
{
Rcp_Timeout_Cntd--; // No - decrement
}
}
else
{
do
{
// Timeout counter has reached zero, pulses are absent
I_Temp1 = RCP_MIN; // RCP_MIN as default
I_Temp2 = RCP_MIN;

if(0 == (Flags2 & (1 << RCP_PPM)))
{
if(0 != (Read_Rcp_Int() & (1 << Rcp_In))) // Look at value of Rcp_In
{
I_Temp1 = RCP_MAX; // Yes - set RCP_MAX
}
Rcp_Int_First(); // Set interrupt trig to first again
Rcp_Clear_Int_Flag(); // Clear interrupt flag
Flags2 &= ~(1<<RCP_EDGE_NO); // Set first edge flag

if(0 != (Read_Rcp_Int() & (1 << Rcp_In))) // Look at value of Rcp_In
{
I_Temp2 = RCP_MAX; // Yes - set RCP_MAX
}
}
}while(I_Temp1 != I_Temp2);

if((0 != (Flags0 & (1 << RCP_MEAS_PWM_FREQ))) ||// Is measure RCP pwm frequency flag set?
(0 == (Flags2 & (1 << RCP_PPM))))
{
Rcp_Timeout_Cntd = RCP_TIMEOUT; // For PWM, set timeout count to start value
}

New_Rcp = I_Temp1; // Store new pulse length
sbr Flags2, (1<<RCP_UPDATED) // Set updated flag
// Check if zero
if(0 != I_Temp1) // Test new pulse value
{
sts Rcp_Stop_Cnt, Zero // Reset rcp stop counter
}
}

if(0 == (Flags2 & (1 << RCP_PPM)))
{
// Check RC pulse skip counter
if(0 != Rcp_Skip_Cntd)// If RC pulse skip count is zero - end skipping RC pulse detection
{
// Decrement skip counter (only if edge counter is zero)
sts Rcp_Skip_Cntd--;
}
else
{
// Skip counter has reached zero, start looking for RC pulses again
Flags2 |= (1<<RCP_INT_NESTED_ENABLED); // Set flag to enabled
Rcp_Clear_Int_Flag(); // Clear interrupt flag
}
}

// Process updated RC pulse
if(0 != (Flags2 & (1 << RCP_UPDATED))) // Is there an updated RC pulse available? No - update pwm limits and exit
{

I_Temp1 = New_Rcp; // Load new pulse value

if(0 == (Flags0 & (1 << RCP_MEAS_PWM_FREQ))) // If measure RCP pwm frequency flag set - do not clear flag
{
Flags2 &= ~(1<<RCP_UPDATED); // Flag that pulse has been evaluated
}
// Use a gain of 1.0625x for pwm input if not governor mode
if(0 == (Flags2 & (1 << RCP_PPM)))
{
#if MODE != 0 // Main - do not adjust gain
#if MODE == 2 // Multi
if(Pgm_Gov_Mode == 4) // Closed loop mode?
#endif
{
// Limit the maximum value to avoid wrap when scaled to pwm range
if(I_Temp1 >= 240) // 240 = (255/1.0625) Needs to be updated according to multiplication factor below
{
ldi I_Temp1, 240 // Set requested pwm to max
}

// Multiply by 1.0625 (optional adjustment gyro gain)
XL = (I_Temp1 >> 4);
I_Temp1 += XL;
// Adjust tail gain
I_Temp2 = Pgm_Motor_Gain; // Is gain 1?
if(I_Temp2 != 3)
{
XL >>= 2; // After this "0.25"

if(0 == (I_Temp2 & (1 << 0))) // (I_Temp2 has Pgm_Motor_Gain)
{
XL >>= 1; // After this "0.125"
}

if(0 == (I_Temp2 & (1 << 2))) // (I_Temp2 has Pgm_Motor_Gain)
{
I_Temp1 = XL - I_Temp1; // Apply negative correction
}
else
{
if((uint16_t)I_Temp1 + XL < 256)
{
I_Temp1 += XL; // Apply positive correction
}
else
{
I_Temp1 = 0xFF; // Yes - limit
}
}
}
}
#endif
}
#if MODE == 1 // Tail - limit minimum pwm
// Limit minimum pwm
if(Pwm_Motor_Idle < XL) // Is requested pwm lower than minimum?
{
I_Temp1 = XL; // Yes - limit pwm to Pwm_Motor_Idle
}
#endif

// Update requested_pwm
Requested_Pwm = I_Temp1; // Set requested pwm
#if MODE >= 1 // Tail or multi
// Boost pwm during direct start
if((0 != (Flags1 & ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE)))) &&
(0 == (Flags1 & (1 << MOTOR_STARTED)))) // Do not boost when changing direction in bidirectional mode
{
XL = (Stall_Cnt << 2); // Add an extra power boost during start
I_Temp2 = (Pwm_Spoolup_Beg >> 2); // Set 25% of max startup power as minimum power
if(I_Temp2 >= I_Temp1)
{
I_Temp1 = I_Temp2;
}
if((uint16_t)I_Temp1 + XL < 256)
{
Requested_Pwm = I_Temp1 + XL;
}
else
{
Requested_Pwm = 255;
}
}
#endif
}

#if MODE == 0 || MODE == 2 // Main or multi
if(Governor == 4)
#endif
{
Current_Pwm = Requested_Pwm; // Set equal as default
#if MODE >= 1 // Tail or multi
// Set current_pwm_limited
I_Temp1 = Current_Pwm;
if(I_Temp1 >= Pwm_Limit) // Check against limit
{
I_Temp1 = Pwm_Limit; // Limit pwm
}

#if MODE == 2 // Multi
// Limit pwm for low rpms
if(I_Temp1 >= Pwm_Limit_By_Rpm) // Check against limit
{
I_Temp1 = Pwm_Limit_By_Rpm // Limit pwm
}
#endif

Current_Pwm_Limited = I_Temp1;
// Dither
XL = Pwm_Dither_Decoded;
if(0 != Pwm_Dither_Decoded) // Load pwm dither
{
I_Temp2 = I_Temp1 - Pwm_Dither_Decoded;
if(I_Temp1 < XL) // Calculate pwm minus dither value
{
XL = I_Temp1; // Set dither level to current pwm
I_Temp2 = 0; // Set pwm minus dither
}
I_Temp4 = XL; // Store dither value in I_Temp4
I_Temp3 = (XL << 1);
I_Temp3 &= ~Random; // And with double dither value
if((uint16_t)I_Temp3 + I_Temp2 >= 256)
{
if(I_Temp4 >= Pwm_Dither_Excess_Power)
{
sts Pwm_Dither_Excess_Power++;
}
I_Temp1 = 255; // Set power to max
}
else
{
I_Temp3 += I_Temp2; // Add pwm minus dither
if(0 != Pwm_Dither_Excess_Power) // Get excess power
{
Pwm_Dither_Excess_Power--;
}
I_Temp3 += Pwm_Dither_Excess_Power; // Add excess power from previous cycles
if((uint16_t)I_Temp3 + Pwm_Dither_Excess_Power >= 256)
{
I_Temp1 = 255; // Set power to max
}
else
{
I_Temp1 = I_Temp3;
}
}
}
Current_Pwm_Lim_Dith = I_Temp1;
#if DAMPED_MODE_ENABLE == 1
// Skip damping fet switching for high throttle
Flags1 &= ~(1<<SKIP_DAMP_ON);
if(I_Temp1 >= 248)
{
Flags1 |= (1<<SKIP_DAMP_ON);
}
#endif
#endif
}
// Increment counter and check if high "interrupt"
sts Timer0_Int_Cnt++;
if(Timer0_Int_Cnt != 0)
{
cli(); // Disable interrupts
T0_Int_Enable(); // Enable timer0 interrupts

if(0 != (Flags2 & (1 << RCP_INT_NESTED_ENABLED)))// Restore rcp interrupt state
{
Rcp_Int_Enable();
}
SREG = I_Sreg;
return;
}
// Every 256th interrupt (happens every 32ms)
Timer2_X++;

I_Temp1 = GOV_SPOOLRATE; // Load governor spool rate

// Check RC pulse timeout counter (used here for PPM only)
I_Temp2 = Rcp_Timeout_Cntd; // RC pulse timeout count zero?
if(0 != I_Temp2)
{
// Decrement timeout counter (if PPM)
if(0 != (Flags2 & (1 << RCP_PPM)))
{
Rcp_Timeout_Cntd--;
}
}
// Check RC pulse against stop value
if(New_Rcp >= RCP_STOP) // Check if pulse is below stop value
{
// RC pulse higher than stop value, reset stop counter
Rcp_Stop_Cnt = 0; // Reset rcp stop counter
}
else
{
// RC pulse less than stop value
Auto_Bailout_Armed = 0; // Disarm bailout
Spoolup_Limit_Cnt = 0;
if(Rcp_Stop_Cnt < 255)
{
Rcp_Stop_Cnt++;
}
}

#if MODE == 0 // Main
do
{
// Update governor variables
I_Temp4 = Requested_Pwm; // Load requested pwm

I_Temp2 = Pgm_Gov_Mode; // Governor target by arm mode?
if((I_Temp2 == 2) &&
((0 != (Flags0 & (1 << GOV_ACTIVE))) ||//Is governor active?
//__zlf__ ??? 此处比较是否有问题
(I_Temp2 != 2)) &&
(I_Temp4 >= 50) // Is requested pwm below 20%? (Requested_Pwm in I_Temp4)
{
Requested_Pwm = Gov_Arm_Target; // Yes - load arm target
}
else if((I_Temp2 == 3) && // Governor target by setup mode? (Pgm_Gov_Mode in I_Temp2)
(0 != (Flags0 & (1 << GOV_ACTIVE))) && // Is governor active?
(I_Temp4 >= 50)) // Is requested pwm below 20%? (Requested_Pwm in I_Temp4)
{
Requested_Pwm = Pgm_Gov_Setup_Target; // Gov by setup - load setup target
}

I_Temp2 = Governor_Req_Pwm;

if(I_Temp2 > I_Temp4) // Is governor requested pwm equal to requested pwm? (Requested_Pwm in I_Temp4)
{
I_Temp2--; // No - if higher, then decrement
}
else if(I_Temp2 < I_Temp4)
{
I_Temp2++; // Increment
}

Governor_Req_Pwm = I_Temp2; // Store governor requested pwm
}
while(0 != --I_Temp1); // Decrement spoolrate variable

I_Temp2 = Spoolup_Limit_Cnt; // Load spoolup count
if(++I_Temp2 == 0) // Increment
{
I_Temp2--; // Yes - decrement
}
Spoolup_Limit_Cnt = I_Temp2;

if(--Spoolup_Limit_Skip == 0)
{
Spoolup_Limit_Skip = 1; // Reset skip count. Default is fast spoolup
I_Temp1 = 8; // Default fast increase for spoolup time of zero
if(0 != Main_Spoolup_Time_3x)
{
I_Temp1 = 5; // Default fast increase
if(I_Temp2 < Main_Spoolup_Time_3x) // No spoolup until 3*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
{
goto t0h_int_exit;
}
else if(I_Temp2 < Main_Spoolup_Time_10x) // Slow spoolup until 10*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
{
I_Temp1 = 1; // Slow initial spoolup
Spoolup_Limit_Skip = 3;
}
else if(I_Temp2 < Main_Spoolup_Time_15x) // Faster spoolup until 15*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
{
I_Temp1 = 1; // Faster middle spoolup
Spoolup_Limit_Skip = I_Temp1;
}
}
// Do not increment spoolup limit if higher pwm is not requested, unless governor is active
I_Temp6 = Pwm_Limit_Spoolup; // Load pwm limit spoolup
I_Temp5 = Current_Pwm; // Load current pwm
if(I_Temp6 >= I_Temp5)
{
if(Pgm_Gov_Mode == 4) // Governor mode?
{
goto t0h_int_rcp_bailout_arm; // No - branch
}
if(0 == (Flags0 & (1 << GOV_ACTIVE))) // Is governor active?
{
Pwm_Limit_Spoolup = I_Temp5; // Set limit to what current pwm is
if(++I_Temp2 != 0) // Check if spoolup limit count is 255
{
Spoolup_Limit_Cnt = Main_Spoolup_Time_3x; // Stay in an early part of the spoolup sequence (unless "bailout" ramp)
}
Spoolup_Limit_Skip = 1; // Set skip count
Governor_Req_Pwm = 60; // Set governor requested speed to ensure that it requests higher speed
// 20=Fail on jerk when governor activates
// 30=Ok
// 100=Fail on small governor settling overshoot on low headspeeds
// 200=Fail on governor settling overshoot
goto t0h_int_exit; // Exit
}
}

if((uint16_t)Pwm_Limit_Spoolup + I_Temp1 >= 256) // Increment spoolup pwm
{
Pwm_Limit_Spoolup = 255;
}
else
{
Pwm_Limit_Spoolup = Pwm_Limit_Spoolup + I_Temp1;
}
t0h_int_rcp_bailout_arm:
if(Pwm_Limit_Spoolup == 255)
{
Auto_Bailout_Armed = 255;// Arm bailout
Spoolup_Limit_Cnt = 255
}
}
#endif
t0h_int_exit:
cli(); // Disable interrupts
T0_Int_Enable(); // Enable timer0 interrupts

if(0 != (Flags2 & (1 >> RCP_INT_NESTED_ENABLED)))// Restore rcp interrupt state
{
Rcp_Int_Enable();
}

SREG = I_Sreg;
}

zlf66778899 · 发表于 2017-3-6 22:31:16

//; Timer 1 output compare A interrupt
void t1oca_int(void)
{
II_Sreg = SREG;
T1oca_Int_Disable();// ; Disable timer1 OCA interrupt
Flags0 &= ~(1<<OC1A_PENDING);// ; Flag that OC1A value is passed

OCR1A = TCNT1 + (uint16_t)Next_Wt_H * 256 + Next_Wt_L;
SREG = II_Sreg;
}

zlf66778899 · 发表于 2017-3-7 12:22:18

//// Timer2 interrupt routine
//// Assumptions: Z register must be set to desired pwm_nfet_on label
//// Requirements: I_Temp variables can NOT be used
void t2_int(void)// // Used for pwm control
{
II_Sreg = SREG//
// Check if pwm is on
if(0 == (Flags0 & (1 << PWM_ON))) // Is pwm on?
{
// Pwm on cycle
if(0 != Current_Pwm_Limited)
{
switch(Z) // Jump to pwm on routines. Z should be set to one of the pwm_nfet_on labels
{
case pwm_nofet: // Dummy pwm on cycle
{
break;
}
case pwm_afet: // Pwm on cycle afet on
{
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
AnFET_on();
}
break;
}
case pwm_bfet: // Pwm on cycle afet on
{
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
BnFET_on();
}
break;
}
case pwm_cfet: // Pwm on cycle afet on
{
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
CnFET_on();
}
break;
}
case pwm_afet_damped:
{
ApFET_off();
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
#if NFETON_DELAY != 0
for(YL = NFETON_DELAY; YL != 0; YL--) // Set delay
{
}
#endif
AnFET_on(); // Switch nFET
}
break;
}
case pwm_bfet_damped:
{
BpFET_off();
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
#if NFETON_DELAY != 0
for(YL = NFETON_DELAY; YL != 0; YL--) // Set delay
{
}
#endif
BnFET_on(); // Switch nFET
}
break;
}
case pwm_cfet_damped:
{
CpFET_off();
if((0 != (Flags1 & (1 << MOTOR_SPINNING))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
#if NFETON_DELAY != 0
for(YL = NFETON_DELAY; YL != 0; YL--) // Set delay
{
}
#endif
pwm_cfet_damped_done:
CnFET_on(); // Switch nFET
}
break;
}
}
t2_int_pwm_on_exit:
// Set timer for coming on cycle length
YL = ~Current_Pwm_Limited; // Load current pwm
if(0 != (Flags2 & (1 << PGM_PWM_HIGH_FREQ))) // Use half the time when pwm frequency is high
{
YL = (YL >> 1) | 0X80;
}
Set_TCNT2 = YL; // Write start point for timer
// Set other variables
Flags0 |= (1<<PWM_ON); // Set pwm on flag
}
t2_int_pwm_on_ret:
// Exit interrupt
out SREG = II_Sreg
return;
}
else
{
// Pwm off cycle
t2_int_pwm_off:
YL = Current_Pwm_Lim_Dith;
if(0 != (Flags2 & (1 << PGM_PWM_HIGH_FREQ))) // Use half the time when pwm frequency is high
{
YL = (YL >> 1) | 0X80;
}
Set_TCNT2 = YL; // Load new timer setting
// Clear pwm on flag
Flags0 &= ~(1<<PWM_ON);
// Set full PWM (on all the time) if current PWM near max. This will give full power, but at the cost of a small "jump" in power
if(Current_Pwm_Lim_Dith != 0xFF) // Full pwm?
{
#if DAMPED_MODE_ENABLE == 1
// Do not execute pwm off when stopped
if(0 == (Flags1 & (1 << MOTOR_SPINNING)))
{
SREG = II_Sreg
return;
}

// If damped operation, set pFETs on in pwm_off
if(0 == (Flags2 & (1 << PGM_PWMOFF_DAMPED))) // Damped operation?
#endif
{
// Separate exit commands here for minimum delay
SREG = II_Sreg;
All_nFETs_Off(); // Switch off all nfets
return;
}
t2_int_pwm_off_damped:
#if PFETON_DELAY < 128
All_nFETs_Off(); // Switch off all nfets
if((0 == (Flags1 & (1 << SKIP_DAMP_ON))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
#if PFETON_DELAY != 0
for(YL = PFETON_DELAY; YL != 0; YL--)
{
}
#endif
Damping_FET_on(); // Damping fet on
}
#endif
#if PFETON_DELAY >= 128 // "Negative", 1's complement
if((0 == (Flags1 & (1 << SKIP_DAMP_ON))) &&
(0 == (Flags0 & (1 << DEMAG_CUT_POWER))))
{
Damping_FET_on(); // Damping fet on
for(YL = ~PFETON_DELAY; YL != 0; YL--)
{
}
}
All_nFETs_Off(); // Switch off all nfets
#endif
SREG = II_Sreg
return;
}
t2_int_pwm_off_fullpower_exit:
Set_TCNT2(0); // Write start point for timer
T2_Clear_Int_Flag(); // Clear interrupt flag
Flags0 |= (1<<PWM_ON);
SREG = II_Sreg;
}
}

sctwp · 发表于 2017-3-8 07:26:16

本帖最后由 sctwp 于 2017-3-8 07:32 编辑

厉害厉害如果time3也写成C那就更完美

yat · 发表于 2017-3-8 08:43:56

感谢分享 BLHeli的汇编源程序改成C语言格式

逗倪豌儿 · 发表于 2017-3-8 08:57:02

话说能跑吗？ mcu flash装的下吗？

giantwjt88 · 发表于 2017-3-8 09:26:08

这个工程量也蛮大的，关注，支持，+1024

sctwp · 发表于 2017-3-18 17:50:34

期待LZ继续更新

RAMILE · 发表于 2017-3-18 22:29:38

胡萝卜红烧肉

无刷电机

blheil

段胖子 · 发表于 2017-10-27 10:47:39

佩服楼主，还在继续吗，汇编看的真是头大啊

小小苹果 · 发表于 2017-10-28 12:22:38

工程浩大，改写难，调试更难

vc9181 · 发表于 2017-12-7 11:48:32

楼主牛

裸奔的蜗牛菌 · 发表于 2019-9-20 09:26:18

mark一下

motor_control · 发表于 2019-10-28 16:43:30

确实花了心思，顶一个！

狂欢的青年 · 发表于 2020-1-5 23:01:07

顶一个mark

chenarchie777 · 发表于 2020-7-13 09:01:12

顶一个

mark

kavin21513 · 发表于 2021-11-22 22:19:26

厉害了!!

yang1479 · 发表于 2021-12-29 11:24:17

牛逼，要是能文件下载就好了

林凡jeery · 发表于 2023-10-9 10:51:16

能直接发个压缩包么，最近在学习无刷的东西了

为了便于阅读，偿试把BLHeli的汇编源程序改成C语言格式

阿莫论坛20周年了！感谢大家的支持与爱护！！

浏览过的版块