int ma_mode(sigset_t* sigmask, sem_t* semaphore){
int ret;
pthread_t senderPID;
send_proc_t sender;
sender.nics = noCI;
sender.semaphore = semaphore;
if ( !MPinfo->iface ){
logmsg(stderr, MAIN, "No MA interface specifed!\n");
logmsg(stderr, MAIN, "See --help for usage.\n");
return EINVAL;
}
/* initialize sender */
if ( (ret=thread_create_sync(&senderPID, NULL, sender_caputils, &sender, "sender", NULL, SENDER_BARRIER_TIMEOUT)) != 0 ){
logmsg(stderr, MAIN, "thread_create_sync() [sender] returned %d: %s\n", ret, strerror(ret));
return ret;
}
/* initialize capture (blocking until all capture threads has finished) */
if ( (ret=setup_capture(semaphore)) != 0 ){
logmsg(stderr, MAIN, "setup_capture() returned %d: %s\n", ret, strerror(ret));
return ret;
}
/* Initialize MA-controller */
if ( (ret=thread_create_sync(&controlPID, NULL, control, NULL, "control", NULL, SENDER_BARRIER_TIMEOUT)) != 0 ){
logmsg(stderr, MAIN, "pthread_create() [controller] returned %d: %s\n", ret, strerror(ret));
return ret;
}
/* restore to default signal mask */
pthread_sigmask(SIG_SETMASK, sigmask, NULL);
/* wait for capture to end */
logmsg(verbose, MAIN, "goes to sleep; waiting for threads to finish.\n");
logmsg(verbose, MAIN, "Waiting for sender thread to finish\n");
pthread_join( senderPID, NULL);
for ( int i = 0; i < noCI; i++ ) {
logmsg(verbose, MAIN, "Waiting for CI[%d] thread to finish\n", i);
pthread_join(_CI[i].thread, NULL);
free(_CI[i].iface);
}
if ( controlPID ){
logmsg(verbose, MAIN, "Waiting for control thread to finish\n");
pthread_join(controlPID, NULL);
}
logmsg(stderr, MAIN, "Thread awakens, all threads finished. Stopping\n");
return 0;
}
/*
static void poll_ana7(void)
{
ioport_set_pin_mode(PIN_TC0_TIOA1,IOPORT_MODE_MUX_BIT0);
ioport_set_pin_dir(PIN_TC0_TIOA1,IOPORT_DIR_INPUT);
while(1)
{
bool b = ioport_get_pin_level(PIN_TC0_TIOA1);
ioport_set_pin_level(LED0_GPIO,b);
}
}
*/
int main(void)
{
sysclk_init();
board_init();
//poll_ana7();
//configure_tc();
setup_capture();
setup_leds();
while (1) {
delay_ms(1000);
}
}
int
main(int argc, char *argv[]) {
char c= 0;
int option_index= 0;
int req_arg_count=0;
printf("starting....\n");
//default init of optional args
config.scale_factor=1.0;
config.color_disable=0;
static struct option long_options[]=
{
{"if_name", required_argument, 0, 'i'},
{"if_mac", required_argument, 0, 'm'},
{"if_addr", required_argument, 0, 'a'},
{"port", required_argument, 0, 'p'},
{"cam_device", required_argument, 0, 'd'},
{"scale_factor", required_argument, 0, 's'},
{"color_disable", required_argument, 0, 'c'},
{0, 0, 0, 0}
};
while ( (c = getopt_long (argc, argv, "i:m:a:p:d:s:c:",
long_options, &option_index)) != -1)
{
switch (c)
{
case 'i':
config.if_name = optarg;
req_arg_count++;
break;
case 'm':
config.if_mac = optarg;
req_arg_count++;
break;
case 'a':
config.if_addr = optarg;
req_arg_count++;
break;
case 'p':
req_arg_count++;
config.port = optarg;
break;
case 'd':
config.cam_device = strtol(optarg, NULL, 10);
req_arg_count++;
break;
case 's':
sscanf(optarg, "%lf", &(config.scale_factor));
break;
case 'c':
config.color_disable = strtol(optarg, NULL, 10);
break;
default:
abort ();
}
}
if (req_arg_count !=5)
{
printf("Not enough required arguments: --if_name --if_mac --if_addr --port --cam_device (--scale_factor --color_disable)\n");
exit(-1);
}
printf("args parsed\n");
if (setup_capture(config.cam_device, config.scale_factor, config.color_disable))
exit(-2);
init_picotcp();
setup_udp_app();
printf("started.\n");
pico_stack_loop();
free_resources();
return 0;
}
/*********************************************************************
Function: int main(void)
PreCondition: None.
Input: None.
Output: None.
Side Effects: None.
Overview: main function of the application. Peripherals are
initialized.
Note: None.
********************************************************************/
int main(void)
{
int cs;
// vars used for detection of incremental motion
unsigned short new_cmd,last_cmd, new_fb,last_fb;
setup_io(); // make all i/o pins go the right dir
STATUS_LED = 0; // led on
setup_uart(); // setup the serial interface to the PC
setup_TMR1(); // set up 1ms timer
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
// needed for delays in following routines
// 1/2 seconds startup delay
timer_test = 5000;
while ( timer_test );
printf("\r\nPowerup..i/o...uart...timer...");
setup_pwm(); // start analog output
set_pwm(0.0);
printf("pwm...");
init_pid();
printf("pid...");
setup_encoder(); // 16 bit quadrature encoder module setup
printf("encoder...");
setup_capture(); // 2 pins with quadrature cmd from PC
printf("capture...");
printf("done\r\n");
// some junk for the serial channel
printf("%s%s\n\r",CPWRT,VERSION);
STATUS_LED = 1; // led off when init finished
// restore config from eeprom
// Read array named "setupEE" from DataEEPROM and place
// the result into array in RAM named, "setup"
restore_setup();
cs = calc_cksum(sizeof(pid)/sizeof(int),(int*)&pid);
if ( cs )
{
// opps, no valid setup detected
// assume we are starting from a new box
printf("No valid setup found in EEPROM\r\n");
init_pid();
}
else
{
printf("Using setup from eeprom.. ? for help\r\n");
print_tuning();
}
printf("using %fms servo loop interval\r\n",pid.ticksperservo * 0.1);
// BLOCK OF TEST ROUTINES FOR HARDWARE DEBUGGING
// test_pwm_interface(); // play with opa549 hardware
// test_pc_interface(); // echo cmded posn to serial port
// test_pid_interface(); // test pid loop operation
new_cmd = last_cmd = new_fb = last_fb = 0;
while (1)
{
if ( do_servo ) // check and see if timer 1 has asked for servo calcs to be run
{
do_servo = 0;
if (SVO_ENABLE)
{
if ( pid.enable == 0 ) // last loop, servo was off
{
set_pwm( 0.0 );
printf("servo-enabled\r\n>");
pid.enable = 1;
// make sure we dont move on enabling
cmd_posn = POSCNT; // make 16bit incr registers match
pid.command = 0L; // make 32 bit counter match
pid.feedback = 0L;
// make the 1ms loop temps match
new_cmd = last_cmd = new_fb = last_fb = 0;
pid.error_i = 0.0; // reset integrator
}
// we can time the servo cycle calcs by scoping the PID_ACTIVE pin
PID_ACTIVE = 1; // seems to take about 140us
new_cmd = cmd_posn; // grab current cmd from pc
new_fb = POSCNT; // grab current posn from encoder
pid.command += (long int)((short)(new_cmd - last_cmd));
pid.feedback += (long int)((short)(new_fb - last_fb ));
last_cmd = new_cmd;
last_fb = new_fb;
calc_pid();
// check for a drive fault ( posn error > allowed )
if (( pid.maxerror > 0.0 ) &&
( fabs(pid.error) > pid.maxerror ))
{
short temp = SVO_ENABLE;
set_pwm( 0.0 );
while (1) // trap here until svo disabled or pwr cycle
{
// reset integrator as it may have wound up
pid.error_i = 0.0;
printf("drive fault... maxerror exceeded\r\n");
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
if (temp != SVO_ENABLE)
break;
}
}
else
{
set_pwm(pid.output); // update motor drive
}
PID_ACTIVE = 0; // i/o pin for timing pid calcs
}
else
{
if ( pid.enable == 1 ) // last loop servo was active
{
set_pwm( 0.0 );
pid.enable = 0;
printf("servo-disabled\r\n>");
// extra delay keeps us faulted for min 1 sec to let mechanicals settle
STATUS_LED = 1; timer_test = 10000; while ( timer_test );
}
}
}
// look for serial cmds
// doing this while svo is enabled will cause bumps in servo loop
// because of serial i/o time ( unless we get smart and move svo loop
// into an isr )
if ( rxrdy )
process_serial_buffer();
if (pid.limit_state) // show we are at drive limit(error)
STATUS_LED = 0;
else
STATUS_LED = 1;
}
// to keep compiler happy....
return 0;
}
int main(int argc, char *argv[])
{
pthread_t parser_thread;
if (argc < 3) {
fprintf(stderr, "Usage: %s <input file> <m2m device>\n", argv[0]);
return 1;
}
if (input_open(argv[1]) < 0)
return -1;
if (m2m_open(argv[2]) < 0)
return -1;
if (subscribe_source_change() < 0)
return -1;
/* Setup flow based on ELCE2014 slides and Nicolas Dufresne's knowledge:
* S_FMT(OUT)
* S_FMT(CAP) to suggest a fourcc for the raw format; may be changed later
* G_CTRL(MIN_BUF_FOR_OUTPUT)
* REQBUFS(OUT)
* QBUF (the header)
* STREAMON(OUT)
* QBUF/DQBUF frames on OUT
* source change event, DQEVENT
* G_FMT(CAP)
* ENUM_FMT(CAP)
* S_FMT(CAP) to set fourcc chosen from ENUM_FMT; also get resolution from returned values?
* G_SELECTION to get visible size
* G_CTRL(MIN_BUF_FOR_CAPTURE)
* REQBUFS(CAP)
* STREAMON(CAP)
*/
if (output_set_format() < 0)
return -1;
// FIXME capture S_FMT
// FIXME G_CTRL(MIN_BUF_FOR_OUTPUT)
if (output_request_buffers() < 0)
return -1;
map_output();
//if (parse_and_queue_header() < 0)
// return -1;
parse_one_nal();
stream(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, VIDIOC_STREAMON);
parser_init();
if (pthread_create(&parser_thread, NULL, parser_thread_func, NULL)) {
fprintf(stderr, "Failed to launch parser thread\n");
return -1;
}
if (wait_for_source_change() < 0)
return -1;
if (setup_capture() < 0)
return -1;
map_capture();
queue_capture();
stream(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, VIDIOC_STREAMON);
capture();
pthread_join(parser_thread, 0);
return 0;
}
