@@ -315,9 +315,16 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
315
315
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
316
316
configureSAMDClock ();
317
317
318
+ if (pwm_frequency==NOT_SET) {
319
+ // use default frequency
320
+ pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
321
+ }
322
+
318
323
// configure the TCC (waveform, top-value, pre-scaler = frequency)
319
324
configureTCC (tccConfs[0 ], pwm_frequency);
320
325
configureTCC (tccConfs[1 ], pwm_frequency);
326
+ getTccPinConfiguration (pinA)->pwm_res = tccConfs[0 ].pwm_res ;
327
+ getTccPinConfiguration (pinB)->pwm_res = tccConfs[1 ].pwm_res ;
321
328
#ifdef SIMPLEFOC_SAMD_DEBUG
322
329
SIMPLEFOC_SAMD_DEBUG_SERIAL.println (" Configured TCCs..." );
323
330
#endif
@@ -408,10 +415,18 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in
408
415
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
409
416
configureSAMDClock ();
410
417
418
+ if (pwm_frequency==NOT_SET) {
419
+ // use default frequency
420
+ pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
421
+ }
422
+
411
423
// configure the TCC (waveform, top-value, pre-scaler = frequency)
412
424
configureTCC (tccConfs[0 ], pwm_frequency);
413
425
configureTCC (tccConfs[1 ], pwm_frequency);
414
426
configureTCC (tccConfs[2 ], pwm_frequency);
427
+ getTccPinConfiguration (pinA)->pwm_res = tccConfs[0 ].pwm_res ;
428
+ getTccPinConfiguration (pinB)->pwm_res = tccConfs[1 ].pwm_res ;
429
+ getTccPinConfiguration (pinC)->pwm_res = tccConfs[2 ].pwm_res ;
415
430
#ifdef SIMPLEFOC_SAMD_DEBUG
416
431
SIMPLEFOC_SAMD_DEBUG_SERIAL.println (" Configured TCCs..." );
417
432
#endif
@@ -479,11 +494,20 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
479
494
// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
480
495
configureSAMDClock ();
481
496
497
+ if (pwm_frequency==NOT_SET) {
498
+ // use default frequency
499
+ pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
500
+ }
501
+
482
502
// configure the TCC (waveform, top-value, pre-scaler = frequency)
483
503
configureTCC (tccConfs[0 ], pwm_frequency);
484
504
configureTCC (tccConfs[1 ], pwm_frequency);
485
505
configureTCC (tccConfs[2 ], pwm_frequency);
486
506
configureTCC (tccConfs[3 ], pwm_frequency);
507
+ getTccPinConfiguration (pin1A)->pwm_res = tccConfs[0 ].pwm_res ;
508
+ getTccPinConfiguration (pin2A)->pwm_res = tccConfs[1 ].pwm_res ;
509
+ getTccPinConfiguration (pin1B)->pwm_res = tccConfs[2 ].pwm_res ;
510
+ getTccPinConfiguration (pin2B)->pwm_res = tccConfs[3 ].pwm_res ;
487
511
#ifdef SIMPLEFOC_SAMD_DEBUG
488
512
SIMPLEFOC_SAMD_DEBUG_SERIAL.println (" Configured TCCs..." );
489
513
#endif
@@ -579,6 +603,11 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
579
603
// e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API
580
604
configureSAMDClock ();
581
605
606
+ if (pwm_frequency==NOT_SET) {
607
+ // use default frequency
608
+ pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
609
+ }
610
+
582
611
// configure the TCC(s)
583
612
configureTCC (pinAh, pwm_frequency, false , (pinAh.tcc .chaninfo ==pinAl.tcc .chaninfo )?dead_zone:-1 );
584
613
if ((pinAh.tcc .chaninfo !=pinAl.tcc .chaninfo ))
@@ -589,6 +618,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
589
618
configureTCC (pinCh, pwm_frequency, false , (pinCh.tcc .chaninfo ==pinCl.tcc .chaninfo )?dead_zone:-1 );
590
619
if ((pinCh.tcc .chaninfo !=pinCl.tcc .chaninfo ))
591
620
configureTCC (pinCl, pwm_frequency, true , -1.0 );
621
+ getTccPinConfiguration (pinA_h)->pwm_res = pinAh.pwm_res ;
622
+ getTccPinConfiguration (pinA_l)->pwm_res = pinAh.pwm_res ; // use the high phase resolution, in case we didn't set it
623
+ getTccPinConfiguration (pinB_h)->pwm_res = pinBh.pwm_res ;
624
+ getTccPinConfiguration (pinB_l)->pwm_res = pinBh.pwm_res ;
625
+ getTccPinConfiguration (pinC_h)->pwm_res = pinCh.pwm_res ;
626
+ getTccPinConfiguration (pinC_l)->pwm_res = pinCh.pwm_res ;
592
627
#ifdef SIMPLEFOC_SAMD_DEBUG
593
628
SIMPLEFOC_SAMD_DEBUG_SERIAL.println (" Configured TCCs..." );
594
629
#endif
@@ -611,9 +646,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const
611
646
*/
612
647
void _writeDutyCycle2PWM (float dc_a, float dc_b, int pinA, int pinB) {
613
648
tccConfiguration* tccI = getTccPinConfiguration (pinA);
614
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_a);
649
+ writeSAMDDutyCycle (tccI, dc_a);
615
650
tccI = getTccPinConfiguration (pinB);
616
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_b);
651
+ writeSAMDDutyCycle (tccI, dc_b);
617
652
return ;
618
653
}
619
654
@@ -635,11 +670,11 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) {
635
670
*/
636
671
void _writeDutyCycle3PWM (float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) {
637
672
tccConfiguration* tccI = getTccPinConfiguration (pinA);
638
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_a);
673
+ writeSAMDDutyCycle (tccI, dc_a);
639
674
tccI = getTccPinConfiguration (pinB);
640
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_b);
675
+ writeSAMDDutyCycle (tccI, dc_b);
641
676
tccI = getTccPinConfiguration (pinC);
642
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_c);
677
+ writeSAMDDutyCycle (tccI, dc_c);
643
678
return ;
644
679
}
645
680
@@ -662,13 +697,13 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB
662
697
*/
663
698
void _writeDutyCycle4PWM (float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){
664
699
tccConfiguration* tccI = getTccPinConfiguration (pin1A);
665
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_1a);
700
+ writeSAMDDutyCycle (tccI, dc_1a);
666
701
tccI = getTccPinConfiguration (pin2A);
667
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_2a);
702
+ writeSAMDDutyCycle (tccI, dc_2a);
668
703
tccI = getTccPinConfiguration (pin1B);
669
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_1b);
704
+ writeSAMDDutyCycle (tccI, dc_1b);
670
705
tccI = getTccPinConfiguration (pin2B);
671
- writeSAMDDutyCycle (tccI-> tcc . chaninfo , dc_2b);
706
+ writeSAMDDutyCycle (tccI, dc_2b);
672
707
return ;
673
708
}
674
709
@@ -705,33 +740,33 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i
705
740
// low-side on a different pin of same TCC - do dead-time in software...
706
741
float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1 ));
707
742
if (ls>1.0 ) ls = 1 .0f ; // no off-time is better than too-short dead-time
708
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_a);
709
- writeSAMDDutyCycle (tcc2-> tcc . chaninfo , ls);
743
+ writeSAMDDutyCycle (tcc1, dc_a);
744
+ writeSAMDDutyCycle (tcc2, ls);
710
745
}
711
746
else
712
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_a); // dead-time is done is hardware, no need to set low side pin explicitly
747
+ writeSAMDDutyCycle (tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly
713
748
714
749
tcc1 = getTccPinConfiguration (pinB_h);
715
750
tcc2 = getTccPinConfiguration (pinB_l);
716
751
if (tcc1->tcc .chaninfo !=tcc2->tcc .chaninfo ) {
717
752
float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1 ));
718
753
if (ls>1.0 ) ls = 1 .0f ; // no off-time is better than too-short dead-time
719
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_b);
720
- writeSAMDDutyCycle (tcc2-> tcc . chaninfo , ls);
754
+ writeSAMDDutyCycle (tcc1, dc_b);
755
+ writeSAMDDutyCycle (tcc2, ls);
721
756
}
722
757
else
723
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_b);
758
+ writeSAMDDutyCycle (tcc1, dc_b);
724
759
725
760
tcc1 = getTccPinConfiguration (pinC_h);
726
761
tcc2 = getTccPinConfiguration (pinC_l);
727
762
if (tcc1->tcc .chaninfo !=tcc2->tcc .chaninfo ) {
728
763
float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1 ));
729
764
if (ls>1.0 ) ls = 1 .0f ; // no off-time is better than too-short dead-time
730
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_c);
731
- writeSAMDDutyCycle (tcc2-> tcc . chaninfo , ls);
765
+ writeSAMDDutyCycle (tcc1, dc_c);
766
+ writeSAMDDutyCycle (tcc2, ls);
732
767
}
733
768
else
734
- writeSAMDDutyCycle (tcc1-> tcc . chaninfo , dc_c);
769
+ writeSAMDDutyCycle (tcc1, dc_c);
735
770
return ;
736
771
}
737
772
0 commit comments