void WSramAddr(int addr) {
if (SramSymbol[addr]) {
Ws(SramSymbol[addr]);
Ws(" ($"); Wx(addr,1); Wc(')');
} else {
Wc('$'); Wx(addr,1);
}
}
int check_burning_wind(int** forest, int row_index, int col_index, int neighbourhood_type, int wind_speed, int wind_direction, long double pImmune,int rows, int cols)
{
int i=row_index;
int j=col_index;
int neighbour_status = -5;
switch(wind_speed)
{
case 0:
return TREE;
break;
case 2:
switch(wind_direction)
{
case SOUTH:
neighbour_status = forest[Nr(Nr(i))%rows][Nc(Nc(j))%cols];
break;
case NORTH:
neighbour_status = forest[Sr(Sr(i))%rows][Sc(Sc(j))%cols];
break;
case EAST:
neighbour_status = forest[Wr(Wr(i))%rows][Wc(Wc(j))%cols];
break;
case WEST:
neighbour_status = forest[Er(Er(i))%rows][Ec(Ec(j))%cols];
break;
}
if (pImmune<U && neighbour_status == BURNING)
return BURNING;
case 1:
neighbour_status = -5;
switch(wind_direction)
{
case SOUTH:
neighbour_status = forest[Nr(i)][Nc(j)];
break;
case NORTH:
neighbour_status = forest[Sr(i)][Sc(j)];
break;
case EAST:
neighbour_status = forest[Wr(i)][Wc(j)];
break;
case WEST:
neighbour_status = forest[Er(i)][Ec(j)];
break;
}
if (pImmune<U && neighbour_status == BURNING)
return BURNING;
else
return TREE;
}
}
void ConnectUsbtinyPort(struct UPort *up) {
Assert(up->port.kind == 'u');
// Ws(" -- ConnectUsbtinyPort entry. "); WriteUPort(up);
Ws("UsbTiny"); Wd(up->port.index,1); Wc(' ');
jmp_buf oldFailPoint;
memcpy(oldFailPoint, FailPoint, sizeof(FailPoint));
if (setjmp(FailPoint)) {
// Wsl(" -- Fail caught in ConnectUsbtinyPort.");
up->port.baud = -1;
} else {
up->port.baud = 0;
if (!up->handle) {up->handle = usb_open(up->device);}
if (!up->handle) {PortFail(up, "Couldn't open UsbTiny port.");}
DigisparkBreakAndSync(up);
}
memcpy(FailPoint, oldFailPoint, sizeof(FailPoint));
Ws("\r \r");
// Ws(" -- ConnectUsbtinyPort complete. "); WriteUPort(up);
}
int do_neighbours_burn(int** forest,int row_index,int col_index)
{
return (forest[Nr(row_index)][Nc(col_index)]==BURNING ||
forest[Er(row_index)][Ec(col_index)]==BURNING ||
forest[Wr(row_index)][Wc(col_index)]==BURNING ||
forest[Sr(row_index)][Sc(col_index)]==BURNING
);
}
/// \brief Constructor: Initializes filter
UnscentedKalmanFilter::UnscentedKalmanFilter(void):
x_hat(N,0.0),P_hat(N,N,0.0),K(N,M,0.0),Q(N,N,0.0),R(M,M,0.0),Wm(1,2*N+1,0.5/C),Wc(1,2*N+1,0.5/C)
{
// Set mean of initial position prior (orientation already 0)
x_hat[0] = 0.0; x_hat[1] = 40.0; x_hat[2] = -15.0;
x_hat[3] = 0.0; x_hat[4] = 0.0; x_hat[5] = 0.0;
// Set diagonal covariance entries for prior
P_hat.fill_diagonal(P_POS_INIT);
P_hat(3,3) = P_ROT_INIT; P_hat(4,4) = P_ROT_INIT; P_hat(5,5) = P_ROT_INIT;
// Set diagonal covariance entries for process noise
Q.fill_diagonal(Q_POS);
Q(3,3) = Q_ROT; Q(4,4) = Q_ROT; Q(5,5) = Q_ROT;
// Set diagonal covariance entries for measurement noise
R.fill_diagonal(R_POS);
R(3,3) = R_ROT; R(4,4) = R_ROT; R(5,5) = R_ROT;
// Set weighting for UKF sigma points
Wm(0,0) = (LAMBDA)/(C);
Wc = Wm;
double temp = Wc(0,0);
Wc(0,0) = temp + 1-pow(ALPHA,2)+BETA;
}
/* This counts the number of burning neighbours */
int count_burning_neighbours(int** forest, int row_index, int col_index, int neighbourhood_type){
int neighbors_on_fire=0;
int i=row_index;
int j=col_index;
switch(neighbourhood_type){
case VON_NEUMANN:
if(forest[Nr(i)][Nc(j)]>=BURNING && forest[Nr(i)][Nc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Er(i)][Ec(j)]>=BURNING && forest[Er(i)][Ec(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Wr(i)][Wc(j)]>=BURNING && forest[Wr(i)][Wc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Sr(i)][Sc(j)]>=BURNING && forest[Sr(i)][Sc(j)]<=OLD_BURNING)
neighbors_on_fire++;
break;
case MOORE:
if(forest[Nr(i)][Nc(j)]>=BURNING && forest[Nr(i)][Nc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Er(i)][Ec(j)]>=BURNING && forest[Er(i)][Ec(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Wr(i)][Wc(j)]>=BURNING && forest[Wr(i)][Wc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[Sr(i)][Sc(j)]>=BURNING && forest[Sr(i)][Sc(j)]<=OLD_BURNING)
neighbors_on_fire++;
/* Diagonals */
if(forest[NEr(i)][NEc(j)]>=BURNING && forest[NEr(i)][NEc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[SEr(i)][SEc(j)]>=BURNING && forest[SEr(i)][SEc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[NWr(i)][NWc(j)]>=BURNING && forest[NWr(i)][NWc(j)]<=OLD_BURNING)
neighbors_on_fire++;
if(forest[SWr(i)][SWc(j)]>=BURNING && forest[SWr(i)][SWc(j)]<=OLD_BURNING)
neighbors_on_fire++;
break;
default:
break;
}
return neighbors_on_fire;
}
/* This returns 1 if any neighbours burn, 0 otherwise */
int do_neighbours_burn(int** forest, int row_index, int col_index, int neighbourhood_type){
int i=row_index;
int j=col_index;
switch(neighbourhood_type){
case VON_NEUMANN:
return ((forest[Nr(i)][Nc(j)]>=BURNING &&
forest[Nr(i)][Nc(j)]<=OLD_BURNING) ||
(forest[Er(i)][Ec(j)]>=BURNING &&
forest[Er(i)][Ec(j)]<=OLD_BURNING) ||
(forest[Wr(i)][Wc(j)]>=BURNING &&
forest[Wr(i)][Wc(j)]<=OLD_BURNING) ||
(forest[Sr(i)][Sc(j)]>=BURNING &&
forest[Sr(i)][Sc(j)]<=OLD_BURNING)
);
case MOORE:
return ((forest[Nr(i)][Nc(j)]>=BURNING &&
forest[Nr(i)][Nc(j)]<=OLD_BURNING) ||
(forest[Er(i)][Ec(j)]>=BURNING &&
forest[Er(i)][Ec(j)]<=OLD_BURNING) ||
(forest[Wr(i)][Wc(j)]>=BURNING &&
forest[Wr(i)][Wc(j)]<=OLD_BURNING) ||
(forest[Sr(i)][Sc(j)]>=BURNING &&
forest[Sr(i)][Sc(j)]<=OLD_BURNING)
||
//Diagonals
(forest[NEr(i)][NEc(j)]>=BURNING &&
forest[NEr(i)][NEc(j)]<=OLD_BURNING) ||
(forest[SEr(i)][SEc(j)]>=BURNING &&
forest[SEr(i)][SEc(j)]<=OLD_BURNING) ||
(forest[NWr(i)][NWc(j)]>=BURNING &&
forest[NWr(i)][NWc(j)]<=OLD_BURNING) ||
(forest[SWr(i)][SWc(j)]>=BURNING &&
forest[SWr(i)][SWc(j)]<=OLD_BURNING)
);
default:
return 0;
}
}
void BFilterUKF::predict(){
//L=numel(x); //numer of states
//m=numel(z); //numer of measurements
unsigned int numStates = particles.samples.n_cols;
float alpha=1.0f; //default 1e-3, tunable
float kappa=0.0f; //default, tunable
float beta=2.0f; //default, tunable
//lambda=alpha^2*(L+kappa)-L; //scaling factor
float lambda=alpha*alpha*(numStates+kappa)-numStates; //scaling factor
float c=numStates+lambda; //scaling factor gamma
//Wm=[lambda/c 0.5/c+zeros(1,2*L)]; //weights for means
Wm=zeros<fmat>(1, 2*numStates+1) + (0.5f/c); //weights for means
Wm(0)=lambda/c;
Wc=Wm;
Wc(0)=Wc(0)+(1-alpha*alpha+beta); //weights for covariance
c=sqrt(c);
fmat X=sigmas(fvec(particles.samples),P,c); //sigma points around x
utProcess(X,Wm,Wc,numStates,process->Q);
}
void ProgramPage(u16 a) {
DwSend(Bytes(0x66));
DwSetRegs(29, Bytes(PGWRT, lo(a), hi(a))); // r29 = op (page write), Z = first byte address of page
DwSetPC(BootSect()); // Set PC that allows access to all of flash
DwSend(Bytes(0x64)); // Set up for single step mode
DwOut(SPMCSR(), 29); // out SPMCSR,r29 (PGWRT)
if (BootSect()) {
DwInst(0x95E8); // spm
while ((ReadSPMCSR() & 0x1F) != 0) {Wc('.'); Wflush();} // Wait while programming busy
} else {
DwSend(Bytes(0xD2, 0x95, 0xE8, 0x33)); // spm and break
DwSync();
}
}
void DigisparkBreakAndSync(struct UPort *up) {
// Ws(" -- DigisparkBreakAndSync entry. "); WriteUPort(up);
for (int tries=0; tries<25; tries++) {
// Tell digispark to send a break and capture any returned pulse timings
int status = usb_control_msg(up->handle, OUT_TO_LW, 60, 33, 0, 0, 0, USB_TIMEOUT);
if (status < 0) {
if (status == -1) {
Wsl("Access denied sending USB message to Digispark. Run dwdebug as root, or add a udev rule such as");
Wsl("file /etc/udev/rules.d/60-usbtiny.rules containing:");
Wsl("SUBSYSTEM==\"usb\", ATTR{idVendor}==\"1781\", ATTR{idProduct}==\"0c9f\", GROUP=\"plugdev\", MODE=\"0666\"");
Wsl("And make sure that you are a member of the plugdev group.");
} else {
Ws("Digispark did not accept 'break and capture' command. Error code "); Wd(status,1); Wsl(".");
}
PortFail(up, "");
} else {
delay(120); // Wait while digispark sends break and reads back pulse timings
if (SetDwireBaud(up)) {return;}
}
Wc('.'); Wflush();
}
Wl(); PortFail(up, "Digispark/LittleWire/UsbTiny could not capture pulse timings after 25 break attempts.");
}
void WregPair(int n) {
Wreg(n+1); Wc(':'); Wreg(n);
}
void Wreg(int n) {
Wc('r'); Wd(n, 1);
}
int
main(int argc, char *argv[])
{
char command[20]; // the command either internal or external.
parameters_t parameters; // all other parameters.
char outpath[1024]; // the path of output redirection.
char inpath[1024]; // the path of input redirection.
int flags[2]; // mark if there is any I/O redirection.
// Check if there is a batch file.
if (argc == 2)
{
commands = fopen(argv[1], "r");
if (commands == NULL)
{
printf("Can't open the file \"%s\"!\n", argv[1]);
commands = stdin;
}
}
else
commands = stdin;
// Set the environment variable SHELL.
SetMyshell();
for (Prompt(); fscanf(commands, "%s", command) != EOF; Prompt()) // Initially get the command.
{
// Get all other parameters.
GetParameters(command, ¶meters, flags, inpath, outpath);
// Check if there is a stdout redirection.
switch (flags[0]) {
case 1:
freopen(outpath, "w", stdout);
break;
case 2:
freopen(outpath, "a", stdout);
break;
}
// Check if there is a stdin redirection.
if (flags[1])
freopen(inpath, "r", stdin);
// Run different command correspondingly.
if (!strcmp(command, "cd"))
Cd(¶meters);
else if (!strcmp(command, "clr"))
Clr();
else if (!strcmp(command, "cmp"))
Cmp(¶meters);
else if (!strcmp(command, "dir"))
Dir(¶meters);
else if (!strcmp(command, "echo"))
Echo(¶meters);
else if (!strcmp(command, "environ"))
Environ();
else if (!strcmp(command, "fgrep"))
Fgrep(¶meters);
else if (!strcmp(command, "help"))
Help();
else if (!strcmp(command, "pause"))
Pause();
else if (!strcmp(command, "pwd"))
Pwd();
else if (!strcmp(command, "quit"))
Quit();
else if (!strcmp(command, "wc"))
Wc(¶meters);
else
RunExternal(command, ¶meters, flags, inpath, outpath);
// If there was an I/O redirection, restore the stdin/stdout.
if (flags[0])
freopen("/dev/tty", "w", stdout);
if (flags[1])
freopen("/dev/tty", "r", stdin);
}
return 0;
}
void ShowPageStatus(u16 a, char *msg) {
Ws("$"); Wx(a,4); Ws(" - $"); Wx(a+PageSize()-1,4);
Wc(' '); Ws(msg); Ws(". "); Wr();
}
