hybrid PWM system

Uses PWM1 to directly control pins 4, 6 & 11 (servo 0, 1 & 3) and PWM1
generated interrupts to control other pins.

Interupt control of the servo pins had too much jitter so switched all
that we could to PWM1 direct control.  The PWM1 direct control pins have
less than 1 microsecond pulse width jitter while the interrupt
controlled ones can have 20+ microseconds of jitter.

Also added insurance to the servo code in the "disable servo after move"
section.
This commit is contained in:
Bob-the-Kuhn 2017-08-18 19:16:57 -05:00 committed by Scott Lahteine
parent 94dd39b3b7
commit 01fb45b4f8
2 changed files with 282 additions and 160 deletions

View file

@ -23,17 +23,26 @@
/** /**
* The class Servo uses the PWM class to implement it's functions * The class Servo uses the PWM class to implement it's functions
* *
* The PWM1 module is only used to generate interrups at specified times. It
* is NOT used to directly toggle pins. The ISR writes to the pin assigned to
* that interrupt
*
* All PWMs use the same repetition rate - 20mS because that's the normal servo rate * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
*/
/**
* This is a hybrid system.
* *
* The PWM1 module is used to directly control the Servo 0, 1 & 3 pins. This keeps
* the pulse width jitter to under a microsecond.
*
* For all other pins the PWM1 module is used to generate interrupts. The ISR
* routine does the actual setting/clearing of pins. The upside is that any pin can
* have a PWM channel assigned to it. The downside is that there is more pulse width
* jitter. The jitter depends on what else is happening in the system and what ISRs
* prempt the PWM ISR. Writing to the SD card can add 20 microseconds to the pulse
* width.
*/ */
/** /**
* The data structures are setup to minimize the computation done by the ISR which * The data structures are setup to minimize the computation done by the ISR which
* minimizes ISR execution time. Execution times are 1.7 to 1.9 microseconds. * minimizes ISR execution time. Execution times are 2.2 - 3.7 microseconds.
* *
* Two tables are used. active_table is used by the ISR. Changes to the table are * Two tables are used. active_table is used by the ISR. Changes to the table are
* are done by copying the active_table into the work_table, updating the work_table * are done by copying the active_table into the work_table, updating the work_table
@ -47,34 +56,39 @@
* *
* The ISR's priority is set to the maximum otherwise other ISRs can cause considerable * The ISR's priority is set to the maximum otherwise other ISRs can cause considerable
* jitter in the PWM high time. * jitter in the PWM high time.
*
* See the end of this file for details on the hardware/firmware interaction
*/ */
#ifdef TARGET_LPC1768 #ifdef TARGET_LPC1768
#include <lpc17xx_pinsel.h> #include <lpc17xx_pinsel.h>
//#include "../HAL.h"
//#include "../../../macros.h"
#include "serial.h"
typedef struct { // holds all data needed to control the 6 PWM channels #define NUM_PWMS 6
typedef struct { // holds all data needed to control/init one of the PWM channels
uint8_t sequence; // 0: available slot, 1 - 6: PWM channel assigned to that slot uint8_t sequence; // 0: available slot, 1 - 6: PWM channel assigned to that slot
uint8_t logical_pin; uint8_t logical_pin;
uint16_t PWM_mask; uint16_t PWM_mask; // MASK TO CHECK/WRITE THE IR REGISTER
volatile uint32_t* set_register; volatile uint32_t* set_register;
volatile uint32_t* clr_register; volatile uint32_t* clr_register;
uint32_t write_mask; uint32_t write_mask; // USED BY SET/CLEAR COMMANDS
uint32_t microseconds; uint32_t microseconds; // value written to MR register
uint32_t min; uint32_t min; // lower value limit checked by WRITE routine before writing to the MR register
uint32_t max; uint32_t max; // upper value limit checked by WRITE routine before writing to the MR register
bool PWM_flag; // bool PWM_flag; // 0 - USED BY sERVO, 1 - USED BY ANALOGWRITE
uint8_t servo_index; // 0 - MAX_SERVO -1 : servo index, 0xFF : PWM channel uint8_t servo_index; // 0 - MAX_SERVO -1 : servo index, 0xFF : PWM channel
bool active_flag; bool active_flag; // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
uint8_t assigned_MR; // Which MR (1-6) is used by this logical channel
uint32_t PCR_bit; // PCR register bit to enable PWM1 control of this pin
uint32_t PINSEL3_bits; // PINSEL3 register bits to set pin mode to PWM1 control
} PWM_map; } PWM_map;
#define MICRO_MAX 0xffffffff #define MICRO_MAX 0xffffffff
#define PWM_MAP_INIT_ROW {0, 0xff, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0} #define PWM_MAP_INIT_ROW {0, 0xff, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0, 0, 0, 0}
#define PWM_MAP_INIT {PWM_MAP_INIT_ROW,\ #define PWM_MAP_INIT {PWM_MAP_INIT_ROW,\
PWM_MAP_INIT_ROW,\ PWM_MAP_INIT_ROW,\
PWM_MAP_INIT_ROW,\ PWM_MAP_INIT_ROW,\
@ -83,18 +97,14 @@ typedef struct { // holds all data needed to control the 6 PWM channels
PWM_MAP_INIT_ROW,\ PWM_MAP_INIT_ROW,\
}; };
PWM_map PWM1_map_A[6] = PWM_MAP_INIT; PWM_map PWM1_map_A[NUM_PWMS] = PWM_MAP_INIT;
PWM_map PWM1_map_B[6] = PWM_MAP_INIT; PWM_map PWM1_map_B[NUM_PWMS] = PWM_MAP_INIT;
PWM_map *active_table = PWM1_map_A; PWM_map *active_table = PWM1_map_A;
PWM_map *work_table = PWM1_map_B; PWM_map *work_table = PWM1_map_B;
PWM_map *ISR_table; PWM_map *ISR_table;
#define NUM_PWMS 6
volatile uint8_t PWM1_ISR_index = 0;
#define IR_BIT(p) (p >= 0 && p <= 3 ? p : p + 4 ) #define IR_BIT(p) (p >= 0 && p <= 3 ? p : p + 4 )
#define COPY_ACTIVE_TABLE for (uint8_t i = 0; i < 6 ; i++) work_table[i] = active_table[i] #define COPY_ACTIVE_TABLE for (uint8_t i = 0; i < 6 ; i++) work_table[i] = active_table[i]
#define PIN_IS_INVERTED(p) 0 // place holder in case inverting PWM output is offered #define PIN_IS_INVERTED(p) 0 // place holder in case inverting PWM output is offered
@ -169,11 +179,12 @@ void LPC1768_PWM_init(void) {
} }
bool PWM_table_swap; // flag to tell the ISR that the tables have been swapped bool PWM_table_swap = false; // flag to tell the ISR that the tables have been swapped
bool PWM_MR0_wait = false; // flag to ensure don't delay MR0 interrupt
bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_PWM1_MR0 - MR0_MARGIN), uint8_t servo_index = 0xff) { bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_PWM1_MR0 - MR0_MARGIN), uint8_t servo_index = 0xff) {
while (PWM_table_swap) delay(5); // don't do anything until the previous change has been implemented by the ISR
COPY_ACTIVE_TABLE; // copy active table into work table COPY_ACTIVE_TABLE; // copy active table into work table
uint8_t slot = 0; uint8_t slot = 0;
for (uint8_t i = 0; i < NUM_PWMS ; i++) // see if already in table for (uint8_t i = 0; i < NUM_PWMS ; i++) // see if already in table
@ -196,6 +207,9 @@ bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_P
work_table[slot].active_flag = false; work_table[slot].active_flag = false;
//swap tables //swap tables
PWM_MR0_wait = true;
while (PWM_MR0_wait) delay(5); //wait until MR0 interrupt has happend so don't delay it.
NVIC_DisableIRQ(PWM1_IRQn); NVIC_DisableIRQ(PWM1_IRQn);
PWM_map *pointer_swap = active_table; PWM_map *pointer_swap = active_table;
active_table = work_table; active_table = work_table;
@ -206,148 +220,181 @@ bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_P
return 1; return 1;
} }
#define pin_11_PWM_channel 2
#define pin_6_PWM_channel 3
#define pin_4_PWM_channel 1
// used to keep track of which Match Registers have been used and if they will be used by the
// PWM1 module to directly control the pin or will be used to generate an interrupt
typedef struct { // status of PWM1 channel
uint8_t map_used; // 0 - this MR register not used/assigned
uint8_t map_PWM_INT; // 0 - available for interrupts, 1 - in use by PWM
uint8_t map_PWM_PIN; // logical pin number for this PwM1 controlled pin / port
volatile uint32_t* MR_register; // address of the MR register for this PWM1 channel
uint32_t PCR_bit; // PCR register bit to enable PWM1 control of this pin
uint32_t PINSEL3_bits; // PINSEL3 register bits to set pin mode to PWM1 control
} MR_map;
MR_map map_MR[NUM_PWMS];
void LPC1768_PWM_update_map_MR(void) {
map_MR[0] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_4_PWM_channel) ? 1 : 0), 4, &LPC_PWM1->MR1, 0, 0};
map_MR[1] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_11_PWM_channel) ? 1 : 0), 11, &LPC_PWM1->MR2, 0, 0};
map_MR[2] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_6_PWM_channel) ? 1 : 0), 6, &LPC_PWM1->MR3, 0, 0};
map_MR[3] = {0, 0, 0, &LPC_PWM1->MR4, 0, 0};
map_MR[4] = {0, 0, 0, &LPC_PWM1->MR5, 0, 0};
map_MR[5] = {0, 0, 0, &LPC_PWM1->MR6, 0, 0};
}
uint32_t LPC1768_PWM_interrupt_mask = 1;
void LPC1768_PWM_update(void) {
for (uint8_t i = NUM_PWMS; --i;) { // (bubble) sort table by microseconds
bool didSwap = false;
PWM_map temp;
for (uint16_t j = 0; j < i; ++j) {
if (work_table[j].microseconds > work_table[j + 1].microseconds) {
temp = work_table[j + 1];
work_table[j + 1] = work_table[j];
work_table[j] = temp;
didSwap = true;
}
}
if (!didSwap) break;
}
LPC1768_PWM_interrupt_mask = 0; // set match registers to new values, build IRQ mask
for (uint8_t i = 0; i < NUM_PWMS; i++) {
if (work_table[i].active_flag == true) {
work_table[i].sequence = i + 1;
// first see if there is a PWM1 controlled pin for this entry
bool found = false;
for (uint8_t j = 0; (j < NUM_PWMS) && !found; j++) {
if ( (map_MR[j].map_PWM_PIN == work_table[i].logical_pin) && map_MR[j].map_PWM_INT ) {
*map_MR[j].MR_register = work_table[i].microseconds; // found one of the PWM pins
work_table[i].PWM_mask = 0;
work_table[i].PCR_bit = map_MR[j].PCR_bit; // PCR register bit to enable PWM1 control of this pin
work_table[i].PINSEL3_bits = map_MR[j].PINSEL3_bits; // PINSEL3 register bits to set pin mode to PWM1 control} MR_map;
map_MR[j].map_used = 2;
work_table[i].assigned_MR = j +1; // only used to help in debugging
found = true;
}
}
// didn't find a PWM1 pin so get an interrupt
for (uint8_t k = 0; (k < NUM_PWMS) && !found; k++) {
if ( !(map_MR[k].map_PWM_INT || map_MR[k].map_used)) {
*map_MR[k].MR_register = work_table[i].microseconds; // found one for an interrupt pin
map_MR[k].map_used = 1;
LPC1768_PWM_interrupt_mask |= _BV(3 * (k + 1)); // set bit in the MCR to enable this MR to generate an interrupt
work_table[i].PWM_mask = _BV(IR_BIT(k + 1)); // bit in the IR that will go active when this MR generates an interrupt
work_table[i].assigned_MR = k +1; // only used to help in debugging
found = true;
}
}
}
else
work_table[i].sequence = 0;
}
LPC1768_PWM_interrupt_mask |= (uint32_t) _BV(0); // add in MR0 interrupt
// swap tables
PWM_MR0_wait = true;
while (PWM_MR0_wait) delay(5); //wait until MR0 interrupt has happend so don't delay it.
NVIC_DisableIRQ(PWM1_IRQn);
LPC_PWM1->LER = 0x07E; // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
PWM_map *pointer_swap = active_table;
active_table = work_table;
work_table = pointer_swap;
PWM_table_swap = true; // tell the ISR that the tables have been swapped
NVIC_EnableIRQ(PWM1_IRQn); // re-enable PWM interrupts
}
bool LPC1768_PWM_write(uint8_t pin, uint32_t value) { bool LPC1768_PWM_write(uint8_t pin, uint32_t value) {
while (PWM_table_swap) delay(5); // don't do anything until the previous change has been implemented by the ISR
COPY_ACTIVE_TABLE; // copy active table into work table COPY_ACTIVE_TABLE; // copy active table into work table
uint8_t slot = 0xFF; uint8_t slot = 0xFF;
for (uint8_t i = 0; i < NUM_PWMS; i++) // find slot for (uint8_t i = 0; i < NUM_PWMS; i++) // find slot
if (work_table[i].logical_pin == pin) slot = i; if (work_table[i].logical_pin == pin) slot = i;
if (slot == 0xFF) return false; // return error if pin not found if (slot == 0xFF) return false; // return error if pin not found
digitalWrite(pin, 0); // set pin to output & set it low
LPC1768_PWM_update_map_MR();
switch(pin) {
case 11: // Servo 0, PWM1 channel 2 (Pin 11 P1.20 PWM1.2)
map_MR[pin_11_PWM_channel - 1].PCR_bit = _BV(8 + pin_11_PWM_channel); // enable PWM1 module control of this pin
map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 1; // 0 - available for interrupts, 1 - in use by PWM
map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0x2 << 8; // ISR must do this AFTER setting PCR
break;
case 6: // Servo 1, PWM1 channel 3 (Pin 6 P1.21 PWM1.3)
map_MR[pin_6_PWM_channel - 1].PCR_bit = _BV(8 + pin_6_PWM_channel); // enable PWM1 module control of this pin
map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 1; // 0 - available for interrupts, 1 - in use by PWM
map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0x2 << 10; // ISR must do this AFTER setting PCR
break;
case 4: // Servo 3, PWM1 channel 1 (Pin 4 P1.18 PWM1.1)
map_MR[pin_4_PWM_channel - 1].PCR_bit = _BV(8 + pin_4_PWM_channel); // enable PWM1 module control of this pin
map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 1; // 0 - available for interrupts, 1 - in use by PWM
map_MR[pin_4_PWM_channel - 1].PINSEL3_bits = 0x2 << 4; // ISR must do this AFTER setting PCR
break;
default: // ISR pins
pinMode(pin, OUTPUT); // set pin to output but don't write anything in case it's already in use
break;
}
work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min); work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);
work_table[slot].active_flag = true; work_table[slot].active_flag = true;
for (uint8_t i = NUM_PWMS; --i;) { // (bubble) sort table by microseconds LPC1768_PWM_update();
bool didSwap = false;
PWM_map temp;
for (uint16_t j = 0; j < i; ++j) {
if (work_table[j].microseconds > work_table[j + 1].microseconds) {
temp = work_table[j + 1];
work_table[j + 1] = work_table[j];
work_table[j] = temp;
didSwap = true;
}
}
if (!didSwap) break;
}
for (uint8_t i = 0; i < NUM_PWMS; i++) // set the index & PWM_mask
if (work_table[i].active_flag == true) {
work_table[i].sequence = i + 1;
work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
}
else work_table[i].sequence = 0;
uint32_t interrupt_mask = 0; // set match registers to new values, build IRQ mask
if (work_table[0].active_flag == true) {
LPC_PWM1->MR1 = work_table[0].microseconds;
interrupt_mask |= _BV(3);
}
if (work_table[1].active_flag == true) {
LPC_PWM1->MR2 = work_table[1].microseconds;
interrupt_mask |= _BV(6);
}
if (work_table[2].active_flag == true) {
LPC_PWM1->MR3 = work_table[2].microseconds;
interrupt_mask |= _BV(9);
}
if (work_table[3].active_flag == true) {
LPC_PWM1->MR4 = work_table[3].microseconds;
interrupt_mask |= _BV(12);
}
if (work_table[4].active_flag == true) {
LPC_PWM1->MR5 = work_table[4].microseconds;
interrupt_mask |= _BV(15);
}
if (work_table[5].active_flag == true) {
LPC_PWM1->MR6 = work_table[5].microseconds;
interrupt_mask |= _BV(18);
}
interrupt_mask |= _BV(0); // add in MR0 interrupt
// swap tables
NVIC_DisableIRQ(PWM1_IRQn);
LPC_PWM1->LER = 0x07E; // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
PWM_map *pointer_swap = active_table;
active_table = work_table;
work_table = pointer_swap;
PWM_table_swap = true; // tell the ISR that the tables have been swapped
LPC_PWM1->MCR = interrupt_mask; // enable new PWM individual channel interrupts
NVIC_EnableIRQ(PWM1_IRQn); // re-enable PWM interrupts
return 1; return 1;
} }
bool LPC1768_PWM_detach_pin(uint8_t pin) { bool LPC1768_PWM_detach_pin(uint8_t pin) {
while (PWM_table_swap) delay(5); // don't do anything until the previous change has been implemented by the ISR
COPY_ACTIVE_TABLE; // copy active table into work table COPY_ACTIVE_TABLE; // copy active table into work table
uint8_t slot = 0xFF; uint8_t slot = 0xFF;
for (uint8_t i = 0; i < NUM_PWMS; i++) // find slot for (uint8_t i = 0; i < NUM_PWMS; i++) // find slot
if (work_table[i].logical_pin == pin) slot = i; if (work_table[i].logical_pin == pin) slot = i;
if (slot == 0xFF) return false; // return error if pin not found if (slot == 0xFF) return false; // return error if pin not found
pinMode(pin, INPUT_PULLUP); // set pin to input with pullup
LPC1768_PWM_update_map_MR();
// OK to make these changes before the MR0 interrupt
switch(pin) {
case 11: // Servo 0, PWM1 channel 2 (Pin 11 P1.20 PWM1.2)
LPC_PWM1->PCR &= ~(_BV(8 + pin_11_PWM_channel)); // disable PWM1 module control of this pin
map_MR[pin_11_PWM_channel - 1].PCR_bit = 0;
LPC_PINCON->PINSEL3 &= ~(0x3 << 8); // return pin to general purpose I/O
map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0;
map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 0; // 0 - available for interrupts, 1 - in use by PWM
break;
case 6: // Servo 1, PWM1 channel 3 (Pin 6 P1.21 PWM1.3)
LPC_PWM1->PCR &= ~(_BV(8 + pin_6_PWM_channel)); // disable PWM1 module control of this pin
map_MR[pin_6_PWM_channel - 1].PCR_bit = 0;
LPC_PINCON->PINSEL3 &= ~(0x3 << 10); // return pin to general purpose I/O
map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0;
map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 0; // 0 - available for interrupts, 1 - in use by PWM
break;
case 4: // Servo 3, PWM1 channel 1 (Pin 4 P1.18 PWM1.1)
LPC_PWM1->PCR &= ~(_BV(8 + pin_4_PWM_channel)); // disable PWM1 module control of this pin
map_MR[pin_4_PWM_channel - 1].PCR_bit = 0;
LPC_PINCON->PINSEL3 &= ~(0x3 << 4); // return pin to general purpose I/O
map_MR[pin_4_PWM_channel - 1].PINSEL3_bits = 0;
map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 0; // 0 - available for interrupts, 1 - in use by PWM
break;
}
pinMode(pin, INPUT);
work_table[slot] = PWM_MAP_INIT_ROW; work_table[slot] = PWM_MAP_INIT_ROW;
for (uint8_t i = NUM_PWMS; --i;) { // (bubble) sort table by microseconds LPC1768_PWM_update();
bool didSwap = false;
PWM_map temp;
for (uint16_t j = 0; j < i; ++j) {
if (work_table[j].microseconds > work_table[j + 1].microseconds) {
temp = work_table[j + 1];
work_table[j + 1] = work_table[j];
work_table[j] = temp;
didSwap = true;
}
}
if (!didSwap) break;
}
for (uint8_t i = 0; i < NUM_PWMS; i++) // set the index & PWM_mask
if (work_table[i].active_flag == true) {
work_table[i].sequence = i + 1;
work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
}
else work_table[i].sequence = 0;
uint32_t interrupt_mask = 0; // set match registers to new values, build IRQ mask
if (work_table[0].active_flag == true) {
LPC_PWM1->MR1 = work_table[0].microseconds;
interrupt_mask |= _BV(3);
}
if (work_table[1].active_flag == true) {
LPC_PWM1->MR2 = work_table[1].microseconds;
interrupt_mask |= _BV(6);
}
if (work_table[2].active_flag == true) {
LPC_PWM1->MR3 = work_table[2].microseconds;
interrupt_mask |= _BV(9);
}
if (work_table[3].active_flag == true) {
LPC_PWM1->MR4 = work_table[3].microseconds;
interrupt_mask |= _BV(12);
}
if (work_table[4].active_flag == true) {
LPC_PWM1->MR5 = work_table[4].microseconds;
interrupt_mask |= _BV(15);
}
if (work_table[5].active_flag == true) {
LPC_PWM1->MR6 = work_table[5].microseconds;
interrupt_mask |= _BV(18);
}
interrupt_mask |= _BV(0); // add in MR0 interrupt
// swap tables
NVIC_DisableIRQ(PWM1_IRQn);
LPC_PWM1->LER = 0x07E; // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
PWM_map *pointer_swap = active_table;
active_table = work_table;
work_table = pointer_swap;
PWM_table_swap = true; // tell the ISR that the tables have been swapped
LPC_PWM1->MCR = interrupt_mask; // enable remaining PWM individual channel interrupts
NVIC_EnableIRQ(PWM1_IRQn); // re-enable PWM interrupts
return 1; return 1;
} }
@ -356,27 +403,101 @@ bool LPC1768_PWM_detach_pin(uint8_t pin) {
#define HAL_PWM_LPC1768_ISR extern "C" void PWM1_IRQHandler(void) #define HAL_PWM_LPC1768_ISR extern "C" void PWM1_IRQHandler(void)
// Both loops could be terminated when the last active channel is found but that would
// result in variations ISR run time which results in variations in pulse width
/**
* Changes to PINSEL3, PCR and MCR are only done during the MR0 interrupt otherwise
* the wrong pin may be toggled or even have the system hang.
*/
HAL_PWM_LPC1768_ISR { HAL_PWM_LPC1768_ISR {
if (PWM_table_swap) ISR_table = work_table; // use old table if a swap was just done if (PWM_table_swap) ISR_table = work_table; // use old table if a swap was just done
else ISR_table = active_table; else ISR_table = active_table;
if (LPC_PWM1->IR & 0x1) { // MR0 interrupt if (LPC_PWM1->IR & 0x1) { // MR0 interrupt
PWM_table_swap = false; // MR0 means new values could have been ISR_table = active_table; // MR0 means new values could have been loaded so set everything
ISR_table = active_table; // loaded so set everything to normal operation if (PWM_table_swap) LPC_PWM1->MCR = LPC1768_PWM_interrupt_mask; // enable new PWM individual channel interrupts
for (uint8_t i = 0; (i < NUM_PWMS) && ISR_table[i].active_flag ; i++)
*ISR_table[i].set_register = ISR_table[i].write_mask; // set all enabled channels active for (uint8_t i = 0; (i < NUM_PWMS) ; i++) {
if(ISR_table[i].active_flag && !((ISR_table[i].logical_pin == 11) ||
(ISR_table[i].logical_pin == 4) ||
(ISR_table[i].logical_pin == 6)))
*ISR_table[i].set_register = ISR_table[i].write_mask; // set pins for all enabled interrupt channels active
if (PWM_table_swap && ISR_table[i].PCR_bit) {
LPC_PWM1->PCR |= ISR_table[i].PCR_bit; // enable PWM1 module control of this pin
LPC_PINCON->PINSEL3 |= ISR_table[i].PINSEL3_bits; // set pin mode to PWM1 control - must be done after PCR
}
}
PWM_table_swap = false;
PWM_MR0_wait = false;
LPC_PWM1->IR = 0x01; // clear the MR0 interrupt flag bit LPC_PWM1->IR = 0x01; // clear the MR0 interrupt flag bit
PWM1_ISR_index = 0;
} }
else { else {
if (ISR_table[PWM1_ISR_index].active_flag && (LPC_PWM1->IR & ISR_table[PWM1_ISR_index].PWM_mask)) { for (uint8_t i = 0; i < NUM_PWMS ; i++)
LPC_PWM1->IR = ISR_table[PWM1_ISR_index].PWM_mask; // clear the interrupt flag bit if (ISR_table[i].active_flag && (LPC_PWM1->IR & ISR_table[i].PWM_mask) ){
*ISR_table[PWM1_ISR_index].clr_register = ISR_table[PWM1_ISR_index].write_mask; // set channel to inactive LPC_PWM1->IR = ISR_table[i].PWM_mask; // clear the interrupt flag bits for expected interrupts
*ISR_table[i].clr_register = ISR_table[i].write_mask; // set channel to inactive
} }
PWM1_ISR_index++; // should be the index for the next interrupt
} }
LPC_PWM1->IR = 0x70F; // guarantees all interrupt flags are cleared which, if there is an unexpected
// PWM interrupt, will keep the ISR from hanging which will crash the controller
return; return;
} }
#endif #endif
/////////////////////////////////////////////////////////////////
///////////////// HARDWARE FIRMWARE INTERACTION ////////////////
/////////////////////////////////////////////////////////////////
/**
* Almost all changes to the hardware registers must be coordinated with the Match Register 0 (MR0)
* interrupt. The only exception is detaching pins. It doesn't matter when they go
* tristate.
*
* The LPC1768_PWM_init routine kicks off the MR0 interrupt. This interrupt is never disabled or
* delayed.
*
* The PWM_table_swap flag is set when the firmware has swapped in an updated table. It is
* cleared by the ISR during the MR0 interrupt as it completes the swap and accompanying updates.
* It serves two purposes:
* 1) Tells the ISR that the tables have been swapped
* 2) Keeps the firmware from starting a new update until the previous one has been completed.
*
* The PWM_MR0_wait flag is set when the firmware is ready to swap in an updated table and cleared by
* the ISR during the MR0 interrupt. It is used to avoid delaying the MR0 interrupt when swapping in
* an updated table. This avoids glitches in pulse width and/or repetition rate.
*
* The sequence of events during a write to a PWM channel is:
* 1) Waits until PWM_table_swap flag is false before starting
* 2) Copies the active table into the work table
* 3) Updates the work table
* NOTES - MR1-MR6 are updated at this time. The updates aren't put into use until the first
* MR0 after the LER register has been written. The LER register is written during the
* table swap process.
* - The MCR mask is created at this time. It is not used until the ISR writes the MCR
* during the MR0 interrupt in the table swap process.
* 4) Sets the PWM_MR0_wait flag
* 5) ISR clears the PWM_MR0_wait flag during the next MR0 interrupt
* 6) Once the PWM_MR0_wait flag is cleared then the firmware:
* disables the ISR interrupt
* swaps the pointers to the tables
* writes to the LER register
* sets the PWM_table_swap flag active
* re-enables the ISR
* 7) On the next interrupt the ISR changes it's pointer to the work table which is now the old,
* unmodified, active table.
* 8) On the next MR0 interrupt the ISR:
* switches over to the active table
* clears the PWM_table_swap and PWM_MR0_wait flags
* updates the MCR register with the possibly new interrupt sources/assignments
* writes to the PCR register to enable the direct control of the Servo 0, 1 & 3 pins by the PWM1 module
* sets the PINSEL3 register to function/mode 0x2 for the Servo 0, 1 & 3 pins
* NOTE - PCR must be set before PINSEL
* sets the pins controlled by the ISR to their active states
*/

View file

@ -159,6 +159,7 @@
#if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE) #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
this->detach(); this->detach();
LPC1768_PWM_detach_pin(servo_info[this->servoIndex].Pin.nbr); // shut down the PWM signal LPC1768_PWM_detach_pin(servo_info[this->servoIndex].Pin.nbr); // shut down the PWM signal
LPC1768_PWM_attach_pin(servo_info[this->servoIndex].Pin.nbr, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, this->servoIndex); // make sure no one else steals the slot
#endif #endif
} }
} }