void sys_tick_handler(void)
{
static int temp32 = 0;
static uint8_t data[8] = {0, 1, 2, 0, 0, 0, 0, 0};
/* We call this handler every 1ms so 1000ms = 1s on/off. */
if (++temp32 != 1000)
return;
temp32 = 0;
/* Transmit CAN frame. */
data[0]++;
if (can_transmit(CAN1,
0, /* (EX/ST)ID: CAN ID */
false, /* IDE: CAN ID extended? */
false, /* RTR: Request transmit? */
8, /* DLC: Data length */
data) == -1)
{
gpio_set(GPIOA, GPIO6); /* LED0 off */
gpio_set(GPIOA, GPIO7); /* LED1 off */
gpio_clear(GPIOB, GPIO0); /* LED2 on */
gpio_set(GPIOB, GPIO1); /* LED3 off */
}
}
void usb_lp_can_rx0_isr(void) {
uint32_t id, fmi;
bool ext, rtr;
uint8_t dlc, data[8];
can_receive(CAN1, 0, false, &id, &ext, &rtr, &fmi, &dlc, data);
can_fifo_release(CAN1, 0);
/* check for extended id, dlc = 5 and id 0x0000xxxx */
if ((ext) && (dlc == 5) && !(id & 0xffff0000)) {
/* M*rklin Start/Stop CMD
00004711 [5] 00 00 00 00 00 -> Stop
00004711 [5] 00 00 00 00 01 -> Start
*/
if (data[4] && 0x01)
if (!status) {
/* send stop immediatly if Stop Button pressed */
can_transmit(CAN1, 0x0000fffe, true, false, 5, data);
gpio_set(GPIOC, GPIO13); /* clear green LED */
gpio_set(GPIOB, GPIO4); /* clear On LED */
gpio_clear(GPIOB, GPIO5); /* light Off LED */
} else {
gpio_clear(GPIOC, GPIO13); /* light green LED */
gpio_clear(GPIOB, GPIO4); /* light On LED */
gpio_set(GPIOB, GPIO5); /* clear Off LED */
} else {
gpio_set(GPIOC, GPIO13); /* clear green LED */
gpio_set(GPIOB, GPIO4); /* clear On LED */
gpio_clear(GPIOB, GPIO5); /* light Off LED */
}
}
}
void can_write_vstats(unsigned int StringNo, unsigned int* vcell, unsigned int* temperature, unsigned int isense)
{
unsigned int i, a;
//Prepare a CAN message for these cell voltages
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_STAT;
// Sum up all cell voltages in stack
a = 0;
for ( i = 0; i < 30; i++ )
{
a += ( vcell[i] / 10 );
}
can_push_ptr->data.data_u16[0] = a;
// Average temperature
a = temperature[1] + temperature[4] + temperature[7];
a /= 3;
can_push_ptr->data.data_u16[1] = a;
// Current measurement
can_push_ptr->data.data_u16[2] = isense;
can_push_ptr->status = 6;
can_push();
can_transmit();
}
int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
{
if (!can_initialized) {
return -2;
}
if(len > 8){
return -1;
}
/* FIXME: we are discarding the const qualifier for buf here.
* We should probably fix libopencm3 to actually have the
* const qualifier too...
*/
return can_transmit(CAN1,
id, /* (EX/ST)ID: CAN ID */
#ifdef USE_CAN_EXT_ID
true, /* IDE: CAN ID extended */
#else
false, /* IDE: CAN ID not extended */
#endif
false, /* RTR: Request transmit? */
len, /* DLC: Data length */
(uint8_t *)buf);
}
void sys_tick_handler(void)
{
static int temp32 = 0;
static u8 data[8] = {0, 1, 2, 0, 0, 0, 0, 0};
/* We call this handler every 1ms so 100ms = 1s
* Resulting in 100Hz message frequency.
*/
if (++temp32 != 100)
return;
temp32 = 0;
/* Transmit CAN frame. */
data[0]++;
if (can_transmit(CAN1,
0, /* (EX/ST)ID: CAN ID */
false, /* IDE: CAN ID extended? */
false, /* RTR: Request transmit? */
8, /* DLC: Data length */
data) == -1)
{
gpio_set(GPIOA, GPIO8); /* LED0 off */
gpio_set(GPIOB, GPIO4); /* LED1 off */
gpio_set(GPIOC, GPIO15); /* LED2 off */
gpio_set(GPIOC, GPIO2); /* LED3 off */
gpio_clear(GPIOC, GPIO5); /* LED4 on */
}
}
int aseba_can_send(const uint16_t *msg, int len)
{
if (can_transmit(CAN1, ASEBA_ID | ASEBA_TYPE_SMALL_PACKET, false, false, len*2, (uint8_t*)msg) >= 0) {
// uart_puts("x");
return 0;
} else {
uart_puts("ERROR sending frame\n");
return -1;
}
}
void can_write_gear(unsigned int StringNo, unsigned int *gear_num)
{
//Prepare a CAN message for gear number
if(StringNo == SCU_GEAR_S1) {can_push_ptr->address = SCU_S1_CAN_BASE;}
can_push_ptr->address += SCU_GEAR;
can_push_ptr->data.data_u16[0] = gear_num[0];
can_push_ptr->current_gear = gear_num[0];
can_push_ptr->status = gear_num[0];
can_push();
can_transmit();
}
//-----------------------------------------------
void can_in_an(void)
{
char temp,i;
signed temp_S;
int tempI;
//if((mess[0]==1)&&(mess[1]==2)&&(mess[2]==3)&&(mess[3]==4)&&(mess[4]==5)&&(mess[5]==6)&&(mess[6]==7)&&(mess[7]==8))can_transmit1(1,2,3,4,5,6,7//,8);
if((mess[6]==ADRESS)&&(mess[7]==ADRESS)&&(mess[8]==GETTM))
{
can_error_cnt=0;
can_transmit(0x18e,ADRESS,PUTTM_NET,*(((char*)&unet_buff[0])+1),*((char*)&unet_buff[0]),*(((char*)&unet_buff[1])+1),*((char*)&unet_buff[1]),*(((char*)unet_buff[2])+1),*((char*)unet_buff[2]));
link_cnt=0;
link=ON;
/* if(flags_tu&0b10000000)
{
if(!res_fl)
{
res_fl=1;
bRES=1;
res_fl_cnt=0;
}
}
else
{
if(main_cnt>20)
{
if(res_fl)
{
res_fl=0;
}
}
}
if(res_fl_)
{
res_fl_=0;
} */
}
can_in_an_end:
bCAN_RX=0;
}
//Write set of 3 temperature readings from a single string to CAN bus
void can_write_temps(unsigned int StringNo, unsigned int* temp)
{
//Prepare a CAN message for these temperatures
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_TEMP;
can_push_ptr->data.data_u16[0] = temp[1];
can_push_ptr->data.data_u16[1] = temp[4];
can_push_ptr->data.data_u16[2] = temp[7];
can_push_ptr->status = 6;
can_push();
can_transmit();
}
LONG cop_transmit(LONG cob_id, SHORT length, BYTE *data)
{
// 1. Configure transmit message object
if((cop_error = can_config(CANBUF_TX, cob_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Transmit the message
if((cop_error = can_transmit(CANBUF_TX, length, data)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// Return if the message is transmitted
can_delete(CANBUF_TX);
return cop_error;
}
static inline void main_periodic( void ) {
/*RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});*/
//RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], &coder_values[1]);});
//RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, &can1_pending);});
/*RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
&bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});*/
servos[0]=coder_values[0];
servos[1]=coder_values[1];
//use id=1 for azimuth board
can_transmit(1, (uint8_t *)servos, 8);
LED_TOGGLE(5);
}
int can_trans(uint32_t id, const uint8_t *buf, uint8_t len)
{
if(len > 8){
return -1;
}
can_tx_msg.id = id;
can_tx_msg.dlc = len;
memcpy(can_tx_msg.data, buf, len);
return can_transmit(CAN1,
can_tx_msg.id,
can_tx_msg.ide,
can_tx_msg.rtr,
can_tx_msg.dlc,
can_tx_msg.data);
}
bool can_interface_send_message(uint32_t id, uint8_t *message, uint8_t length, uint32_t retries)
{
do {
can_transmit(
CAN1, // canport
id, // can id
false, // extended id
false, // request transmit
length, // data length
message // data
);
while((CAN_TSR(CAN1) & CAN_TSR_RQCP0) == 0);
if ((CAN_TSR(CAN1) & CAN_TSR_TXOK0)) {
return true; // can ok
}
} while (retries-- > 0);
return false;
}
void can_write_vcell(unsigned int StringNo, unsigned int* vcell)
{
unsigned int i;
//Prepare a CAN message for these cell voltages
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_CV;
for ( i = 0; i < 30; i += 4 )
{
can_push_ptr->data.data_u16[0] = vcell[i];
can_push_ptr->data.data_u16[1] = vcell[i+1];
if ( i+2 < 30 )
{
can_push_ptr->data.data_u16[2] = vcell[i+2];
can_push_ptr->data.data_u16[3] = vcell[i+3];
}
can_push_ptr->status = 8;
can_push();
can_transmit();
can_push_ptr->address++;
}
}
// ********************************
// Error Transmit
// ********************************
void dn_error_transmit(void){
can_transmit(&tx_fd_error);
}
static LONG sdo_receive(BYTE node_id, WORD index, BYTE subindex, SHORT *length, BYTE *data, SHORT max)
{
short n; // data length code
short rc; // return value
short t = 0; // toggle bit
// --- Initiate SDO Upload ---
cop_buffer[0] = 0x40; // client command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = 0x00; // reserved: set to 00h
cop_buffer[5] = 0x00; // -"-
cop_buffer[6] = 0x00; // -"-
cop_buffer[7] = 0x00; // -"-
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
*length = 0; // no data received!
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x40) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x02) == 0x02) { // expedited transfer?
if((cop_buffer[0] & 0x01) == 0x01)
n = 4 - (short)((cop_buffer[0] & 0x0C) >> 2);
else
n = 4;
memcpy(data, &cop_buffer[4], n < max? n : max);
*length = n < max? n : max; // data received!!!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
break;
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR); // segmented transfer:
// --- Upload SDO Segment ---
for(*length = 0;;)
{
cop_buffer[0] = 0x60 | t; // client command specifier
cop_buffer[1] = 0x00; // reserved: set to 00h
cop_buffer[2] = 0x00; // -"-
cop_buffer[3] = 0x00; // -"-
cop_buffer[4] = 0x00; // -"-
cop_buffer[5] = 0x00; // -"-
cop_buffer[6] = 0x00; // -"-
cop_buffer[7] = 0x00; // -"-
n = 8; // 8 bytes to transmit!
// 5. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 6. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 7. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
*length = 0; // no data received!
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x00) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered?
cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x0E) != 0x00) // number of segment data bytes
n = 7 - (int)((cop_buffer[0] & 0x0E) >> 1);
else
n = 7;
if(max - *length > 0) // copy segment data if space
memcpy(&data[*length], &cop_buffer[1], *length + n < max? n : max - *length);
*length += n;
if((cop_buffer[0] & 0x01) == 0x01) { // no more segments?
if(*length > max)
*length = max; // truncate to buffer size!
if(*length < max)
data[*length] = '\0'; // for zero-closed strings!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
break;
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
t = t? 0x00 : 0x10; // alternate toggle bit!
}
}
static LONG sdo_segmented(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data)
{
short n, i; // data length code
short rc; // return value
short t = 0; // toggle bit
// --- Initiate SDO Download ---
cop_buffer[0] = (BYTE)0x21; // client command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOBYTE(length); // number of data bytes (LSB)
cop_buffer[5] = HIBYTE(length); // -"-
cop_buffer[6] = (BYTE)0x00; // -"-
cop_buffer[7] = (BYTE)0x00; // number of data bytes (MSB)
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
// --- Download SDO Segment ---
for(i = 0;;)
{
if(length <= 7) // bytes that does not contain data
n = 7 - length;
else // no segment size indicated
n = 0;
cop_buffer[0] = (BYTE)(n << 1); // client command specifier
memset(&cop_buffer[1], 0x00, 7);// clear data buffer
memcpy(&cop_buffer[1], &data[i], 7 - n);// copy segment data
length -= 7 - n; // remaining number of bytes
i += 7 - n; // index to remainung bytes
cop_buffer[0]|= length? 0x00 : 0x01;// last segment to transmit
cop_buffer[0]|= t; // toggle bit
n = 8; // 8 bytes to transmit!
// 5. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 6. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 7. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[0] & 0xE0) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x20) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered?
cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if(length == 0) { // all data tranmitted?
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
t = t? 0x00 : 0x10; // alternate toggle bit!
}
}
static LONG sdo_expedited(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data)
{
short n; // data length code
short rc; // return value
// --- Expedited SDO Download ---
cop_buffer[0] = (BYTE)0x23; // client command specifier
cop_buffer[0] |= (BYTE)((4 - length) << 2);
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
memset(&cop_buffer[4],0x00,4); // clear data buffer
memcpy(&cop_buffer[4],data,length); // copy data bytes
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
else { // success: data written!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(1); // "the torture never stops!"
}
void can_tx_left_ankle_angle(void){can_transmit(&tx_fd_left_ankle_angle);}
void can_tx_max_exec_time(void){can_transmit(&tx_fd_max_exec_time);}
void can_tx_status(void){can_transmit(&tx_fd_status);}
//Wrapper functions for calling CAN transmit from the scheduler
void can_tx_left_ankle_rate(void){can_transmit(&tx_fd_left_ankle_rate);}
/*===========================================================================
*===========================================================================*/
void
display_adapter(struct mg_connection *c, const struct mg_request_info *ri, void *user_data)
{
struct Squirrel *squirrel = (struct Squirrel *)user_data;
char adapter_name[256];
char description[256];
unsigned exists;
const char *driver = "";
unsigned interface_channel = 0;
pixie_enter_critical_section(squirrel->cs);
if (memcmp(ri->uri, "/adapter/", 9) != 0) {
mg_printf(c, "404 Not Found\r\nConnection: closed\r\n\r\n");
goto _return;
} else
sprintf_s(adapter_name, sizeof(adapter_name), "%s", ri->uri+9);
if (strlen(adapter_name) > 5 && memcmp(adapter_name+strlen(adapter_name)-5, ".html", 5) == 0)
adapter_name[strlen(adapter_name)-5] = '\0';
if (strlen(adapter_name) > 7 && memcmp(adapter_name, "airpcap", 7) == 0 && strlen(adapter_name) < sizeof(adapter_name)-5) {
memmove(adapter_name+4, adapter_name, strlen(adapter_name)+1);
memcpy(adapter_name, "\\\\.\\", 4);
}
exists = adapter_description(adapter_name, description, sizeof(description));
if (!exists) {
mg_printf(c, "404 Not Found\r\nConnection: closed\r\n\r\n");
goto _return;
}
if (strstr(adapter_name, "\\airpcap")) {
driver = "airpcap";
} else {
driver = "ndis";
}
mg_headers_ok(c, "text/html");
X(c, "Connection: close\r\n");
X(c, "\r\n");
//X(c, "<!DOCTYPE HTML PUBLIC \"-//W3C//DTD HTML 4.01//EN\" \"http://www.w3.org/TR/html4/strict.dtd\">\r\n");
X(c,"<html>\n");
X(c,"<head>\n");
X(c," <title>Squirrel WiFi monitor / Adapter</title>\n");
X(c," <link rel=\"stylesheet\" type=\"text/css\" href=\"../squirrel.css\" />\n");
X(c," <link rel=\"Shortcut Icon\" href=\"../favicon.ico\" type=\"image/x-icon\">\n");
X(c,"</head>\n");
X(c,"<body>\n");
display_topmenu(c, ri, user_data, 0);
/*
* Do any necessary changes
*/
{
char *status = mg_get_var(c, "status");
char *channel = mg_get_var(c, "channel");
if (status && channel) {
unsigned is_running = squirrel_get_interface_status(squirrel, adapter_name, &interface_channel);
unsigned new_channel;
if (strcmp(channel, "scan") == 0)
new_channel = (unsigned)-1;
else if (isdigit(channel[0]))
new_channel = atoi(channel);
else
new_channel = 0;
if (is_running && strcmp(status, "monitor") != 0) {
/* Turn off the adapter */
squirrel_set_interface_status(squirrel, adapter_name, 0, 0);
X(c, "<b>Turned off adapter</b>\n");
} else if (!is_running && strcmp(status, "monitor") == 0) {
launch_thread(squirrel, adapter_name);
squirrel_set_interface_status(squirrel, adapter_name, 1, new_channel);
X(c, "<b>Turned on adapter, channel %u</b>\n", new_channel);
} else if (is_running && interface_channel != new_channel) {
squirrel_set_interface_status(squirrel, adapter_name, 1, new_channel);
X(c, "<b>Changed channel to %u</b>\n", new_channel);
} else
X(c, "<b>Nothing changed</b>\n");
}
if (status)
free(status);
if (channel)
free(channel);
}
X(c, "<table class=\"adapter\">\n");
X(c, " <tr><th>Adapter:</th><td>%s</td></tr>\n", adapter_name);
X(c, " <tr><th>Description:</th><td>%s</td></tr>\n", description);
X(c, " <tr><th>Driver:</th><td>%s</td></tr>\n", driver);
X(c, " <tr><th>Monitor Mode:</th><td>%s</td></tr>\n", can_monitor_mode(adapter_name)?"yes":"no");
X(c, " <tr><th>Can Transmit:</th><td>%s</td></tr>\n", can_transmit(adapter_name)?"yes":"no");
if (squirrel_get_interface_status(squirrel, adapter_name, &interface_channel)) {
X(c, " <tr><th>Status:</th><td>%s</td></tr>\n", "monitoring");
if (interface_channel == 0)
X(c, " <tr><th>Channel:</th><td>%s</td></tr>\n", "");
else if (interface_channel == (unsigned)-1)
X(c, " <tr><th>Channel:</th><td>%s</td></tr>\n", "scan");
else
X(c, " <tr><th>Channel:</th><td>%u</td></tr>\n", interface_channel);
} else {
X(c, " <tr><th>Status:</th><td>%s</td></tr>\n", "off");
X(c, " <tr><th>Channel:</th><td>%s</td></tr>\n", "");
}
X(c, "</table>\n");
X(c, "<hr/>\n");
X(c, "<form action=\"%s.html\">\n", adapter_name);
X(c, " <input type=\"radio\" name=\"status\" value=\"monitor\" /> Monitor<br/>\n");
X(c, " <input type=\"radio\" name=\"status\" value=\"off\" /> Off<br/>\n");
X(c, " Channel: <input type=\"text\" name=\"channel\" value=\"scan\"/><br/>\n");
X(c, " <input type=\"submit\" value=\"Submit\">\n");
X(c, "</form>\n");
_return:
pixie_leave_critical_section(squirrel->cs);
}
void can_tx_color_red(void){can_transmit(&tx_fd_color_red);}
void can_tx_color_blue(void){can_transmit(&tx_fd_color_blue);}
void can_tx_color_white(void){can_transmit(&tx_fd_color_white);}
void can_tx_color_green(void){can_transmit(&tx_fd_color_green);}
void sys_tick_handler(void) {
static uint8_t data[5] = { 0, 0, 0, 0, 0 };
/* We call this handler every 1ms so every 1ms = 0.001s
* resulting in 1Hz message rate.
*/
/* Transmit CAN frame. */
counter++;
if (gpio_port_read(GPIO_PORT_B_BASE) && (1 << 6)) {
/* if status changed send command */
if (status == 1) {
data[4] = 0;
can_transmit(CAN1, 0x0000fffe, true, false, 5, data);
}
status = 0;
} else if (!(gpio_port_read(GPIO_PORT_B_BASE) && (1 << 7))) {
/* if status changed send command */
if (status == 0) {
data[4] = 1;
can_transmit(CAN1, 0x0000fffe, true, false, 5, data);
}
status = 1;
}
#if 1
switch (counter) {
case 125:
gpio_clear(GPIOB, GPIO3);
break;
case 250:
gpio_set(GPIOB, GPIO3);
break;
case 375:
gpio_clear(GPIOB, GPIO3);
break;
case 500:
gpio_set(GPIOB, GPIO4);
break;
case 1000:
counter = 0;
break;
default:
break;
}
#else
if (counter == 1000) {
counter = 0;
if (status) {
data[4] = 1;
} else {
data[4] = 0;
}
if (can_transmit(CAN1, 0x0000fffe, /* (EX/ST)ID: CAN ID */
true, /* IDE: CAN ID extended? */
false, /* RTR: Request transmit? */
5, /* DLC: Data length */
data) == -1) {
gpio_set(GPIOC, GPIO13); /* LED green off */
}
}
#endif
}
void vControl ( void *pvParameters )
{
portTickType xLastWakeTime;
signed char valx, valy;
unsigned char ls, rs, lb, rb;
can_frame_t out_frame;
can_frame_t in_frame;
out_frame.id = 1;
out_frame.dlc = 6;
xLastWakeTime = xTaskGetTickCount ();
/* Button init */
buttons_init ();
/* FSM init */
fsm_init ();
/* CAN init */
can_init ();
/* ADC init */
adc_init ( ctrlNUM_ADC_VALUES );
/* Touch init */
touch_init ( 30, 30, 30, 30 );
while (1)
{
vTaskDelayUntil ( &xLastWakeTime, ctrlTASK_FREQUENCY );
if ( adc_conversion_complete () == pdTRUE )
{
adc_disable ();
adc_get_value ( &valx, 0 );
adc_get_value ( &valy, 0 );
touch_measure ( &ls, &rs, &lb, &rb );
out_frame.data[0] = valx;
out_frame.data[1] = valy;
out_frame.data[2] = ls;
out_frame.data[3] = rs;
out_frame.data[4] = lb;
out_frame.data[5] = rb;
if (fsm_get_state() == ST_PLAY)
{
can_transmit (&out_frame);
}
can_receive (&in_frame, 0);
if (in_frame.data[0] == GAME_SENSOR_TRIGGERED)
{
// TODO: set triggered state
fsm_event_t *event = pvPortMalloc (sizeof (fsm_event_t));
event->type = EV_STOP;
event->ptr = NULL;
fsm_event_put (event, portMAX_DELAY);
in_frame.data[0] = 0;
}
else if (in_frame.data[0] == GAME_SENSOR_CLEARED)
{
// TODO: set cleared state
in_frame.data[0] = 0;
}
adc_enable ();
adc_conversion_start ();
}
fsm_update ();
}
}