// PC⇔DUT通信
int rs_commu(char *cmd) {
// DUTが接続されたポートを検出
// 接続されていない場合は、エラー表示して戻る
// if (!com_search(g.dut_port)) {
// SbarPrintf(g.hSbar,0,"%%DUT not connect\n");
// return 1;
// }
if (g.dut_state == DUT_CONNECT) { // 接続中ならば初期化なし
rs_cmd_proc(cmd);
}
else {
// シリアルポートの初期化
if (!rs_init(g.dut_port,g.dut_speed,8,0,0)) {
rs_cmd_proc(cmd);
g.dut_state = DUT_CONNECT;
}
else {
printf("%%DUT busy\n");
sprintf(g.dut_result,"%%DUT busy");
SbarPrintf(g.hSbar,0,"%%DUT busy");
g.dut_state = DUT_BUSY;
}
}
if (!g.rs_mode) {
rs_end();
g.dut_state = DUT_DISCONNECT;
}
return 1;
}
// 初始化字符设备
void tty_init(void)
{
// 设置串口
rs_init();
// 设置显示器
con_init();
}
void init_robot(struct robot *bot){
rs_init(&bot->rs);
cs_init(&bot->cs_a);
cs_init(&bot->cs_d);
quadramp_init(&bot->qr_a);
quadramp_init(&bot->qr_d);
pid_init(&bot->pid_a);
pid_init(&bot->pid_d);
position_init(&bot->posr); //nous
// bot->EVENT_DO_CS =1;
bot->events = EVENT_DO_CS | EVENT_DO_POS;
quadramp_init(&bot->qr_a);
quadramp_init(&bot->qr_d);
#ifdef UART_VERBOSE
UART_CPutString("\r\n init robot structures : [OK]");
#endif
}
int main(int argc1, char *argv[])
{
route_init(NULL, 0);
route_register(&rt_filea_method);
route_register(&rt_fileov_method);
route_register(&rt_stdin_method);
route_register(&rt_stdout_method);
route_register(&rt_stderr_method);
route_register(&rt_rs_method);
if ( ! elog_init(1, "cascade test", NULL))
elog_die(FATAL, "didn't initialise elog\n");
out = route_open("stdout", NULL, NULL, 0);
err = route_open("stderr", NULL, NULL, 0);
rs_init();
/* run cascade with all the possible modes */
test_cascade(CASCADE_AVG, "avg",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_AVGSING, RES_AVGSINGKEY, RES_AVGMULT, RES_AVGMULTKEY);
test_cascade(CASCADE_MIN, "min",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_MINSING, RES_MINSINGKEY, RES_MINMULT, RES_MINMULTKEY);
test_cascade(CASCADE_MAX, "max",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_MAXSING, RES_MAXSINGKEY, RES_MAXMULT, RES_MAXMULTKEY);
test_cascade(CASCADE_SUM, "sum",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_SUMSING, RES_SUMSINGKEY, RES_SUMMULT, RES_SUMMULTKEY);
test_cascade(CASCADE_FIRST, "first",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_FIRSTSING, RES_FIRSTSINGKEY, RES_FIRSTMULT,
RES_FIRSTMULTKEY);
test_cascade(CASCADE_LAST, "last",
TAB_SING, TAB_SINGINFO, TAB_SINGINFOKEY,
TAB_MULT, TAB_MULTINFO, TAB_MULTINFOKEY,
RES_LASTSING, RES_LASTSINGKEY, RES_LASTMULT,
RES_LASTMULTKEY);
rs_fini();
elog_fini();
route_close(err);
route_close(out);
route_fini();
printf("tests finished successfully\n");
exit(0);
}
int librlc_rs_repair(int k, int m, int w, int packet_size,
char *available_data, int *data_list, int data_num,int chunk_len,
int *repair_list, int repair_num, char **out_data)
{
int ret = 0;
rs_coder_t rs_code;
//rs init
rs_init(&rs_code, k+m, k, w, packet_size);
// re repair
ret = rs_repair(rs_code.prsi, available_data, data_list, data_num,
chunk_len, repair_list, repair_num, out_data);
// rs free
rs_free(&rs_code);
return ret;
}
int librlc_rs_encode(int k, int m, int w, int packet_size,
const char *orig_data, int orig_data_len,
char **encoded_data, char **encoded_parity,
int *chunk_len)
{
rs_coder_t rs_code;
// rs init
rs_init(&rs_code, k+m, k, w, packet_size);
// rs encode
rs_encode(rs_code.prsi, orig_data, orig_data_len, encoded_data, encoded_parity, chunk_len);
// rs free
rs_free(&rs_code);
return 0;
}
/**
* init_rs_internal - Find a matching or allocate a new rs control structure
* @symsize: the symbol size (number of bits)
* @gfpoly: the extended Galois field generator polynomial coefficients,
* with the 0th coefficient in the low order bit. The polynomial
* must be primitive;
* @gffunc: pointer to function to generate the next field element,
* or the multiplicative identity element if given 0. Used
* instead of gfpoly if gfpoly is 0
* @fcr: the first consecutive root of the rs code generator polynomial
* in index form
* @prim: primitive element to generate polynomial roots
* @nroots: RS code generator polynomial degree (number of roots)
*/
static struct rs_control *init_rs_internal(int symsize, int gfpoly,
int (*gffunc)(int), int fcr,
int prim, int nroots)
{
/* Sanity checks */
if (symsize < 1)
return NULL;
if (fcr < 0 || fcr >= (1<<symsize))
return NULL;
if (prim <= 0 || prim >= (1<<symsize))
return NULL;
if (nroots < 0 || nroots >= (1<<symsize))
return NULL;
return rs_init(symsize, gfpoly, gffunc, fcr, prim, nroots);
}
void __init pcibios_init(void)
{
pci_controller_probe();
if (pci_controller_root == NULL)
return;
pci_scan_each_controller_bus();
if (pci_device_reorder)
pci_reorder_devs();
isa_init();
ebus_init();
rs_init();
clock_probe();
power_init();
}
/**
* main entry point
*/
void main(void)
{
unsigned char n;
// hardware initialisieren
restart_hw();
// warten bis der BUS stabil ist
for (n=0; n<50; n++)
{
set_timer0(0xFFFF);
while(!TF0);
}
// Protokoll zurücksetzen
restart_prot();
#ifdef DEBUG
// serielle Schnittstelle für DEBUG Ausgaben initialisieren
rs_init(576);
#endif
// initialize/reset application
restart_app();
// main loop
while (1)
{
if (!TR1) // check if TR1 is active -> send/receive finished
{
// applikation ausführen
app_process();
if (RTCCON>=0x80)
{
stop_rtc();
rtc_process(); // Realtime clock Ueberlauf
start_rtc(65); // RTC mit 65ms neu starten
}
}
// check program button
check_prog_button();
}
}
int main(int argc, char *argv[])
{
Cell root;
clock_t start;
char *prog_file = NULL;
int i;
int print_stats = 0;
int parse_only = 0;
for (i = 1; i < argc && argv[i][0] == '-'; i++) {
if (strcmp(argv[i], "-g") == 0)
gc_notify = 1;
else if (strcmp(argv[i], "-s") == 0)
print_stats = 1;
else if (strcmp(argv[i], "-p") == 0)
parse_only = 1;
else if (strcmp(argv[i], "-u") == 0)
setbuf(stdout, NULL);
else
errexit("unknown option %s\n", argv[i]);
}
input_init(argv + i);
storage_init(INITIAL_HEAP_SIZE);
rs_init();
root = load_program();
if (parse_only) {
unparse(root);
return 0;
}
start = clock();
eval_print(root);
if (print_stats) {
double evaltime = (clock() - start) / (double)CLOCKS_PER_SEC;
printf("\n%d reductions\n", reductions);
printf(" total eval time --- %5.2f sec.\n", evaltime - total_gc_time);
printf(" total gc time --- %5.2f sec.\n", total_gc_time);
printf(" max stack depth --- %d\n", rs_max_depth());
}
return 0;
}
void tty_init(void)
{
rs_init(); // initializes console interupts, see kernel/console.c
con_init(); // kernel/serial.c
}
void __init sbus_init(void)
{
int nd, this_sbus, sbus_devs, topnd, iommund;
unsigned int sbus_clock;
struct sbus_bus *sbus;
struct sbus_dev *this_dev;
int num_sbus = 0; /* How many did we find? */
#ifndef __sparc_v9__
register_proc_sparc_ioport();
#endif
#ifdef CONFIG_SUN4
return sun4_dvma_init();
#endif
topnd = prom_getchild(prom_root_node);
/* Finding the first sbus is a special case... */
iommund = 0;
if(sparc_cpu_model == sun4u) {
nd = prom_searchsiblings(topnd, "sbus");
if(nd == 0) {
#ifdef CONFIG_PCI
if (!pcibios_present()) {
prom_printf("Neither SBUS nor PCI found.\n");
prom_halt();
} else {
#ifdef __sparc_v9__
firetruck_init();
#endif
}
return;
#else
prom_printf("YEEE, UltraSparc sbus not found\n");
prom_halt();
#endif
}
} else if(sparc_cpu_model == sun4d) {
if((iommund = prom_searchsiblings(topnd, "io-unit")) == 0 ||
(nd = prom_getchild(iommund)) == 0 ||
(nd = prom_searchsiblings(nd, "sbi")) == 0) {
panic("sbi not found");
}
} else if((nd = prom_searchsiblings(topnd, "sbus")) == 0) {
if((iommund = prom_searchsiblings(topnd, "iommu")) == 0 ||
(nd = prom_getchild(iommund)) == 0 ||
(nd = prom_searchsiblings(nd, "sbus")) == 0) {
#ifdef CONFIG_PCI
if (!pcibios_present()) {
prom_printf("Neither SBUS nor PCI found.\n");
prom_halt();
}
return;
#else
/* No reason to run further - the data access trap will occur. */
panic("sbus not found");
#endif
}
}
/* Ok, we've found the first one, allocate first SBus struct
* and place in chain.
*/
sbus = sbus_root = kmalloc(sizeof(struct sbus_bus), GFP_ATOMIC);
sbus->next = NULL;
sbus->prom_node = nd;
this_sbus = nd;
if(iommund && sparc_cpu_model != sun4u && sparc_cpu_model != sun4d)
iommu_init(iommund, sbus);
/* Loop until we find no more SBUS's */
while(this_sbus) {
#ifdef __sparc_v9__
/* IOMMU hides inside SBUS/SYSIO prom node on Ultra. */
if(sparc_cpu_model == sun4u) {
extern void sbus_iommu_init(int prom_node, struct sbus_bus *sbus);
sbus_iommu_init(this_sbus, sbus);
}
#endif
#ifndef __sparc_v9__
if (sparc_cpu_model == sun4d)
iounit_init(this_sbus, iommund, sbus);
#endif
printk("sbus%d: ", num_sbus);
sbus_clock = prom_getint(this_sbus, "clock-frequency");
if(sbus_clock == -1)
sbus_clock = (25*1000*1000);
printk("Clock %d.%d MHz\n", (int) ((sbus_clock/1000)/1000),
(int) (((sbus_clock/1000)%1000 != 0) ?
(((sbus_clock/1000)%1000) + 1000) : 0));
prom_getstring(this_sbus, "name",
sbus->prom_name, sizeof(sbus->prom_name));
sbus->clock_freq = sbus_clock;
#ifndef __sparc_v9__
if (sparc_cpu_model == sun4d) {
sbus->devid = prom_getint(iommund, "device-id");
sbus->board = prom_getint(iommund, "board#");
}
#endif
sbus_bus_ranges_init(iommund, sbus);
sbus_devs = prom_getchild(this_sbus);
if (!sbus_devs) {
sbus->devices = NULL;
goto next_bus;
}
sbus->devices = kmalloc(sizeof(struct sbus_dev), GFP_ATOMIC);
this_dev = sbus->devices;
this_dev->next = NULL;
this_dev->bus = sbus;
this_dev->parent = NULL;
fill_sbus_device(sbus_devs, this_dev);
/* Should we traverse for children? */
if(prom_getchild(sbus_devs)) {
/* Allocate device node */
this_dev->child = kmalloc(sizeof(struct sbus_dev),
GFP_ATOMIC);
/* Fill it */
this_dev->child->bus = sbus;
this_dev->child->next = 0;
fill_sbus_device(prom_getchild(sbus_devs),
this_dev->child);
sbus_do_child_siblings(prom_getchild(sbus_devs),
this_dev->child,
this_dev,
sbus);
} else {
this_dev->child = NULL;
}
while((sbus_devs = prom_getsibling(sbus_devs)) != 0) {
/* Allocate device node */
this_dev->next = kmalloc(sizeof(struct sbus_dev),
GFP_ATOMIC);
this_dev = this_dev->next;
this_dev->next = NULL;
/* Fill it */
this_dev->bus = sbus;
this_dev->parent = NULL;
fill_sbus_device(sbus_devs, this_dev);
/* Is there a child node hanging off of us? */
if(prom_getchild(sbus_devs)) {
/* Get new device struct */
this_dev->child = kmalloc(sizeof(struct sbus_dev),
GFP_ATOMIC);
/* Fill it */
this_dev->child->bus = sbus;
this_dev->child->next = 0;
fill_sbus_device(prom_getchild(sbus_devs),
this_dev->child);
sbus_do_child_siblings(prom_getchild(sbus_devs),
this_dev->child,
this_dev,
sbus);
} else {
this_dev->child = NULL;
}
}
/* Walk all devices and apply parent ranges. */
sbus_fixup_all_regs(sbus->devices);
dvma_init(sbus);
next_bus:
num_sbus++;
if(sparc_cpu_model == sun4u) {
this_sbus = prom_getsibling(this_sbus);
if(!this_sbus)
break;
this_sbus = prom_searchsiblings(this_sbus, "sbus");
} else if(sparc_cpu_model == sun4d) {
iommund = prom_getsibling(iommund);
if(!iommund)
break;
iommund = prom_searchsiblings(iommund, "io-unit");
if(!iommund)
break;
this_sbus = prom_searchsiblings(prom_getchild(iommund), "sbi");
} else {
this_sbus = prom_getsibling(this_sbus);
if(!this_sbus)
break;
this_sbus = prom_searchsiblings(this_sbus, "sbus");
}
if(this_sbus) {
sbus->next = kmalloc(sizeof(struct sbus_bus), GFP_ATOMIC);
sbus = sbus->next;
sbus->next = NULL;
sbus->prom_node = this_sbus;
} else {
break;
}
} /* while(this_sbus) */
if (sparc_cpu_model == sun4d) {
extern void sun4d_init_sbi_irq(void);
sun4d_init_sbi_irq();
}
rs_init();
#ifdef __sparc_v9__
if (sparc_cpu_model == sun4u) {
firetruck_init();
}
#endif
#ifdef CONFIG_SUN_AUXIO
if (sparc_cpu_model == sun4u)
auxio_probe ();
#endif
#ifdef __sparc_v9__
if (sparc_cpu_model == sun4u) {
extern void clock_probe(void);
clock_probe();
}
#endif
}
void tty_init(void)
{
rs_init();
con_init();
}
int main(int argc, char **argv) {
ROUTE err, saveroute;
int i;
TABLE tab;
char *str;
route_init(NULL, 0);
route_register(&rt_filea_method);
route_register(&rt_fileov_method);
route_register(&rt_stdin_method);
route_register(&rt_stdout_method);
route_register(&rt_stderr_method);
route_register(&rt_rs_method);
rs_init();
if ( ! elog_init(1, argv[0], NULL))
elog_die(FATAL, "Didn't initialise elog\n");
err = route_open("stderr", NULL, NULL, 0);
/* first lot of messages sent to the default places */
elog_send(INFO, "This is an eventlog test");
elog_send(INFO, NULL);
elog_send(INFO, "");
elog_send(INFO, "Event!!");
elog_send(DEBUG, "Event!!");
elog_send(WARNING, "Event!!");
elog_send(ERROR, "Event!!");
elog_send(INFO, "Event!!");
/* change origin */
elog_setorigin("etest");
elog_send(INFO, "test of set origin");
/* set one new purl route */
elog_setsevpurl(DEBUG, FILE1);
elog_send(INFO, "on screen");
elog_send(DEBUG, "in file");
elog_send(WARNING, "on screen");
/* set second identical purl route to reuse the previous one */
elog_setsevpurl(ERROR, FILE1);
if (elog_opendest[DEBUG].route !=
elog_opendest[ERROR].route)
route_die(err, "[13] didnt reuse already open DEBUG route\n");
elog_send(ERROR, "in file");
/* set identical below purl route */
if ( !elog_setbelowpurl(INFO, FILE1))
route_die(err, "[14] unable to setbelowpurl() file\n");
if (elog_opendest[DEBUG].route !=
elog_opendest[ERROR].route ||
elog_opendest[INFO].route !=
elog_opendest[ERROR].route)
route_die(err, "[14] didnt reuse already open ERROR route\n");
elog_send(DEBUG, "in file");
elog_send(INFO, "in file");
elog_send(WARNING, "on screen");
elog_send(ERROR, "in file");
elog_send(FATAL, "on screen");
/* set identical above purl route */
if ( !elog_setabovepurl(ERROR, FILE1))
route_die(err, "[19] unable to setabovepurl() file\n");
if (elog_opendest[ERROR].route !=
elog_opendest[INFO].route ||
elog_opendest[FATAL].route !=
elog_opendest[INFO].route)
route_die(err, "[19] didnt reuse already open INFO route\n");
elog_send(DEBUG, "in file");
elog_send(INFO, "in file");
elog_send(WARNING, "on screen");
elog_send(ERROR, "in file");
elog_send(FATAL, "in file");
/* set identical all purl route */
saveroute = elog_opendest[DEBUG].route;
if ( !elog_setallpurl(FILE1))
route_die(err, "[24] unable to setallpurl() file\n");
for (i=0; i < ELOG_NSEVERITIES; i++)
if (elog_opendest[i].route != saveroute)
route_die(err, "[24] didnt reuse already open %s route\n",
elog_sevstring[i]);
elog_send(DEBUG, "in file");
elog_send(INFO, "in file");
elog_send(WARNING, "in file");
elog_send(ERROR, "in file");
elog_send(FATAL, "in file");
/* set one different purl - timestore that we currently have to set up
* ourselves */
saveroute = route_open(RS1, "event log test", NULL, 10);
if ( ! saveroute)
route_die(err, "[29] unable to create/open timestore\n");
route_close(saveroute);
if ( ! elog_setsevpurl(INFO, RS1))
route_die(err, "[29] unable to setsevpurl() timestore\n");
if (elog_opendest[INFO].route ==
elog_opendest[WARNING].route)
route_die(err, "[29] different route same as WARNING\n");
elog_send(DEBUG, "in file");
elog_send(INFO, "in timestore");
elog_send(WARNING, "in file");
elog_send(ERROR, "in file");
elog_send(FATAL, "in file");
/* set one different route */
#if 0
elog_send(INFO, "");
elog_send(INFO, "Event!!");
elog_send(DEBUG, "Event!!");
elog_send(WARNING, "Event!!");
elog_send(ERROR, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
elog_send(INFO, "Event!!");
#endif
/* change format */
elog_setsevroute(WARNING, err);
elog_setformat(WARNING, "%s %s");
elog_send(WARNING, "still works??");
/* safe logging */
elog_safeprintf(INFO, "This is an eventlog test 35");
elog_safeprintf(INFO, NULL);
elog_safeprintf(INFO, "");
elog_safeprintf(INFO, "Event!! 38");
elog_safeprintf(DEBUG, "Event!! 39");
elog_safeprintf(WARNING, "Event!! 40");
elog_safeprintf(ERROR, "Event!! 41");
elog_safeprintf(INFO, "Event!!");
/* print the status out */
tab = elog_getstatus();
str = table_printcols_a(tab, elog_colnames);
printf("%s\n", str);
nfree(str);
table_destroy(tab);
rs_fini();
elog_fini();
route_close(err);
route_fini();
exit(0);
}
void sys_init()
{
char *p,buf[LN_buf+1];
char t;
time_t e,s;
ch.frug.called=0;
ch.frug.monitor=1;
ch.frug.esc=0;
ch.fp=NULL;
sys.rs=FALSE;
/* ch.env=NULL;*/
/* debug_start();*/
ed_init();
sion_init();
nowvar=varinit(LN_str,MAX_var);
sys.var=varinit(LN_str,MAX_var);
sys.result=varinit(LN_result,MAX_var);
strcpy(sys.home,".");
op_system();
if (sion_sr("config","system")==0)
{
exit(1);
}
sion_read(sys.var);
sion_sr("config","result");
sion_read(sys.result);
sion_close();
sys.execute=atoi(getvar("execute",sys.var))+1;
sys.logon =atoi(getvar("logon",sys.var));
sys.post =atoi(getvar("post",sys.var));
sys.t_logon=atoi(getvar("tdlogon",sys.var));
sys.t_post =atoi(getvar("tdpost",sys.var));
strjncpy(sys.netname,getvar("netname",sys.var),LN_title);
strjncpy(sys.netid,getvar("netid",sys.var),LN_netid);
strjncpy(sys.home ,getvar("home",sys.var),LN_dirs);
strjncpy(sys.temp ,getvar("temp",sys.var),LN_dirs);
pool.max=atoi(getvar("poolmax",sys.var));
copyright();
p=getvar("opentime",sys.var);
tzset(); /* 時空関数がまだ現れていないので呼ぶ必要あり */
if (p==NULL)
{
sys.opentime=DT_daytime;
sys.starttime=0;
}else
{
p=pull(buf,p);
t=atoi(buf);
p=pull(buf,p);
s=(t*60+atoi(buf))*60;
p=pull(buf,p);
t=atoi(buf);
p=pull(buf,p);
e=(t*60+atoi(buf))*60;
/*dbprintf("s:%ld,e:%ld",s,e);*/
sys.opentime=e+(e<s?DT_daytime:0)-s;
sys.starttime=DT_daytime-s;
/*dbprintf("o:%ld,s:%ld",sys.opentime,sys.starttime);*/
}
stack_init();
where_init();
user_init();
art_init();
pool_init();
sys.rs=rs_init();
/* sys_log("●システム起動");*/
}
/*
* Initialise standard routes, logging, configuration, interpret
* command line and change directory.
*
* A standard set of rules is followed to setup the correct
* configuration from central places local files and the command line.
* All the information is then available in the global CF_VALS: iiab_cf.
* The command line config is available in iiab_cmdarg on its own and also
* placed in iiab_cf.
* There are some standard options implemented by this routine:-
* -c Configuration file (arg expected)
* -C Configuration option (arg expected)
* -d Diagnostic debug mode
* -D Developer debug mode
* -e 'off the shelf' error format (int arg expected 1-7)
* -h Help
* -v Version print and exit
* Note:-
* 1. If the symbol defined by NM_CFNAME (nmalloc) is not defined or
* set to 0, nmalloc leak checks are disabled
*/
void iiab_start(char *opts, /* Command line option string as getopts(3) */
int argc, /* Number of arguments in command line */
char **argv, /* Array of argument strings */
char *usage, /* String describing usage */
char *appcf /* Application configuration string */ )
{
int r;
char *appcf_expanded;
int elogfmt;
if ( !(opts && usage) )
elog_die(FATAL, "opts or usage not set");
/* save our launch directory & work out the standard places */
iiab_dir_locations(argv[0]);
/* consolidated command line options and usage */
/* -- setup global variables -- */
iiab_cmdopts = util_strjoin(IIAB_DEFOPTS, opts, NULL);
iiab_cmdusage = util_strjoin(IIAB_DEFUSAGE, usage, usage?"\n":"",
IIAB_DEFWHERE, NULL);
iiab_cmdarg = cf_create();
iiab_cf = cf_create();
/* initialise common IIAB classes: callbacks, error logging,
* i/o and old storage routes */
callback_init();
iiab_init_routes(); /* initialise new route mechanism */
elog_init(1, argv[0], NULL);
rs_init();
/* collect command line arguments and place in special config list
* which is use for generating the main config list
*/
r = cf_cmd(iiab_cmdarg, iiab_cmdopts, argc, argv, iiab_cmdusage);
if ( ! r ) {
nm_deactivate();
elog_send(FATAL, "incorrect command line, can't continue further");
exit(1);
}
/* help! print out help before we do anything else and send it
* to stderr as the user needs to read it */
if (cf_defined(iiab_cmdarg, "h")) {
nm_deactivate();
fprintf(stderr, "usage %s %s",
cf_getstr(iiab_cmdarg,"argv0"), iiab_cmdusage);
exit(1);
}
/* load app's dir locations into config, expand the application config
* string (which needs the dir locations in iiab_cf), then do the
* fullconfig load */
iiab_dir_setcf(iiab_cf);
appcf_expanded = nmalloc(strlen(appcf) * 2);
route_expand(appcf_expanded, appcf, NULL, 0);
iiab_cf_load(iiab_cf, iiab_cmdarg, iiab_cmdusage, appcf_expanded);
nfree(appcf_expanded);
/***********************************************
* (d) Carry out common configuration actions
* 1. configure event logging
* 2. memory checking
* 3. version switch
* 4. adapt logging for -d and -D special debug cases
* 5. adapt logging format for -e
* 6. final initialisation for subsystems needing configuration
* 7. license checking: go/no-go
***********************************************/
/* configure event logging */
elog_configure(iiab_cf);
/*
* nmalloc deactivation check.
* Deactivate nmalloc checking if NM_CFNAME is set to 0 in config
*/
if ( !cf_defined(iiab_cf, NM_CFNAME) ||
cf_getint(iiab_cf, NM_CFNAME) == 0 )
nm_deactivate();
/* command line switches (excluding -c and -C) */
/* -v flag: version print and exit */
if (cf_defined(iiab_cmdarg, "v")) {
fprintf(stderr, "Version of %s is %s\n",
cf_getstr(iiab_cmdarg,"argv0"), VERSION);
exit(0);
}
/* if -d diag flag specified, override supplied event
* configuration and instead route DIAG and above to stderr */
if (cf_defined(iiab_cmdarg, "d")) {
elog_setallpurl("none:");
elog_setabovepurl(DIAG, "stderr:");
elog_printf(DIAG, "event configuration overidden: diagnosis "
"to stderr");
}
/* -D debug flag: as above but route everything to stderr */
if (cf_defined(iiab_cmdarg, "D")) {
elog_setabovepurl(DEBUG, "stderr:");
elog_printf(DEBUG, "event configuration overidden: debug "
"to stderr");
}
/* -e flag: use precanned elog formats */
if (cf_defined(iiab_cmdarg, "e")) {
elogfmt = cf_getint(iiab_cmdarg, "e");
if (elogfmt < 0 || elogfmt > ELOG_MAXFMT)
elog_printf(ERROR, "standard error format out of range "
"(0-%d), using default", ELOG_MAXFMT);
else
elog_setallformat(elog_stdfmt[elogfmt]);
}
/* diagnostics: config and dirs */
cf_dump(iiab_cf); /* dump config to diag */
iiab_dir_dump(); /* dump dir locations to diag */
/* final set of initialisations that require configurations */
http_init();
#if 0
/* self destruction due to snapshot time out, licenses etc
* disabled, but kept here just in case we want to do it again */
if ( ! cf_defined(iiab_cf, IIAB_LICNAME) )
if (time(NULL) > IIAB_EXPIRE)
elog_printf(FATAL, "This is development software "
"and almost certainly out of date.\n"
"You should arrange an update from system garden "
"by e-mailing [email protected]\n"
"or checking the web site "
"http://www.systemgarden.com.\n"
"This software will run unaffected in its current "
"form but you will be nagged from now on");
#endif
#if 0
/* return to the launch dir */
chdir(iiab_dir_launch);
#endif
}
void microb_cs_init(void)
{
/* ROBOT_SYSTEM */
rs_init(&mainboard.rs);
rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist, increase left and it will turn more left */
rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value,
LEFT_ENCODER, IMP_COEF * -1.0000);
rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value,
RIGHT_ENCODER, IMP_COEF * 1.0000);
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
/* POSITION MANAGER */
position_init(&mainboard.pos);
position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
//position_set_centrifugal_coef(&mainboard.pos, 0.000016);
position_use_ext(&mainboard.pos);
/* TRAJECTORY MANAGER */
trajectory_init(&mainboard.traj);
trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
&mainboard.angle.cs);
trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */
/* distance window, angle window, angle start */
trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);
/* ---- CS angle */
/* PID */
pid_init(&mainboard.angle.pid);
pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
pid_set_out_shift(&mainboard.angle.pid, 10);
pid_set_derivate_filter(&mainboard.angle.pid, 4);
/* QUADRAMP */
quadramp_init(&mainboard.angle.qr);
quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */
/* CS */
cs_init(&mainboard.angle.cs);
cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
cs_set_consign(&mainboard.angle.cs, 0);
/* Blocking detection */
bd_init(&mainboard.angle.bd);
bd_set_speed_threshold(&mainboard.angle.bd, 80);
bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);
/* ---- CS distance */
/* PID */
pid_init(&mainboard.distance.pid);
pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
pid_set_out_shift(&mainboard.distance.pid, 10);
pid_set_derivate_filter(&mainboard.distance.pid, 6);
/* QUADRAMP */
quadramp_init(&mainboard.distance.qr);
quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */
/* CS */
cs_init(&mainboard.distance.cs);
cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
cs_set_consign(&mainboard.distance.cs, 0);
/* Blocking detection */
bd_init(&mainboard.distance.bd);
bd_set_speed_threshold(&mainboard.distance.bd, 60);
bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
/* ---- CS fessor */
fessor_autopos();
/* PID */
pid_init(&mainboard.fessor.pid);
pid_set_gains(&mainboard.fessor.pid, 300, 10, 150);
pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095);
pid_set_out_shift(&mainboard.fessor.pid, 10);
pid_set_derivate_filter(&mainboard.fessor.pid, 4);
/* CS */
cs_init(&mainboard.fessor.cs);
cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid);
cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL);
cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC);
fessor_up();
/* ---- CS elevator */
elevator_autopos();
/* PID */
pid_init(&mainboard.elevator.pid);
pid_set_gains(&mainboard.elevator.pid, 300, 10, 150);
pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095);
pid_set_out_shift(&mainboard.elevator.pid, 10);
pid_set_derivate_filter(&mainboard.elevator.pid, 4);
/* CS */
cs_init(&mainboard.elevator.cs);
cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid);
cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL);
cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC);
elevator_down();
/* ---- CS wheel */
/* PID */
pid_init(&mainboard.wheel.pid);
pid_set_gains(&mainboard.wheel.pid, 100, 100, 0);
pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095);
pid_set_out_shift(&mainboard.wheel.pid, 5);
pid_set_derivate_filter(&mainboard.wheel.pid, 4);
/* CS */
cs_init(&mainboard.wheel.cs);
cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid);
cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL);
cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL);
cs_set_consign(&mainboard.wheel.cs, 1000);
/* set them on !! */
mainboard.angle.on = 0;
mainboard.distance.on = 0;
mainboard.fessor.on = 1;
mainboard.elevator.on = 0;
mainboard.wheel.on = 1;
mainboard.flags |= DO_CS;
scheduler_add_periodical_event_priority(do_cs, NULL,
5000L / SCHEDULER_UNIT,
CS_PRIO);
}
int main(void){
sr_log("serial startup...\n");
rs_init();
workloop();
return 0;
}
static int filter_get_image( mlt_frame frame, uint8_t **image, mlt_image_format *format, int *width, int *height, int writable )
{
mlt_filter filter = mlt_frame_pop_service( frame );
*format = mlt_image_rgb24;
mlt_properties_set_int( MLT_FRAME_PROPERTIES(frame), "consumer_deinterlace", 1 );
int error = mlt_frame_get_image( frame, image, format, width, height, 1 );
if ( !error && *image )
{
videostab self = filter->child;
mlt_position length = mlt_filter_get_length2( filter, frame );
int h = *height;
int w = *width;
// Service locks are for concurrency control
mlt_service_lock( MLT_FILTER_SERVICE( filter ) );
if ( !self->initialized )
{
// Initialize our context
self->initialized = 1;
self->es = es_init( w, h );
self->pos_i = (vc*) malloc( length * sizeof(vc) );
self->pos_h = (vc*) malloc( length * sizeof(vc) );
self->pos_y = (vc*) malloc( h * sizeof(vc) );
self->rs = rs_init( w, h );
}
char *vectors = mlt_properties_get( MLT_FILTER_PROPERTIES(filter), "vectors" );
if ( !vectors )
{
// Analyse
int pos = (int) mlt_filter_get_position( filter, frame );
self->pos_i[pos] = vc_add( pos == 0 ? vc_zero() : self->pos_i[pos - 1], es_estimate( self->es, *image ) );
// On last frame
if ( pos == length - 1 )
{
mlt_profile profile = mlt_service_profile( MLT_FILTER_SERVICE(filter) );
double fps = mlt_profile_fps( profile );
// Filter and store the results
hipass( self->pos_i, self->pos_h, length, fps );
serialize_vectors( self, length );
}
} else {
// Apply
if ( self->initialized != 2 )
{
// Load analysis results from property
self->initialized = 2;
deserialize_vectors( self, vectors, length );
}
if ( self->initialized == 2 )
{
// Stabilize
float shutter_angle = mlt_properties_get_double( MLT_FRAME_PROPERTIES(frame) , "shutterangle" );
float pos = mlt_filter_get_position( filter, frame );
int i;
for (i = 0; i < h; i ++)
self->pos_y[i] = interp( self->lanc_kernels,self->pos_h, length, pos + (i - h / 2.0) * shutter_angle / (h * 360.0) );
rs_resample( self->lanc_kernels,self->rs, *image, self->pos_y );
}
}
mlt_service_unlock( MLT_FILTER_SERVICE( filter ) );
}
return error;
}
// TTY终端初始化函数
// 初始化串口终端和控制台终端
void tty_init(void)
{
// 初始化串行中断程序和串行接口1和2（serial.c）
rs_init();
con_init(); // 初始化控制台终端(console.c文件中)
}
int main(int argc, char *argv[]) {
int opt_shutter_angle = 0;
int opt_mjpeg_quality = 100;
int nf, i, nc, nr;
int tfs, fps;
vc *pos_i, *pos_h, *pos_y;
es_ctx *es;
rs_ctx *rs;
opterr = 0;
while ((i = getopt(argc, argv, "r:q:")) != -1) {
switch (i) {
case 'r':
opt_shutter_angle = atoi(optarg);
break;
case 'q':
opt_mjpeg_quality = atoi(optarg);
break;
default:
print_help(argv);
}
}
if (argc < optind + 2)
print_help(argv);
if (AVI_open_movie(argv[optind], &mv_in) != AVI_ERROR_NONE) {
printf("error: can't read from %s\n", argv[optind]);
return EXIT_FAILURE;
}
if (mv_in.header->Streams < 1 || mv_in.streams[0].sh.Type != AVIST_VIDEO) {
printf("error: video stream not found on %s\n", argv[optind]);
return EXIT_FAILURE;
}
if (AVI_open_compress(argv[optind + 1], &mv_out, 1, AVI_FORMAT_MJPEG) != AVI_ERROR_NONE) {
printf("error: can't write to %s\n", argv[optind + 1]);
return EXIT_FAILURE;
}
printf("status: setup\n");
prepare_lanc_kernels();
nc = mv_in.header->Width;
nr = mv_in.header->Height;
tfs = mv_in.header->TotalFrames;
fps = 1000000 / mv_in.header->MicroSecPerFrame;
pos_i = (vc *)malloc(tfs * sizeof(vc));
pos_h = (vc *)malloc(tfs * sizeof(vc));
pos_y = (vc *)malloc(nr * sizeof(vc));
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_WIDTH, &nc);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_HEIGHT, &nr);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_FRAMERATE, &fps);
AVI_set_compress_option(&mv_out, AVI_OPTION_TYPE_MAIN, 0, AVI_OPTION_QUALITY, &opt_mjpeg_quality);
es = es_init(nc, nr);
rs = rs_init(nc, nr);
printf("status: estimating\n");
for (nf = 0; nf < tfs; nf ++) {
unsigned char *fr = (unsigned char *)AVI_read_frame(&mv_in, AVI_FORMAT_RGB24, nf, 0);
pos_i[nf] = vc_add(
nf > 0 ? pos_i[nf - 1] : vc_set(0.0, 0.0),
es_estimate(es, fr)
);
free(fr);
if ((nf + 1) % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\nstatus: filtering\n");
hipass(pos_i, pos_h, tfs, fps / 2);
printf("status: resampling\n");
for (nf = 0; nf < tfs; nf ++) {
unsigned char *fr = (unsigned char *)AVI_read_frame(&mv_in, AVI_FORMAT_RGB24, nf, 0);
for (i = 0; i < nr; i ++) {
pos_y[i] = interp(
pos_h, tfs,
nf + (i - nr / 2.0) * opt_shutter_angle / (nr * 360.0)
);
}
rs_resample(rs, fr, pos_y);
AVI_write_frame(&mv_out, nf, AVI_FORMAT_RGB24, fr, nc * nr * 3 * sizeof(unsigned char));
if ((nf + 1) % 10 == 0) {
printf(".");
fflush(stdout);
}
}
printf("\nstatus: closing\n");
es_free(es);
rs_free(rs);
free_lanc_kernels();
AVI_close(&mv_in);
AVI_close_compress(&mv_out);
return EXIT_SUCCESS;
}
