/*
* move_lines: Move a range of lines to after given address. Return status.
*/
int
move_lines (off_t from, off_t to, off_t after, ed_buffer_t *ed)
{
ed_line_node_t *b1, *a1, *b2, *a2;
off_t succ = INC_MOD (to, ed->state->lines);
off_t prec = from - 1;
int done = (after == from - 1 || after == to);
spl1 ();
if (done)
{
a2 = get_line_node (succ, ed);
b2 = get_line_node (prec, ed);
ed->state->dot = to;
}
else if (!append_undo_node (UMOV, prec, succ, ed)
|| !append_undo_node (UMOV, after,
INC_MOD (after, ed->state->lines), ed))
{
spl0 ();
return ERR;
}
else
{
a1 = get_line_node (succ, ed);
if (from > after)
{
b1 = get_line_node (prec, ed);
/* this get_line_node last! */
b2 = get_line_node (after, ed);
}
else
{
b2 = get_line_node (after, ed);
/* this get_line_node last! */
b1 = get_line_node (prec, ed);
}
a2 = b2->q_forw;
LINK_NODES (b2, b1->q_forw);
LINK_NODES (a1->q_back, a2);
LINK_NODES (b1, a1);
ed->state->dot = after + (from > after ? to - from + 1 : 0);
}
if (ed->exec->global)
delete_global_nodes (b2->q_forw, a2, ed);
ed->state->is_modified = 1;
spl0 ();
return 0;
}
/* unset_active_nodes: remove a range of lines from the global-active list */
void
unset_active_nodes(line_t *np, line_t *mp)
{
line_t *lp;
int i;
for (lp = np; lp != mp; lp = lp->q_forw)
for (i = 0; i < active_last; i++)
if (active_list[active_ndx] == lp) {
active_list[active_ndx] = NULL;
active_ndx = INC_MOD(active_ndx, active_last - 1);
break;
} else active_ndx = INC_MOD(active_ndx, active_last - 1);
}
/*! Read from UART (using software buffer) */
static int uart_recv ( void *data, size_t size, uint flags, device_t *dev )
{
arch_uart_t *up;
uint8 *d;
int i;
ASSERT ( dev );
up = dev->params;
/* first, copy from uart to software buffer */
uart_read ( up );
/* second, copy from software buffer to data */
d = data;
i = 0;
while ( i < size && up->insz > 0 )
{
d[i] = up->inbuff[up->inf];
INC_MOD ( up->inf, up->inbufsz );
up->insz--;
i++;
}
return i; /* bytes read */
}
/*! Send data to uart */
static int uart_send ( void *data, size_t size, uint flags, device_t *dev )
{
arch_uart_t *up;
uint8 *d;
ASSERT ( dev );
up = dev->params;
d = data;
/* first, copy to software buffer */
while ( size > 0 && up->outsz < up->outbufsz )
{
if ( *d == 0 && flags == CONSOLE_PRINT )
{
size = 0;
break;
}
up->outbuff[up->outl] = *d++;
INC_MOD ( up->outl, up->outbufsz );
up->outsz++;
size--;
}
/* second, copy from software buffer to uart */
uart_write ( up );
return size; /* 0 if all sent, otherwise not send part length */
}
/* get_matching_node_addr: Return the address of the next line
matching a pattern in a given direction. Wrap around begin/end of
editor buffer if necessary. */
int
get_matching_node_address (const regex_t *re, int dir, off_t *addr,
ed_buffer_t *ed)
{
regmatch_t rm[1];
ed_line_node_t *lp;
char *s;
*addr = ed->state->dot;
if (!re)
return ERR;
do
{
if ((*addr = (dir ? INC_MOD (*addr, ed->state->lines)
: DEC_MOD (*addr, ed->state->lines))))
{
lp = get_line_node (*addr, ed);
if (!(s = get_buffer_line (lp, ed)))
return ERR;
#ifdef REG_STARTEND
rm->rm_so = 0;
rm->rm_eo = lp->len;
if (!regexec (re, s, 0, rm, REG_STARTEND))
#else
if (ed->state->is_binary)
NUL_TO_NEWLINE (s, lp->len);
if (!regexec (re, s, 0, NULL, 0))
#endif /* !defined (REG_STARTEND) */
return 0;
}
}
while (*addr != ed->state->dot);
ed->exec->err = _("No match");
return ERR;
}
void app_app_conf_debug_main(uint8_t view, const app_t *app, svc_main_proc_event_t event) {
hal_lcd_clear();
if(event & SVC_MAIN_PROC_EVENT_KEY_UP) {
INC_MOD(PRIV(app)->debug_item_current, HAL_DEBUG_N+1);
}
else if (event & SVC_MAIN_PROC_EVENT_KEY_DOWN) {
DEC_MOD(PRIV(app)->debug_item_current, HAL_DEBUG_N+1);
}
else if (event & SVC_MAIN_PROC_EVENT_KEY_ENTER_LONG) {
app_set_view(app, 0);
PRIV(app)->debug_item_current = 0;
}
else if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER) {
PRIV(app)->debug_value = hal_debug_read(PRIV(app)->debug_item_current);
}
svc_lcd_puts(8, "db");
if(PRIV(app)->debug_item_current == HAL_DEBUG_N) {
svc_lcd_puts(0, "----up");
if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER) {
app_set_view(app, 0);
PRIV(app)->debug_item_current = 0;
}
}
else {
svc_lcd_puti(6, 2, PRIV(app)->debug_item_current);
svc_lcd_putix(0, 4, PRIV(app)->debug_value);
}
}
static void main(uint8_t view, const app_t *app, svc_main_proc_event_t event) {
hal_lcd_clear();
if(event & SVC_MAIN_PROC_EVENT_KEY_UP) {
INC_MOD(PRIV(app)->item_current, svc_melodies_n+1);
}
else if (event & SVC_MAIN_PROC_EVENT_KEY_DOWN) {
DEC_MOD(PRIV(app)->item_current, svc_melodies_n+1);
}
else if (event & SVC_MAIN_PROC_EVENT_KEY_ENTER_LONG) {
app_exit();
}
svc_lcd_puts(8, "pl");
if(PRIV(app)->item_current == svc_melodies_n) {
svc_lcd_puts(0, "----up");
if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER) {
PRIV(app)->item_current = 0;
app_exit();
}
}
else {
svc_lcd_puts(0, " pla");
svc_lcd_puti(4, 2, PRIV(app)->item_current);
if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER) {
svc_melody_play(PRIV(app)->item_current);
}
}
}
char usart_getc(void)
{
char c;
// wait for a character
while (usart_tstc() == 0);
cli();
c = rxbuf[rx_rd];
rx_rd = INC_MOD(rx_rd, RXBUF_SIZE);
sei();
return c;
}
void USART2_IRQHandler(void)
{
USART_TypeDef* const usart = USART2;
uint32_t status = usart->SR;
// check for rx errors
if (status & (USART_SR_ORE | USART_SR_PE | USART_SR_FE | USART_SR_NE)) {
rx_errors ++;
}
// receive
if (status & USART_SR_RXNE) {
uint8_t c = usart->DR;
int rx_wr_inc = INC_MOD(rx_wr, RXBUF_SIZE);
if (rx_wr_inc != rx_rd) {
rxbuf[rx_wr] = c;
rx_wr = rx_wr_inc;
} else {
// rx buffer overflow
rx_errors ++;
}
}
// transmit
if (status & USART_SR_TXE) {
if (tx_rd != tx_wr) {
usart->DR = txbuf[tx_rd];
tx_rd = INC_MOD(tx_rd, TXBUF_SIZE);
} else {
// no more tx data, disable the tx empty interrupt
usart->CR1 &= ~USART_CR1_TXEIE;
}
}
// indicate any rx errors
if (rx_errors) {
gpio_set(LED_BLUE);
}
}
void usart_putc(char c)
{
USART_TypeDef* const usart = USART2;
int tx_wr_inc = INC_MOD(tx_wr, TXBUF_SIZE);
// wait for space
while (tx_wr_inc == tx_rd);
// put the character in the tx buffer
cli();
txbuf[tx_wr] = c;
tx_wr = tx_wr_inc;
sei();
// enable the tx empty interrupt
usart->CR1 |= USART_CR1_TXEIE;
}
/*! If there is data in software buffer, send it to UART */
static void uart_write ( arch_uart_t *up )
{
volatile unsigned int *uart_dr;
volatile unsigned int *uart_fr;
ASSERT ( up );
uart_dr = (unsigned int *) (up->base + UART_DR);
uart_fr = (unsigned int *) (up->base + UART_FR);
/* If UART is ready to transmit and software buffer is not empty */
while ( up->outsz > 0 && ( (*uart_fr) & UART_FR_TXFF ) == 0 )
{
*uart_dr = (unsigned int) up->outbuff[up->outf];
INC_MOD ( up->outf, up->outbufsz );
up->outsz--;
}
}
/*! Read data from UART to software buffer */
static void uart_read ( arch_uart_t *up )
{
volatile unsigned int *uart_dr;
volatile unsigned int *uart_fr;
ASSERT ( up );
uart_dr = (unsigned int *) (UART0_BASE + UART_DR);
uart_fr = (unsigned int *) (UART0_BASE + UART_FR);
/* While UART is not empty and software buffer is not full */
while ( ( (*uart_fr) & UART_FR_RXFE ) == 0 && up->insz < up->inbufsz )
{
up->inbuff[up->inl] = *uart_dr;
INC_MOD ( up->inl, up->inbufsz );
up->insz++;
}
}
static void main(uint8_t view, const app_t *app, svc_main_proc_event_t event) {
hal_lcd_clear();
if(event & SVC_MAIN_PROC_EVENT_KEY_UP) {
INC_MOD(PRIV(app)->countdown_current, svc_countdowns_n);
}
else if (event & SVC_MAIN_PROC_EVENT_KEY_DOWN) {
DEC_MOD(PRIV(app)->countdown_current, svc_countdowns_n);
}
else if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER_LONG) {
app_exit();
}
else if(event & SVC_MAIN_PROC_EVENT_KEY_DOWN_LONG) {
svc_countdown_t cd;
svc_countdown_get(PRIV(app_current)->countdown_current, &cd);
if(cd.state == SVC_COUNTDOWN_STATE_STOP) {
svc_countdown_start(PRIV(app_current)->countdown_current);
}
else {
svc_countdown_stop(PRIV(app_current)->countdown_current);
}
}
svc_countdown_t cd;
svc_countdown_get(PRIV(app)->countdown_current, &cd);
svc_lcd_puti(0, 2, cd.h);
svc_lcd_puti(2, 2, cd.m);
svc_lcd_puti(4, 2, cd.s);
hal_lcd_seg_set(HAL_LCD_SEG_COLON, 1);
hal_lcd_seg_set_blink(HAL_LCD_SEG_COLON, cd.state == SVC_COUNTDOWN_STATE_RUN);
if(cd.state == SVC_COUNTDOWN_STATE_RUN) {
svc_lcd_puts(8, "ru");
}
else {
svc_lcd_puts(8, "st");
}
svc_lcd_puti(6, 2, PRIV(app)->countdown_current);
if(event & SVC_MAIN_PROC_EVENT_KEY_ENTER) {
app_set_view(app, 1);
}
}
/* delete_lines: Delete a range of lines. */
int
delete_lines (off_t from, off_t to, ed_buffer_t *ed)
{
ed_line_node_t *b, *a;
/* Assert: spl1 () */
if (!append_undo_node (UDEL, from, to, ed))
return ERR;
a = get_line_node (INC_MOD (to, ed->state->lines), ed);
/* This get_line_node last! */
b = get_line_node (from - 1, ed);
if (ed->exec->global)
delete_global_nodes (b->q_forw, a, ed);
LINK_NODES (b, a);
if (to == ed->state->lines)
ed->state->newline_appended = 0;
ed->state->lines -= to - from + 1;
ed->state->dot = from - 1;
ed->state->is_modified = 1;
return 0;
}
void set_dac(int val)
{
int i,pm,dval;
unsigned int sval;
unsigned int x;
volatile int tmp;
dval=val;
if (val!=0)
{
if (ec_cmd.command[0].pwm_db<0)
{
if (ABS(dval)<ABS(ec_cmd.command[0].pwm_db))
dval=0;
}
else
{
if (dval>0)
dval=dval+ec_cmd.command[0].pwm_db;
else
dval=dval-ec_cmd.command[0].pwm_db;
}
pm=MIN(DAC_MAX_DUTY,ec_cmd.command[0].pwm_max);
if (dval>pm)
dval=pm;
if (dval<-pm)
dval=-pm;
}
dac_buffer[dac_idx]=dval;
dac_idx=INC_MOD(dac_idx,DAC_NUM_SMOOTH);
dac_cmd_val=get_dac_smooth();//This goes back up as for current computations.
//dac_cmd_val=dval;//This goes back up as for current computations.
sval=convert_val_to_unsigned_dac(dac_cmd_val);
//sval=convert_val_to_unsigned_dac(dval);
tmp++;
#ifdef M3_DEV
BB_DAC_CS0=0; //DAC on CH0 of DevBoard
BB_DAC_CS1=0;
#endif
BB_DAC_SEL=0; //SCLK must fall > 50NS after this
for (i=0;i<16;i++)
{
BB_DAC_CLK=1;
x=0x8000&sval;
if (x!=0)
BB_DAC_DI=1;
else
BB_DAC_DI=0;
sval=sval<<1;
asm("nop"); //140ns min clock period
asm("nop");
BB_DAC_CLK=0;
asm("nop");
asm("nop");
asm("nop");
}
BB_DAC_SEL=1;
}
void __attribute__((__interrupt__, no_auto_psv)) _ADC1Interrupt(void)
{
_AD1IF = 0; //Clear the flag
#if defined M3_LOADX6_A2R2 || defined M3_LOADX6_A2R3
#ifdef USE_TIMESTAMP_DC
SetTimestampLatch;
#endif
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
#ifdef USE_TIMESTAMP_DC
ClrTimestampLatch;
#endif
#endif
#if defined M3_LOADX6_A2R1
#ifdef USE_TIMESTAMP_DC
SetTimestampLatch;
#endif
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
adc_raw[8]=ADC1BUF8;
#ifdef USE_TIMESTAMP_DC
ClrTimestampLatch;
#endif
#endif
#if defined M3_BMW_A2R2 || defined M3_BMW_A2R3
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
#endif
#ifdef M3_HEX4_S2R1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
#endif
#if defined M3_LEDX2_S1R1 || defined M3_LEDX2XN_S2R1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
#endif
#ifdef M3_DEV
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
adc_raw[8]=ADC1BUF8;
#endif
#ifdef M3_HMB_H1R1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
adc_raw[8]=ADC1BUF8;
#endif
#if defined M3_HB2_H2R1_J0J1 || defined M3_HB2_H2R1_J2J3J4
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
#endif
#if defined M3_HB2_H2R2_J0J1 || defined M3_HB2_H2R3_J0J1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
#endif
#if defined M3_HB2_H2R2_J2J3J4 || defined M3_HB2_H2R3_J2J3J4
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
#endif
#ifdef M3_HEX2_S1R1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
adc_raw[8]=ADC1BUF8;
#endif
#ifdef M3_GMB_G1R1
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
adc_raw[6]=ADC1BUF6;
adc_raw[7]=ADC1BUF7;
adc_raw[8]=ADC1BUF8;
#endif
#if defined M3_PWR_0_2 || defined M3_PWR_0_3 || defined M3_PWR_0_4 || defined M3_DAC_0_1 || defined M3_ELMO_RNA_R0 || defined M3_PWR_0_5
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
}
#endif
#if defined M3_ELMO_B1R1 || defined M3_ELMO_Z1R1
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
}
#endif
#if defined M3_BMA_A1R1 || defined M3_WMA_0_1
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
adc_raw[3]=ADC1BUFB;
adc_raw[4]=ADC1BUFC;
adc_raw[5]=ADC1BUFD;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
adc_raw[5]=ADC1BUF5;
}
#endif
#if defined M3_MAX2_BDC_A2R1 || defined M3_MAX2_BLDC_A2R1 || defined M3_MAX2_BDC_T2R1 || defined M3_MAX2_BLDC_T2R1 || defined M3_BMW_A2R1
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
adc_raw[3]=ADC1BUFB;
adc_raw[4]=ADC1BUFC;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
adc_raw[4]=ADC1BUF4;
}
#endif
#if defined M3_MAX2_BDC_A2R2 || defined M3_MAX2_BLDC_A2R2 || defined M3_MAX2_BDC_S1R1 \
|| defined M3_MAX2_BDC_ARMH || defined M3_MAX2_BDC_A2R3 || defined M3_MAX2_BLDC_A2R3
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
adc_raw[3]=ADC1BUFB;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
}
#endif
#if defined M3_MAX2_BLDC_T2R2 || defined M3_MAX2_BDC_T2R2 || defined M3_MAX2_BLDC_T2R3
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
adc_raw[3]=ADC1BUFB;
//adc_raw[4]=ADC1BUFC;
//adc_raw[5]=ADC1BUFD;
//adc_raw[6]=ADC1BUFE;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
//adc_raw[4]=ADC1BUF4;
//adc_raw[5]=ADC1BUF5;
//adc_raw[6]=ADC1BUF6;
}
#endif
#if defined M3_MAX2_BDC_T2R3
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
//adc_raw[3]=ADC1BUFB;
//adc_raw[4]=ADC1BUFC;
//adc_raw[5]=ADC1BUFD;
//adc_raw[6]=ADC1BUFE;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
//adc_raw[3]=ADC1BUF3;
//adc_raw[4]=ADC1BUF4;
//adc_raw[5]=ADC1BUF5;
//adc_raw[6]=ADC1BUF6;
}
#endif
#if defined M3_DAC_0_1
adc_buffer[ADC_NC][adc_idx]=0;//adc_raw[ADC_NC]; //Not connected, read it anyhow to simplify code.
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT]; //Used EXT for analog torque feedback
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined M3_ELMO_RNA_R0
adc_buffer[ADC_SEAS][adc_idx]=adc_raw[ADC_SEAS];
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_SEAS]; //Used ADC_SEAS for analog torque feedback
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined M3_ELMO_B1R1 || defined M3_ELMO_Z1R1
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_BMA_A1R1 || defined M3_WMA_0_1
adc_buffer[ADC_SEAS][adc_idx]=adc_raw[ADC_SEAS];
adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_SEAS];
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined M3_MAX2_BDC_A2R1 || defined M3_MAX2_BLDC_A2R1 || defined M3_BMW_A2R1
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_MAX2_BDC_A2R2 || defined M3_MAX2_BLDC_A2R2 || defined M3_MAX2_BDC_S1R1 \
|| defined M3_MAX2_BDC_ARMH || defined M3_MAX2_BDC_A2R3 || defined M3_MAX2_BLDC_A2R3
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_BMW_A2R2 || defined M3_BMW_A2R3
adc_buffer[ADC_EXT_TEMP][adc_idx]=adc_raw[ADC_EXT_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_HEX4_S2R1
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_AMP_TEMP_A][adc_idx]=adc_raw[ADC_AMP_TEMP_A];
adc_buffer[ADC_AMP_TEMP_B][adc_idx]=adc_raw[ADC_AMP_TEMP_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_MAX2_BDC_T2R1 || defined M3_MAX2_BLDC_T2R1 || defined M3_MAX2_BLDC_T2R2 || defined M3_MAX2_BDC_T2R2
//adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined M3_MAX2_BLDC_T2R3
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
//adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined M3_MAX2_BDC_T2R3
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
//adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#ifdef M3_HMB_H1R1
adc_buffer[ADC_MOTOR_TEMP_A][adc_idx]=adc_raw[ADC_MOTOR_TEMP_A];
adc_buffer[ADC_EXT_A][adc_idx]=adc_raw[ADC_EXT_A];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_MOTOR_TEMP_B][adc_idx]=adc_raw[ADC_MOTOR_TEMP_B];
adc_buffer[ADC_EXT_B][adc_idx]=adc_raw[ADC_EXT_B];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_HB2_H2R1_J0J1
adc_buffer[ADC_MOTOR_TEMP_A][adc_idx]=adc_raw[ADC_MOTOR_TEMP_A];
adc_buffer[ADC_MOTOR_TEMP_B][adc_idx]=adc_raw[ADC_MOTOR_TEMP_B];
adc_buffer[ADC_SEAS_A][adc_idx]=adc_raw[ADC_SEAS_A];
adc_buffer[ADC_SEAS_B][adc_idx]=adc_raw[ADC_SEAS_B];
adc_buffer[ADC_AMP_TEMP_A][adc_idx]=adc_raw[ADC_AMP_TEMP_A];
adc_buffer[ADC_AMP_TEMP_B][adc_idx]=adc_raw[ADC_AMP_TEMP_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_HB2_H2R2_J0J1 || defined M3_HB2_H2R3_J0J1
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer[ADC_SEAS_A][adc_idx]=adc_raw[ADC_SEAS_A];
adc_buffer[ADC_SEAS_B][adc_idx]=adc_raw[ADC_SEAS_B];
adc_buffer[ADC_AMP_TEMP_A][adc_idx]=adc_raw[ADC_AMP_TEMP_A];
adc_buffer[ADC_AMP_TEMP_B][adc_idx]=adc_raw[ADC_AMP_TEMP_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_HB2_H2R1_J2J3J4
adc_buffer[ADC_MOTOR_TEMP_A][adc_idx]=adc_raw[ADC_MOTOR_TEMP_A];
adc_buffer[ADC_MOTOR_TEMP_B][adc_idx]=adc_raw[ADC_MOTOR_TEMP_B];
adc_buffer[ADC_MOTOR_TEMP_C][adc_idx]=adc_raw[ADC_MOTOR_TEMP_C];
adc_buffer[ADC_SEAS_A][adc_idx]=adc_raw[ADC_SEAS_A];
adc_buffer[ADC_SEAS_B][adc_idx]=adc_raw[ADC_SEAS_B];
adc_buffer[ADC_SEAS_C][adc_idx]=adc_raw[ADC_SEAS_C];
adc_buffer[ADC_AMP_TEMP_A][adc_idx]=adc_raw[ADC_AMP_TEMP_A];
adc_buffer[ADC_AMP_TEMP_B][adc_idx]=adc_raw[ADC_AMP_TEMP_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_HB2_H2R2_J2J3J4 || defined M3_HB2_H2R3_J2J3J4
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer[ADC_CURRENT_C][adc_idx]=adc_raw[ADC_CURRENT_C];
adc_buffer[ADC_SEAS_A][adc_idx]=adc_raw[ADC_SEAS_A];
adc_buffer[ADC_SEAS_B][adc_idx]=adc_raw[ADC_SEAS_B];
adc_buffer[ADC_SEAS_C][adc_idx]=adc_raw[ADC_SEAS_C];
adc_buffer[ADC_AMP_TEMP_A][adc_idx]=adc_raw[ADC_AMP_TEMP_A]; //B=C=A
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#ifdef M3_HEX2_S1R1
adc_buffer[ADC_MOTOR_TEMP_A][adc_idx]=adc_raw[ADC_MOTOR_TEMP_A];
adc_buffer[ADC_MOTOR_TEMP_B][adc_idx]=adc_raw[ADC_MOTOR_TEMP_B];
adc_buffer[ADC_EXT_A][adc_idx]=adc_raw[ADC_EXT_A];
adc_buffer[ADC_EXT_B][adc_idx]=adc_raw[ADC_EXT_B];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#ifdef M3_GMB_G1R1
adc_buffer[ADC_SEAS_A][adc_idx]=adc_raw[ADC_SEAS_A];
adc_buffer[ADC_SEAS_B][adc_idx]=adc_raw[ADC_SEAS_B];
adc_buffer[ADC_MOTOR_TEMP_A][adc_idx]=adc_raw[ADC_MOTOR_TEMP_A];
adc_buffer[ADC_MOTOR_TEMP_B][adc_idx]=adc_raw[ADC_MOTOR_TEMP_B];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_EXT_A][adc_idx]=adc_raw[ADC_EXT_A];
adc_buffer[ADC_EXT_B][adc_idx]=adc_raw[ADC_EXT_B];
adc_buffer[ADC_EXT_C][adc_idx]=adc_raw[ADC_EXT_C];
adc_buffer[ADC_EXT_D][adc_idx]=adc_raw[ADC_EXT_D];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_LEDX2_S1R1 || defined M3_LEDX2XN_S2R1
adc_buffer[ADC_EXT_A][adc_idx]=adc_raw[ADC_EXT_A];
adc_buffer[ADC_EXT_B][adc_idx]=adc_raw[ADC_EXT_B];
adc_buffer[ADC_EXT_C][adc_idx]=adc_raw[ADC_EXT_C];
adc_buffer[ADC_EXT_D][adc_idx]=adc_raw[ADC_EXT_D];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_PWR_0_2 || defined M3_PWR_0_3 || defined M3_PWR_0_4 || defined M3_PWR_0_5
adc_buffer[ADC_BUS_VOLTAGE][adc_idx]=adc_raw[ADC_BUS_VOLTAGE];
adc_buffer[ADC_CURRENT_DIGITAL][adc_idx]=adc_raw[ADC_CURRENT_DIGITAL];
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_LOADX6_A2R2 || defined M3_LOADX6_A2R3
adc_buffer[ADC_LOAD_0][adc_idx]=adc_raw[ADC_LOAD_0];
adc_buffer[ADC_LOAD_1][adc_idx]=adc_raw[ADC_LOAD_1];
adc_buffer[ADC_LOAD_2][adc_idx]=adc_raw[ADC_LOAD_2];
adc_buffer[ADC_LOAD_3][adc_idx]=adc_raw[ADC_LOAD_3];
adc_buffer[ADC_LOAD_4][adc_idx]=adc_raw[ADC_LOAD_4];
adc_buffer[ADC_LOAD_5][adc_idx]=adc_raw[ADC_LOAD_5];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined M3_LOADX6_A2R1
adc_buffer[ADC_EXT_0][adc_idx]=adc_raw[ADC_EXT_0];
adc_buffer[ADC_EXT_1][adc_idx]=adc_raw[ADC_EXT_1];
adc_buffer[ADC_EXT_2][adc_idx]=adc_raw[ADC_EXT_2];
adc_buffer[ADC_LOAD_0][adc_idx]=adc_raw[ADC_LOAD_0];
adc_buffer[ADC_LOAD_1][adc_idx]=adc_raw[ADC_LOAD_1];
adc_buffer[ADC_LOAD_2][adc_idx]=adc_raw[ADC_LOAD_2];
adc_buffer[ADC_LOAD_3][adc_idx]=adc_raw[ADC_LOAD_3];
adc_buffer[ADC_LOAD_4][adc_idx]=adc_raw[ADC_LOAD_4];
adc_buffer[ADC_LOAD_5][adc_idx]=adc_raw[ADC_LOAD_5];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
}
void __attribute__((__interrupt__, no_auto_psv)) _ADC1Interrupt(void)
{
static unsigned int count = 0;
_AD1IF = 0; //Clear the flag
adc_raw[0] = ADC1BUF0;
adc_raw[1] = ADC1BUF1;
adc_raw[2] = ADC1BUF2;
adc_buffer[ADC_TEMP_BOARD][adc_idx] = adc_raw[ADC_TEMP_BOARD];
adc_buffer[ADC_CURRENT][adc_idx] = adc_raw[ADC_CURRENT];
adc_buffer[ADC_TEMP_AMB][adc_idx] = adc_raw[ADC_TEMP_AMB];
adc_idx = INC_MOD(adc_idx,ADC_NUM_SMOOTH);
//Timed actions - 2kHz
//Originaly in timer3 ISR
//======================
count = INC_MOD(count,5);
if(count == 0)
{
//Latch encoder timestamp on Rising edge.
#ifdef USE_TIMESTAMP_DC //Takes 0.05us to execute
SetTimestampLatch;
ClrTimestampLatch;
#endif
#if defined USE_ENCODER_VERTX //Takes 122us to execute
step_vertx();
#endif
#ifdef USE_CURRENT //Takes 0.475us to execute
step_current();
#endif
#ifdef USE_CONTROL //Takes 2.3us to execute
step_control(); //(will probably increase in PID mode...)
#endif
irq_cnt++;
#ifdef USE_WATCHDOG
wd_cnt++;
if ((ec_cmd.command[0].config & 0x4000) != last_status) // if the WD bit changes, everything is cool
{
wd_cnt = 0;
watchdog_expired = 0;
}
else if (wd_cnt > 500) // if the status doesn't change in 250ms, problem
{
watchdog_expired = 1;
}
last_status = (ec_cmd.command[0].config & 0x4000);
#else
watchdog_expired = 0; //Always off
#endif
//Sum = 125us, 25% of this time slice
}
}
void __attribute__((__interrupt__, no_auto_psv)) _ADC1Interrupt(void)
{
static unsigned int count = 0;
_AD1IF = 0; //Clear the flag
if (AD1CON2bits.BUFS==0) //ADC module filling lower group, read from upper
{
adc_raw[0]=ADC1BUF8;
adc_raw[1]=ADC1BUF9;
adc_raw[2]=ADC1BUFA;
adc_raw[3]=ADC1BUFB;
}
else
{
adc_raw[0]=ADC1BUF0;
adc_raw[1]=ADC1BUF1;
adc_raw[2]=ADC1BUF2;
adc_raw[3]=ADC1BUF3;
}
#if defined MAX2_BDC_0_3_A2R2 || defined MAX2_BLDC_0_3_A2R2 || defined MAX2_BDC_0_2_A2R3 \
|| defined MAX2_BLDC_0_2_A2R3
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
#endif
#if defined MAX2_BLDC_0_3_T2R2 || defined MAX2_BDC_0_3_T2R2
adc_buffer[ADC_EXT][adc_idx]=adc_raw[ADC_EXT];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined MAX2_BLDC_0_2_T2R3
adc_buffer[ADC_MOTOR_TEMP][adc_idx]=adc_raw[ADC_MOTOR_TEMP];
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
//adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
#if defined MAX2_BDC_0_2_T2R3
adc_buffer[ADC_AMP_TEMP][adc_idx]=adc_raw[ADC_AMP_TEMP];
adc_buffer[ADC_CURRENT_A][adc_idx]=adc_raw[ADC_CURRENT_A];
adc_buffer[ADC_CURRENT_B][adc_idx]=adc_raw[ADC_CURRENT_B];
//adc_buffer_fast[adc_idx_fast]=adc_raw[ADC_EXT];
adc_idx=INC_MOD(adc_idx,ADC_NUM_SMOOTH);
adc_idx_fast=INC_MOD(adc_idx_fast,ADC_NUM_SMOOTH_FAST);
#endif
//Timed actions - 2kHz
//Originaly in timer3 ISR
//======================
count = INC_MOD(count,5);
if(count == 0)
{
//Latch encoder timestamp on Rising edge.
#ifdef USE_TIMESTAMP_DC //Takes 0.05us to execute
SetTimestampLatch;
ClrTimestampLatch;
#endif
#if defined USE_ENCODER_VERTX //Takes 122us to execute
step_vertx();
#endif
#ifdef USE_CURRENT //Takes 0.475us to execute
step_current();
#endif
#ifdef USE_CONTROL //Takes 2.3us to execute
step_control(); //(will probably increase in PID mode...)
#endif
//Torso external ADC
#ifdef USE_ADC_SPI
step_adc_spi();
#endif
irq_cnt++;
#ifdef USE_WATCHDOG
wd_cnt++;
if ((ec_cmd.command[0].config & 0x4000) != last_status) // if the WD bit changes, everything is cool
{
wd_cnt = 0;
watchdog_expired = 0;
}
else if (wd_cnt > 500) // if the status doesn't change in 250ms, problem
{
watchdog_expired = 1;
}
last_status = (ec_cmd.command[0].config & 0x4000);
#else
watchdog_expired = 0; //Always off
#endif
//Sum = 125us, 25% of this time slice
}
}
