void
sys_app_init(void)
{
#if defined(PGAS)
pgas_app_init();
#else /* PGAS */
tm2c_shmalloc_init(TM2C_SHMEM_SIZE);
#endif /* PGAS */
#if !defined(NOCM) && !defined(BACKOFF_RETRY) /* if real cm: wholly, greedy, faircm */
cm_abort_flag_mine = cm_init(NODE_ID());
*cm_abort_flag_mine = NO_CONFLICT;
# if defined(GREEDY) && defined(GREEDY_GLOBAL_TS)
greedy_global_ts = cm_greedy_global_ts_init();
# endif
#endif
BARRIERW;
tm2c_rpc_remote_msg = NULL;
PRINTD("sys_app_init: done");
BARRIERW;
}
void
sys_dsl_init(void)
{
#if defined(PGAS)
pgas_dsl_init();
#else /* PGAS */
tm2c_shmalloc_init(TM2C_SHMEM_SIZE);
#endif /* PGAS */
BARRIERW;
#if !defined(NOCM) && !defined(BACKOFF_RETRY) /* if real cm: wholly, greedy, faircm */
cm_abort_flags = (int32_t**) malloc(TOTAL_NODES() * sizeof(int32_t*));
assert(cm_abort_flags != NULL);
uint32_t i;
for (i = 0; i < TOTAL_NODES(); i++)
{
//TODO: make it open only for app nodes
if (is_app_core(i))
{
cm_abort_flags[i] = cm_init(i);
}
}
#endif
BARRIERW;
}
int main(int argc, char *argv[])
{
init();
if(init_setting() != 0){
print_errmsg("ERROR: SETTING init fail. \n");
return 1;
}
if(gc_init() != 0){
print_errmsg("ERROR: G_CODE init fail. \n");
return 1;
}
if (cm_init() == false)
{
print_errmsg("ERROR: USB-DEV init fail. \n");
return 1;
}
if (cmd_init() == false)
{
print_errmsg("ERROR: G code init fail.\n");
cm_close();
return 1;
}
if (tp_init() == false)
{
print_errmsg("ERROR: Temperature library init fail. \n");
cmd_close();
cm_close();
return 1;
}
plan_init();
Config_ResetDefault();
st_init();
while(1) {
LCD(10UL);
getCommand(10UL);
process_commands(50UL);
stepper(1000L);
manageHeater(10UL);
}
st_close();
plan_close();
tp_close();
cmd_close();
cm_close();
//debug
//sfp_close();
return 0;
}
void tg_application_reset(void)
{
cli(); // disable global interrupts
st_reset(); // reset stepper subsystem
mp_init(); // motion planning subsystem
cm_init(); // canonical machine
gc_init(); // gcode-parser
PMIC_SetVectorLocationToApplication(); // as opposed to boot ROM
PMIC_EnableHighLevel(); // all levels are used, so don't bother to abstract them
PMIC_EnableMediumLevel();
PMIC_EnableLowLevel();
sei(); // enable global interrupts
tg_ready(); // (LAST) announce app is online
}
int __init arch_board_early_init(void)
{
int rc;
/* Host virtual memory, device tree, heap is up.
* Do necessary early stuff like iomapping devices
* memory or boot time memory reservation here.
*/
/* The function omap3_beagle_init_early() of
* <linux>/arch/arm/mach-omap2/board-omap3beagle.c
* does the following:
* 1. Initialize Clock & Power Domains using function
* omap2_init_common_infrastructure() of
* <linux>/arch/arm/mach-omap2/io.c
* 2. Initialize & Reprogram Clock of SDRC using function
* omap2_sdrc_init() of <linux>/arch/arm/mach-omap2/sdrc.c
*/
/* Initialize Clock Mamagment */
if ((rc = cm_init())) {
return rc;
}
/* Initialize Power & Reset Mamagment */
if ((rc = prm_init())) {
return rc;
}
/* Enable I-clock for S32K timer
* Note: S32K is our reference clocksource and also used
* as clock reference for GPTs
*/
cm_setbits(OMAP3_WKUP_CM,
OMAP3_CM_ICLKEN_WKUP,
OMAP3_CM_ICLKEN_WKUP_EN_32KSYNC_M);
/* Initialize SDRAM Controller (SDRC) */
if ((rc = sdrc_init(OMAP3_SDRC_BASE,
OMAP3_SMS_BASE,
mt46h32m32lf6_sdrc_params,
mt46h32m32lf6_sdrc_params))) {
return rc;
}
return 0;
}
int main(void)
{
// There are a lot of dependencies in the order of these inits.
// Don't change the ordering unless you understand this.
// Inits can assume that all memory has been zeroed by either
// a hardware reset or a watchdog timer reset.
cli();
// system and drivers
sys_init(); // system hardware setup - must be first
rtc_init(); // real time counter
xio_init(); // xmega io subsystem
sig_init(); // signal flags
st_init(); // stepper subsystem - must precede gpio_init()
gpio_init(); // switches and parallel IO
pwm_init(); // pulse width modulation drivers - must follow gpio_init()
// application structures
tg_init(STD_INPUT); // tinyg controller (controller.c) - must be first app init; reqs xio_init()
cfg_init(); // config records from eeprom - must be next app init
mp_init(); // motion planning subsystem
cm_init(); // canonical machine - must follow cfg_init()
sp_init(); // spindle PWM and variables
// now bring up the interupts and get started
PMIC_SetVectorLocationToApplication(); // as opposed to boot ROM
PMIC_EnableHighLevel(); // all levels are used, so don't bother to abstract them
PMIC_EnableMediumLevel();
PMIC_EnableLowLevel();
sei(); // enable global interrupts
rpt_print_system_ready_message();// (LAST) announce system is ready
_unit_tests(); // run any unit tests that are enabled
tg_canned_startup(); // run any pre-loaded commands
while (true) {
// if (tg.network == NET_MASTER) {
// tg_repeater();
// } else if (tg.network == NET_SLAVE) {
// tg_receiver();
// } else {
tg_controller(); // NET_STANDALONE
// }
}
}
static int
cm_setup(void **state)
{
createDataTreeExampleModule();
cm_ctx_t *ctx = NULL;
int rc = 0;
sr_logger_init("cm_test");
sr_log_stderr(SR_LL_ERR); /* log only errors to stderr */
rc = cm_init(CM_MODE_LOCAL, CM_AF_SOCKET_PATH, &ctx);
assert_int_equal(rc, SR_ERR_OK);
assert_non_null(ctx);
*state = ctx;
rc = cm_start(ctx);
assert_int_equal(rc, SR_ERR_OK);
return 0;
}
/*******************************************************************************
* Simple function to initialise all BL31 helper libraries.
******************************************************************************/
void bl31_lib_init(void)
{
cm_init();
}
static void __init intcp_init_irq_of(void)
{
cm_init();
irqchip_init();
}
static void __init intcp_init_irq_of(void)
{
cm_init();
of_irq_init(fpga_irq_of_match);
}
/* Tar file extraction
* gzFile in, handle of input tarball opened with gzopen
* int cm, compressionMethod
* int junkPaths, nonzero indicates to ignore stored path (don't create directories)
* enum KeepMode keep, indicates to perform if file exists
* int iCnt, char *iList[], argv style list of files to extract, {0,NULL} for all
* int xCnt, char *xList[], argv style list of files NOT to extract, {0,NULL} for none
* int failOnHardLinks, if nonzero then will treat failure to create a hard link same as
* failure to create a regular file, 0 prints a warning if fails - note that hardlinks
* will always fail on Windows prior to NT 5 (Win 2000) or later and non NTFS file systems.
*
* returns 0 (or positive value) on success
* returns negative value on error, where
* -1 means error reading from tarball
* -2 means error extracting file from tarball
* -3 means error creating hard link
*/
int tgz_extract(gzFile in, int cm, int junkPaths, enum KeepMode keep, int iCnt, TCHAR *iList[], int xCnt, TCHAR *xList[], int failOnHardLinks)
{
int getheader = 1; /* assume initial input has a tar header */
HANDLE outfile = INVALID_HANDLE_VALUE;
union tar_buffer buffer;
unsigned long remaining;
TCHAR fname[BLOCKSIZE]; /* must be >= BLOCKSIZE bytes */
time_t tartime;
/* do any prep work for extracting from compressed TAR file */
if (cm_init(in, cm))
{
PrintMessage(_T("tgz_extract: unable to initialize decompression method."));
cm_cleanup(cm);
return -1;
}
while (1)
{
if (readBlock(cm, &buffer) < 0) return -1;
/*
* If we have to get a tar header
*/
if (getheader >= 1)
{
/*
* if we met the end of the tar
* or the end-of-tar block,
* we are done
*/
if (/* (len == 0) || */ (buffer.header.name[0]== 0)) break;
/* compute and check header checksum, support signed or unsigned */
if (!valid_checksum(&(buffer.header)))
{
PrintMessage(_T("tgz_extract: bad header checksum"));
cm_cleanup(cm);
return -1;
}
/* store time, so we can set the timestamp on files */
tartime = (time_t)getoct(buffer.header.mtime,12);
/* copy over filename chunk from header, avoiding overruns */
if (getheader == 1) /* use normal (short or posix long) filename from header */
{
/* NOTE: prepends any prefix, including separator, and ensures terminated */
memset(fname, 0, sizeof(fname));
getFullName(&buffer, fname);
}
else /* use (GNU) long filename that preceeded this header */
{
#if 0
/* if (strncmp(fname,buffer.header.name,SHORTNAMESIZE-1) != 0) */
char fs[SHORTNAMESIZE]; /* force strings to same max len, then compare */
lstrcpyn(fs, fname, SHORTNAMESIZE);
fs[SHORTNAMESIZE-1] = '\0';
buffer.header.name[SHORTNAMESIZE-1] = '\0';
if (lstrcmp(fs, buffer.header.name) != 0)
{
PrintMessage(_T("tgz_extract: mismatched long filename"));
cm_cleanup(cm);
return -1;
}
#else
PrintMessage(_T("tgz_extract: using GNU long filename [%s]"), fname);
#endif
}
/* LogMessage("buffer.header.name is:"); LogMessage(fname); */
switch (buffer.header.typeflag)
{
case DIRTYPE:
dirEntry:
if (!junkPaths)
{
safetyStrip(fname);
makedir(fname);
}
break;
case LNKTYPE: /* hard link */
case CONTTYPE: /* contiguous file, for compatibility treat as normal */
case REGTYPE:
case AREGTYPE:
/* Note: a file ending with a / may actually be a BSD tar directory entry */
if (fname[strlen(fname)-1] == '/')
goto dirEntry;
remaining = getoct(buffer.header.size,12);
if ( /* add (remaining > 0) && to ignore 0 zero byte files */
( (iList == NULL) || (matchname(fname, iCnt, iList, junkPaths)) ) &&
(!matchname(fname, xCnt, xList, junkPaths))
)
{
if (!junkPaths) /* if we want to use paths as stored */
{
/* try creating directory */
TCHAR *p = tstrrchr(fname, '/');
if (p != NULL)
{
*p = '\0';
makedir(fname);
*p = '/';
}
}
else
{
/* try ignoring directory */
TCHAR *p = tstrrchr(fname, '/');
if (p != NULL)
{
/* be sure terminating '\0' is copied and */
/* use ansi memcpy equivalent that handles overlapping regions */
MoveMemory(fname, p+1, (strlen(p+1) + 1) * sizeof(TCHAR) );
}
}
if (*fname) /* if after stripping path a fname still exists */
{
/* Attempt to open the output file and report action taken to user */
const TCHAR szERRMsg[] = _T("Error: Could not create file "),
szSUCMsg[] = _T("Writing "),
szSKPMsg[] = _T("Skipping ");
const TCHAR * szMsg = szSUCMsg;
safetyStrip(fname);
if (buffer.header.typeflag == LNKTYPE)
{
outfile = INVALID_HANDLE_VALUE;
/* create a hardlink if possible, else produce just a warning unless failOnHardLinks is true */
if (!MakeHardLink(fname, buffer.header.linkname))
{
PrintMessage(_T("Warning: unable to create hard link %s [%d]"), fname, GetLastError());
if (failOnHardLinks)
{
cm_cleanup(cm);
return -3;
}
}
else
{
outfile = CreateFile(fname,GENERIC_WRITE,FILE_SHARE_READ,NULL,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,NULL);
goto setTimeAndCloseFile;
}
} else
{
/* Open the file for writing mode, creating if doesn't exist and truncating if exists and overwrite mode */
outfile = CreateFile(fname,GENERIC_WRITE,FILE_SHARE_READ,NULL,(keep==OVERWRITE)?CREATE_ALWAYS:CREATE_NEW,FILE_ATTRIBUTE_NORMAL,NULL);
/* failed to open file, either valid error (like open) or it already exists and in a keep mode */
if (outfile == INVALID_HANDLE_VALUE)
{
/* if skip existing or only update existing and failed to open becauses exists */
if ((keep!=OVERWRITE) && (GetLastError()==ERROR_FILE_EXISTS))
{
/* assume skipping initially (mode==SKIP or ==UPDATE with existing file newer) */
szMsg = szSKPMsg; /* and update output message accordingly */
/* if in update mode, check filetimes and reopen in overwrite mode */
if (keep == UPDATE)
{
FILETIME ftm_a;
HANDLE h;
WIN32_FIND_DATA ffData;
cnv_tar2win_time(tartime, &ftm_a); /* archive file time */
h = FindFirstFile(fname, &ffData); /* existing file time */
if (h!=INVALID_HANDLE_VALUE)
FindClose(h); /* cleanup search handle */
else
goto ERR_OPENING;
/* compare date+times, is one in tarball newer? */
if (*((LONGLONG *)&ftm_a) > *((LONGLONG *)&(ffData.ftLastWriteTime)))
{
outfile = CreateFile(fname,GENERIC_WRITE,FILE_SHARE_READ,NULL,CREATE_ALWAYS,FILE_ATTRIBUTE_NORMAL,NULL);
if (outfile == INVALID_HANDLE_VALUE) goto ERR_OPENING;
szMsg = szSUCMsg;
}
}
}
else /* in overwrite mode or failed for some other error than exists */
{
ERR_OPENING:
PrintMessage(_T("%s%s [%d]"), szERRMsg, fname, GetLastError());
cm_cleanup(cm);
return -2;
}
}
/* Inform user of current extraction action (writing, skipping file XYZ) */
PrintMessage(_T("%s%s"), szMsg, fname);
}
}
}
else
outfile = INVALID_HANDLE_VALUE;
/*
* could have no contents, in which case we close the file and set the times
*/
if (remaining > 0)
getheader = 0;
else
{
setTimeAndCloseFile:
getheader = 1;
if (outfile != INVALID_HANDLE_VALUE)
{
FILETIME ftm;
cnv_tar2win_time(tartime, &ftm);
SetFileTime(outfile,&ftm,NULL,&ftm);
CloseHandle(outfile);
outfile = INVALID_HANDLE_VALUE;
}
}
break;
case GNUTYPE_LONGLINK:
case GNUTYPE_LONGNAME:
{
remaining = getoct(buffer.header.size,12);
if (readBlock(cm, fname) < 0) return -1;
fname[BLOCKSIZE-1] = '\0';
if ((remaining >= BLOCKSIZE) || ((unsigned)strlen(fname) > remaining))
{
PrintMessage(_T("tgz_extract: invalid long name"));
cm_cleanup(cm);
return -1;
}
getheader = 2;
break;
}
default:
/*
if (action == TGZ_LIST)
printf(" %s <---> %s\n",strtime(&tartime),fname);
*/
break;
}
}
else /* (getheader == 0) */
{
unsigned int bytes = (remaining > BLOCKSIZE) ? BLOCKSIZE : remaining;
unsigned long bwritten;
if (outfile != INVALID_HANDLE_VALUE)
{
WriteFile(outfile,buffer.buffer,bytes,&bwritten,NULL);
if (bwritten != bytes)
{
PrintMessage(_T("Error: write failed for %s"), fname);
CloseHandle(outfile);
DeleteFile(fname);
cm_cleanup(cm);
return -2;
}
}
remaining -= bytes;
if (remaining == 0) goto setTimeAndCloseFile;
}
} /* while(1) */
cm_cleanup(cm);
return 0;
}
void main_init(int argc, char *argv[])
{
bool override_hw = false;
if (argc > 1)
{
if (strcmp(argv[1], "calibrate") == 0)
calibrate = true;
else
override_hw = true;
}
/* init data structures: */
memset(&pos_in, 0, sizeof(pos_in_t));
vec3_init(&pos_in.acc);
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("pilot") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
die();
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("pilot.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
die();
}
syslog(LOG_CRIT, "logger opened");
LOG(LL_INFO, "initializing platform");
if (arcade_quad_init(&platform, override_hw) < 0)
{
LOG(LL_ERROR, "could not initialize platform");
die();
}
acc_mag_cal_init();
cmc_init();
const size_t array_len = sizeof(float) * platform.n_motors;
setpoints = malloc(array_len);
ASSERT_NOT_NULL(setpoints);
memset(setpoints, 0, array_len);
rpm_square = malloc(array_len);
ASSERT_NOT_NULL(rpm_square);
memset(rpm_square, 0, array_len);
LOG(LL_INFO, "initializing model/controller");
pos_init();
ne_speed_ctrl_init(REALTIME_PERIOD);
att_ctrl_init();
yaw_ctrl_init();
u_ctrl_init();
u_speed_init();
navi_init();
LOG(LL_INFO, "initializing command interface");
cmd_init();
motors_state_init();
blackbox_init();
/* init flight logic: */
flight_logic_init();
/* init calibration data: */
cal_init(&gyro_cal, 3, 1000);
cal_ahrs_init();
flight_state_init(50, 150, 4.0);
piid_init(REALTIME_PERIOD);
interval_init(&gyro_move_interval);
gps_data_init(&gps_data);
mag_decl_init();
cal_init(&rc_cal, 3, 500);
tsfloat_t acc_fg;
opcd_param_t params[] =
{
{"acc_fg", &acc_fg},
OPCD_PARAMS_END
};
opcd_params_apply("main.", params);
filter1_lp_init(&lp_filter, tsfloat_get(&acc_fg), 0.06, 3);
cm_init();
mon_init();
LOG(LL_INFO, "entering main loop");
}
void
pl_init(void)
{
cm_init();
}
