/**
* We have just hit top and are starting to count down again.
* The new PWM duty cycle has been clocked in for the next servo
*/
static void service(const Timer *timer, void* data){
SERVO_DRIVER* driver = data;
uint8_t i;
// Move to the next servo
uint8_t index = (driver->specific.softwareMUX.currentServo + 1)& 7;
driver->specific.softwareMUX.currentServo = index;
// Set the multiplex pins so the pulse goes to the correct servo
// This needs to be done quickly to stop the start of the pulse
// going to the wrong sevo
uint8_t bits = index;
for (i = 0; i < NUM_MUX_PINS; i++){
const IOPin* pin = driver->specific.softwareMUX.muxPins[i];
if(bits & 1){
pin_high(pin);
}else{
pin_low(pin);
}
bits >>= 1;
}
// Keep track of the lowest value of the timer counter
// Setting a delay between this value and TOP will mean the mux bits dont get changed in time
uint16_t thisDelay = timerGetCounter(timer);
if(thisDelay < maxDelay){
maxDelay = thisDelay;
}
// Time critical part is over
// Now calculate the pulse width for next servo
uint16_t newPos=0;
index = (index + 1) & 7;
if(index < driver->num_servos){
SERVO* servo = (SERVO *)pgm_read_word(&driver->servos[index]);
if(servo->actuator.connected){
newPos = servo->delay;
// Limit to the maximum value
uint16_t limit = maxDelay;
if(newPos > limit){
newPos = limit;
}
}
}
// Set the threshold inline - to save more registers push/pops
//compareSetThreshold(channel,newPos);
PORT port = driver->specific.softwareMUX.pwmPort;
_SFR_MEM16(port) = newPos; // set 16 bit word
}
/* Initialize a new timer.
* \param t Pointer to timer to initialize
* \param period Period in timer "ticks" (overflows) for the new timer. Maximum value is 0x7FFF ticks.
*/
void timerInit(timer_t *t, uint16_t period) {
uint16_t count;
if (t == NULL) {
return;
}
period &= 0x7FFF; // mask highest bit of period - this is reserved for overflow tracking
// initialize timer
count = timerGetCounter();
// if expireCount overflows, so will timer. Therefore period should be preserved through overflow.
t->expireCount = count + period;
}
/*! Check if a timer is active.
* \param timer Pointer to timer to check.
* \return Returns 0 if timer has elapsed, or 1 if timer is active.
*/
uint8_t timerActive(timer_t *t) {
uint16_t count;
if (t == NULL) {
return 0;
}
count = timerGetCounter();
// compare overflow bits
if ((t->expireCount & 0x8000) == (count & 0x8000)) {
if ((count & 0x7FFF) < (t->expireCount & 0x7FFF)) {
return 1;
}
} else {
if ((count & 0x7FFF) >= (t->expireCount & 0x7FFF)) {
return 1;
}
}
return 0;
}
