int main(int argc, char **argv) {
int nbytes,socket_can1, socket_can2, ret;
fd_set readfds;
long count;
struct can_frame *pcan_frame;
struct timeval tv1, tv2;
float elapsed_time, bandwidth, packets_per_second;
signal(SIGINT, handler);
socket_can1=init_can("vcan0");
socket_can2=init_can("vcan1");
pcan_frame = (struct can_frame *)malloc(sizeof(struct can_frame));
bzero(pcan_frame,sizeof(struct can_frame));
pcan_frame->can_id=123;
pcan_frame->can_dlc=4;
count = NUM_OF_FRAMES;
gettimeofday(&tv1, NULL);
bounce_can_frame(socket_can2, pcan_frame);
while (count>=0) {
FD_ZERO(&readfds);
FD_SET(socket_can1, &readfds);
FD_SET(socket_can2, &readfds);
ret = select((socket_can1 > socket_can2)?socket_can1+1:socket_can2+1, &readfds, NULL, NULL, NULL);
/* received a CAN frame */
if (FD_ISSET(socket_can1, &readfds)) {
if ((nbytes = read(socket_can1, pcan_frame, sizeof(struct can_frame))) < 0) {
perror("read error on CAN\n");
return -1;
}
bounce_can_frame(socket_can1, pcan_frame);
}
if (FD_ISSET(socket_can2, &readfds)) {
if ((nbytes = read(socket_can2, pcan_frame, sizeof(struct can_frame))) < 0) {
perror("read error on CAN\n");
return -1;
}
bounce_can_frame(socket_can2, pcan_frame);
}
count--;
}
gettimeofday(&tv2, NULL);
elapsed_time = (float)((tv2.tv_sec - tv1.tv_sec) * 1E6 + (tv2.tv_usec - tv1.tv_usec)) / 1E6;
bandwidth=NUM_OF_FRAMES * CAN_NORMAL_FRAME_BITS(4) / elapsed_time / 1E6;
packets_per_second=NUM_OF_FRAMES / elapsed_time / 1000;
printf("Send %d in %.3f sec (%.3f tpackets/sec %.3f Mbit)\n",NUM_OF_FRAMES,elapsed_time, packets_per_second, bandwidth);
return 0;
}
void candle_can_init(void) {
rx_callback = 0;
memset(led_status, 0, sizeof(led_status));
clear_tx_queue(0);
clear_tx_queue(1);
// enable led outputs
rcc_periph_clock_enable(RCC_GPIOD);
gpio_mode_setup(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO12 | GPIO13 | GPIO14 | GPIO15);
// enable can1 peripheral
rcc_periph_clock_enable(RCC_GPIOD);
rcc_periph_clock_enable(RCC_CAN1);
gpio_mode_setup(GPIOD, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO0 | GPIO1);
gpio_set_output_options(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO0 | GPIO1);
gpio_set_af(GPIOD, GPIO_AF9, GPIO0 | GPIO1);
reset_can(CAN1);
init_can(CAN1);
// enable can1 transceiver
rcc_periph_clock_enable(RCC_GPIOC);
gpio_mode_setup(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6);
gpio_clear(GPIOC, GPIO6);
// enable can2 peripheral
rcc_periph_clock_enable(RCC_GPIOB);
rcc_periph_clock_enable(RCC_CAN2);
gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO12 | GPIO13);
gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO12 | GPIO13);
gpio_set_af(GPIOB, GPIO_AF9, GPIO12 | GPIO13);
reset_can(CAN2);
init_can(CAN2);
// enable can2 transceiver
rcc_periph_clock_enable(RCC_GPIOD);
gpio_mode_setup(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO11);
gpio_clear(GPIOD, GPIO11);
// init filter banks
CAN_FMR(CAN1) |= CAN_FMR_FINIT; // switch filter banks to init mode
// configure usage of 14 filter banks for can1 and 14 banks for can2
CAN_FMR(CAN1) &= ~CAN_FMR_CAN2SB_MASK;
CAN_FMR(CAN1) |= (14<<CAN_FMR_CAN2SB_SHIFT);
can_filter_id_mask_32bit_init(CAN1, 0, 0, 0, 0, 1); // set a catch-all filter for CAN1 fifo 0
can_filter_id_mask_32bit_init(CAN2, 14, 0, 0, 0, 1); // set a catch-all filter for CAN2 fifo 0
CAN_FMR(CAN1) &= ~CAN_FMR_FINIT; // switch filter banks to active mode
}
main()
{
int i;
unsigned char frame_basic0[10] = {0xEA,0x28,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char frame_basic1[10] = {0xEB,0x28,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char frame_basic2[10] = {0xEC,0x28,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char frame_basic3[10] = {0xED,0x28,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char frame_extended[13] = {0x88,0xA6,0xB0,0x12,0x30,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char r_val;
init_can("Basic","normal");
enable_irq_sff();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
release_receive_buffer();
printf("interrupt register 0x%X \n",interrupt_register);
while(1)
{
if ((interrupt_register & receive_interrupt) == 0)
{
kill_time(4000000);
printf("No data!\n");
}
else
{
read_receive_buffer_basic();
release_receive_buffer();
}
}
}
void main(void){
init_general();// Set general runtime configuration bits
init_gpio_pins(); // Set all I/O pins to low outputs
init_oscillator(0);// Initialize oscillator configuration bits
init_timer2();// Initialize timer2 (millis)
init_adc(NULL); // Initialize ADC module
init_termination(NOT_TERMINATING);
init_adcs();// Initialize all of the ADC's
init_can(); // Initialize CAN
canAnalogMillis = canDiagMillis = 0;
ADCCON3bits.GSWTRG = 1; // Initial ADC Conversion?
STI();// Enable interrupts
while(1){
update_analog_channels();
strain_calc();
if(millis - canAnalogMillis >= CAN_ANALOG_INTV){
CANAnalogChannels();
canAnalogMillis = millis;
}
if(millis - canDiagMillis >= CAN_DIAG_INTV){
CANdiag();
canDiagMillis = millis;
}
sample_temp(); // Sample internal and external temperature sensors
}
}
void init_communication_interfaces( void )
{
#ifdef DEBUG
init_serial( );
#endif
DEBUG_PRINT( "init Control CAN - " );
init_can( g_control_can );
}
int main(void) {
/* Init Clock */
PLLFBD = 126;
CLKDIVbits.PLLPOST = 0;
CLKDIVbits.PLLPRE = 1;
__builtin_write_OSCCONH(0x03);
__builtin_write_OSCCONL(OSCCON | 0x01);
while (OSCCONbits.COSC != 3) ;
while (OSCCONbits.LOCK != 1) ;
tx_fifo.head=0;
tx_fifo.tail=0;
rx_fifo.head=0;
rx_fifo.tail=0;
/* Init PORT I/O */
init_io();
init_pps();
init_uart();
init_can();
while (true) {
counter++;
counter_can++;
fifo_putchar(&tx_fifo);
if (can_test_receive())
print_rom_fifo("received CAN packet\r\n", &tx_fifo);
if (counter_can == 30000) {
can_test_send();
counter_can = 0;
}
if (counter == 50000) {
print_rom_fifo("Hello dsPIC33 !\r\n", &tx_fifo);
//print_debug_fifo(&tx_fifo);
// ClrWdt();
counter = 0;
}
__delay_us(10);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
__builtin_btg((unsigned int *)&LATA, 0);
}
}
void init_communication_interfaces( void )
{
#ifdef DEBUG
init_serial( );
#endif
DEBUG_PRINT( "init Control CAN - " );
init_can( g_control_can );
// Filter CAN messages - accept if (CAN_ID & mask) == (filter & mask)
// Set buffer 0 to filter only brake module and global messages
g_control_can.init_Mask( 0, 0, 0x7F0 ); // Filter for 0x0N0 to 0x0NF
g_control_can.init_Filt( 0, 0, OSCC_BRAKE_CAN_ID_INDEX );
g_control_can.init_Filt( 1, 0, OSCC_FAULT_CAN_ID_INDEX );
// Accept only CAN Disable when buffer overflow occurs in buffer 0
g_control_can.init_Mask( 1, 0, 0x7FF ); // Filter for one CAN ID
g_control_can.init_Filt( 2, 1, OSCC_BRAKE_DISABLE_CAN_ID );
}
int main() {
printf("INIT %d %d\r\n", CAN_THREAD_PERIOD, PERIOD_100ms);
led1 = 0;
set_time(0);
uint32 message_mask = NET_TX_CMD_BODY | NET_RX_STS_BODY |
NET_TX_CMD_ENGINE |
NET_TX_CMD_DIAG | NET_RX_STS_DIAG |
NET_TX_CMD_TIME;
init_can(message_mask);
Thread* th_txer = new Thread(sender, 40, SENDER_PERIOD_MS, NULL, osPriorityHigh);
Thread* th_rxer = new Thread(receiver, 25, RECEIVER_PERIOD_MS, NULL, osPriorityHigh);
Thread* th_can = new Thread(thread_can, 12, CAN_THREAD_PERIOD, NULL, osPriorityRealtime);
printf("RUN\r\n");
while(1) {};
}
int main( int argc, char *argv[] )
{
int ch;
int pgn; /* Parameter Group Number */
struct j1939_pdu pdu; /* Protocol Data Unit */
posix_timer_typ *ptimer; /* Timing proxy */
int interval = 20;
int slot_number = 1;
int i;
int do_send = 0; /* receives by default */
int msg_count = 0;
int fd; /* "file descriptor" for CAN device */
char *filename = "/dev/can1";
int extended = -1;
int flags = O_RDONLY;
while ((ch = getopt(argc, argv, "d:f:i:n:p:st:")) != EOF) {
switch (ch) {
case 'f': filename = strdup(optarg);
break;
case 'i': interval = atoi(optarg);
break;
case 'n': slot_number = atoi(optarg);
break;
case 'p': pgn = atoi(optarg);
break;
case 's': do_send = 1;
flags = O_WRONLY;
break;
case 't': interval = atoi(optarg);
break;
default: printf(
"Usage: %s [-p PGN number -i interval]\n",
argv[0]);
exit(1);
break;
}
}
fprintf(stderr, "request PGN %d (0x%x)\n", pgn, pgn);
/* Initialize CAN device */
fd = init_can(filename, flags, NULL);
if (fd < 0) {
printf("Error opening device %s for input\n", filename);
exit(EXIT_FAILURE);
}
if (do_send) {
int chid = ChannelCreate(0);
if ((ptimer = timer_init(interval, chid )) == NULL) {
printf("Unable to initialize canj1939 timer\n");
exit(EXIT_FAILURE);
}
}
if( setjmp( exit_env ) != 0 ) {
printf("canj1939 exits, %s %d\n",
do_send?"sent":"received", msg_count);
close_can(&fd);
exit( EXIT_SUCCESS );
} else
sig_ign( sig_list, sig_hand );
if (do_send) {
/* assemble request from arguments */
pdu.priority = 6;
pdu.R = 0;
pdu.DP = 0;
pdu.pdu_format = HIBYTE(RQST);
pdu.pdu_specific = GLOBAL;
pdu.src_address = 249; /* Service Tool address */
pdu.numbytes = 3;
/* current PGNs are only two bytes, but field is 3 bytes,
LSB first */
pdu.data_field[0] = LOBYTE(pgn);
pdu.data_field[1] = HIBYTE(pgn);
pdu.data_field[2] = 0;
for (i = 3; i < 8; i++)
pdu.data_field[i] = 0xff;
}
/* state code and slot number not used */
if (do_send) {
for ( ; ; ) {
send_can(fd, &pdu, slot_number);
TIMER_WAIT( ptimer );
msg_count++;
}
}
else {
for ( ; ; ) {
if (!receive_can(fd, &pdu, &extended, &slot_number))
printf("call to receive_can returned error\n");
else {
printf("Pr %d PF %d PS %d", pdu.priority,
pdu.pdu_format, pdu.pdu_specific);
printf("SA %d: ", pdu.src_address);
for (i = 0; i < 8; i++)
printf("%d ", pdu.data_field[i]);
printf(" extended %d\n", extended);
msg_count++;
}
}
}
}
void testloop()
{
//select which loop routine you want to execute
//#define TEST_GROUNDLINK
//#define TEST_UAVTALK
//#define TEST_MAVLINK
//#define TEST_ARDUIMU_V3
//#define TEST_NODELINK
//#define TEST_BLINK_APBOARD
//#define TEST_SERVOS
//#define TEST_QUADCOPTER
//#define TEST_SPEKTRUMSATELLITE
//#define TEST_IMU_CHR6D
#ifdef TEST_QUADCOPTER
/* Basic quadrocopter nick and roll damping. Sequence of events:
* 1.) Reads gyros to get d(nick)/dt and d(roll)/dt in °/s
* 2.) Gets RC receiver input ([-1000, +1000]) to determine
* a) the desired vertical acceleration [m/s²]
* b) the desired pitch and roll rate [°/s]
* 3.) calculates
* a) the necessary level of thrust for all 4 engines to achieve the desired vertical acceleration (open loop, for now)
* b) the necessary level of thrust offsets for the left/right and leading/rear engine. E.g. for a desired nick
* rate of 10°/s that may be +120/-120
* 4.) Sets the calculated gas positions.
*/
int32_t v = 0;
while(true){
TimeBase::waitMicrosec(100000);
//led->toggle();
// gLink->sendValue(v);
v--;
}
#endif
#ifdef TEST_ARDUIMU_V3
ArduIMU_V3* imu = new ArduIMU_V3();
Devices::backPointer->statusLED.set(LED::state_OFF);
while(true){
imu->test();
TimeBase::waitMicrosec(500000);
Devices::backPointer->statusLED.toggle();
}
#endif
#ifdef TEST_MAVLINK
LED* lede = new LED(GPIOC, GPIO_Pin_12, LED::state_ON);
MAVLink* mavlink = new MAVLink(USART2);
ArduIMU_V3* imu = new ArduIMU_V3();
lede->set(LED::state_OFF);
uint32_t heartBeatTime = TimeBase::getSystemTimeMs();
uint32_t airdataTime = TimeBase::getSystemTimeMs();
uint32_t newTime = 0;
while(true){
newTime = TimeBase::getSystemTimeMs();
// Do every second:
if((newTime - heartBeatTime)>1000)
{
mavlink->sendHeartbeat();
// lede->toggle();
heartBeatTime = newTime;
}
//Do every 10 milliseconds
if((newTime-airdataTime)>100)
{
mavlink->sendAirData((uint8_t)(imu->values.temp));
airdataTime = newTime;
}
}
#endif
#ifdef TEST_UAVTALK
LED* lede = new LED(GPIOC, GPIO_Pin_12, LED::state_ON);
UAVTalk* talk = new UAVTalk(USART2);
int32_t v = 0;
lede->set(LED::state_OFF);
while(true){
TimeBase::waitMicrosec(500000);
lede->set(LED::state_ON);
TimeBase::waitMicrosec(1000);
lede->set(LED::state_OFF);
talk->sendTestMessage();
v--;
}
#endif
#ifdef TEST_GROUNDLINK
LED* led = new LED(GPIOC, GPIO_Pin_12, LED::state_ON);
GroundLink* gLink = new GroundLink(USART2);
int32_t v = 0;
led->set_GPIOC_10(0);
while(true){
TimeBase::waitMicrosec(500000);
led->toggle();
TimeBase::waitMicrosec(500000)
gLink->sendRawInertial();
v--;
}
#endif
#ifdef TEST_NODELINK
init_can();
while(true){
TimeBase::waitMicrosec(100000);
can_send_test();
Devices::backPointer->statusLED.toggle();
}
#endif
#ifdef TEST_IMU_CHR6D
while(true)
{
IMU_CHR6D* imu = new IMU_CHR6D(USART3);
imu->testLoop();
}
#endif
#ifdef TEST_SERVOS
bool setPulsewidth = false;
bool setPosition = true;
uint32_t servoBankGpioPins[2] = { (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6 | GPIO_Pin_7),
(GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_6| GPIO_Pin_7| GPIO_Pin_8| GPIO_Pin_9)
};
GPIO_TypeDef* servoBankGPIOs[2] = {GPIOA, GPIOB};
TIM_TypeDef* servoBankTimers[3] = {TIM2, TIM3, TIM4};
uint32_t defaultChannelWidths[12] = {1000000, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000,
1000000, 1000000, 1000000, 1000000, };
ServoBank* servoBank = new ServoBank (12,
servoBankTimers, 3,
servoBankGPIOs,
&servoBankGpioPins[0],
2,
20000000,
defaultChannelWidths);
uint32_t pwidth[12] = {0};
int16_t position[12] = {0};
//vary servo position from -100% to +100%
while(1)
{
if(setPosition){
for(int16_t p=-1000; p<1000; p+=5){
for(uint8_t i=0; i<12; i++){
position[i]= p;
}
servoBank->setPositions(position);
TimeBase::waitMicrosec(5000);
}
}
else if(setPulsewidth){
for(uint32_t pwi= 1000000; pwi<2000000; pwi+=300){
for(uint8_t i=0; i<12; i++){
pwidth[i]= pwi;
}
servoBank->setPWMPulseWidth(pwidth);
TimeBase::waitMicrosec(5000);
}
}
}
#endif
#ifdef TEST_BLINK_APBOARD
SerialPort* s = new SerialPort(USART1, 115200);
LED* led = new LED(GPIOC, GPIO_Pin_12, LED::state_ON);
while(1)
{
led->toggle();
s->sendTestMessage();
TimeBase::waitMicrosec(600000);
}
#endif
#ifdef TEST_BLINK
while(1)
{
TimeBase::waitMicrosec(500000);
Devices::backPointer->statusLED.toggle();
Modules::backPointer->messagePort.sendTestMessage();
}
#endif
#ifdef TEST_SPEKTRUMSATELLITE
//GroundLink* gLink = new GroundLink(USART1);
uint16_t counter = 0;
int16_t rcChannels[12] = {0};
while(1){
counter++;
Devices::backPointer->spektrumSatellite.getRcChannels(rcChannels);
Devices::backPointer->servoBank.setPositions(rcChannels);
// gLink->sendValue((int32_t)rcChannels[2]*1000);
TimeBase::waitMicrosec(10000);
if(counter>=10)
{
Devices::backPointer->statusLED.toggle();
counter = 0;
}
}
#endif
return;
}//eof
main()
{
unsigned char frame_basic[10] = {0xEA,0x28,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char frame_extended[13] = {0x88,0xA6,0xB0,0x12,0x30,0x12,0x34,0x56,0x78,0x9A,0xBC,0xDE,0xF0};
unsigned char r_val;
// test *** 1 Basic --> send frame basic
printf("************************************\n");
printf("****** Test 1 **********************\n");
printf("************************************\n");
printf("****** Send frame basic ************\n");
printf("************************************\n");
printf("************************************\n\n");
init_can("Basic","normal");
enable_irq_sff();
write_frame_basic(frame_basic);
tx_request_command();
//read_receive_buffer_basic();
reset_all_irqs();
// test *** 2 Extended --> send frame extended
printf("************************************\n");
printf("****** Test 1 **********************\n");
printf("************************************\n");
printf("****** Send frame extended *********\n");
printf("************************************\n");
printf("************************************\n\n");
init_can("Extended","no_self_test");
enable_irq_eff();
write_frame_extended(frame_extended);
tx_request_command();
read_receive_buffer_extended();
reset_all_irqs();
// test *** 3 Extended --> self test mode
printf("************************************\n");
printf("****** Test 1 **********************\n");
printf("************************************\n");
printf("****** Self test mode **************\n");
printf("****** extended ********************\n");
printf("************************************\n\n");
init_can("Extended","self_test");
enable_irq_eff();
write_frame_extended(frame_extended);
self_reception_request();
read_receive_buffer_extended();
if (test_read_frame_extended(frame_extended) == 1)
{
printf( "Test Passed !!!\n");
}
else
{
printf( "Test failed !!!\n");
}
reset_all_irqs();
// test *** 3 Extended --> self test mode
}
