static float calc_roll_setpoint(float sp, plane_data_t *plane_data)
{
float setpoint = 0.0f;
if (plane_data->mode == TAKEOFF) {
setpoint = 0.0f;
} else {
setpoint = calc_bearing(&plane_data) - plane_data->yaw;
if (setpoint < (-sp/180.0f)*F_M_PI)
return (-sp/180.0f)*F_M_PI;
if (setpoint > (sp/180.0f)*F_M_PI)
return (sp/180.0f)*F_M_PI;
}
return setpoint;
}
static float calc_roll_setpoint()
{
float setpoint = 0;
if (plane_data.mode == TAKEOFF) {
setpoint = 0;
} else {
setpoint = calc_bearing() - plane_data.yaw;
if (setpoint < -35)
return -35;
if (setpoint > 35)
return 35;
}
return setpoint;
}
static float calc_roll_setpoint()
{
float setpoint = 0;
if (plane_data.mode == TAKEOFF) {
setpoint = 0;
} else {
setpoint = calc_bearing() - plane_data.yaw;
if (setpoint < (-35.0f/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
if (setpoint > (35/180.0f)*F_M_PI)
return (-35.0f/180.0f)*F_M_PI;
}
return setpoint;
}
void plot_course(struct plane *p, int row, int col, int alt) {
const bool trace = (p->id == 'i' && frame_no == 575);
struct frame *frstart = malloc(sizeof *frstart);
struct frame *frend = frstart;
frstart->prev = frstart->next = NULL;
frstart->opc_start = NULL;
struct op_courses *opc_end = NULL;
assert(alt == 7 || alt == 0);
int bearing = alt ? calc_bearing(row, col)
: get_airport_xy(row, col)->bearing;
bool cleared_exit = false;
struct xyz target;
for (struct plane *pi = plstart; pi; pi = pi->next) {
if (pi == p)
continue;
new_op_course(pi->current, &frstart->opc_start, &opc_end, pi->isjet);
}
incr_opc(frstart->opc_start);
if (p->target_airport) {
struct airport *a = get_airport(p->target_num);
if (a == NULL) {
errexit('u', "Plane '%c' headed to unknown airport %d.",
p->id, p->target_num);
}
target.alt = 1;
target.row = a->trow;
target.col = a->tcol;
} else {
struct exitspec *e = get_exit(p->target_num);
if (e == NULL) {
errexit('u', "Plane '%c' headed to unknown exit %d.",
p->id, p->target_num);
}
target.alt = 9;
target.row = e->row;
target.col = e->col;
}
tracelog(trace, "Tracing plane %c's course from %d:(%d, %d, %d)@%d to "
"(%d, %d, %d)\n",
p->id, frame_no, row, col, alt, bearings[bearing].degree,
target.row, target.col, target.alt);
p->start = p->current = p->end = NULL;
add_course_elem(p, row, col, alt, bearing, false, trace ? frame_no : 0);
p->start_tm = p->current_tm = frame_no;
int tick = frame_no+1;
int steps = 0, moves = 0;
/* Operation of the "plotting course" machine:
* (A) Get a frame for the current pos'n.
* (B) If frame has cands, step ahead to the best cand and return to (A).
* (C) If not, step back to parent frame and remove the cand and
* return to (B).
*/
for (;;) {
if (++steps > 200) {
log_course(p);
errexit('8', "Plane %c stuck in an infinite loop.", p->id);
}
// Plane doesn't move if it's a prop and the tick is odd...
// ...except that a prop plane in an exit will pop out of it.
if (!p->isjet && tick%2 == 1 && row != 0 && col != 0 &&
row != board_height-1 && col != board_width-1) {
add_course_elem(p, row, col, alt, bearing, cleared_exit,
trace ? tick : 0);
tick++;
frend->n_cand = -3;
make_new_fr(&frend);
continue;
}
moves++;
calc_next_move(p, row, col, &alt, target, &bearing, cleared_exit,
frend);
assert((alt < 0) == (frend->n_cand <= 0));
while (frend->n_cand <= 0) {
tracelog(trace, "Backtracking at step %d move %d tick %d\n",
steps, moves, tick);
struct xyz bt_pos = backtrack(&tick, &cleared_exit, &p->end,
&frend);
moves--;
// Check for a prop. plane's non-move.
// TODO: Do we have to worry about the "pop out of an exit" move
// that props get at their first tick?
if (frend->n_cand == -3) {
tracelog(trace,
"Backtracking over prop's non-move at tick %d\n",
tick);
assert(!p->isjet);
bt_pos = backtrack(&tick, &cleared_exit, &p->end, &frend);
assert(frend->n_cand != -3);
}
row = bt_pos.row; col = bt_pos.col;
tracelog(trace, "After backtracking: %d: pos(%d, %d, %d) and %d "
"remaining candidates\n", tick, bt_pos.row,
bt_pos.col, bt_pos.alt, frend->n_cand - 1);
if (--frend->n_cand > 0) {
// We've found a new candidate that's available after
// backtracking, so stop backtracing and get on with it.
alt = frend->cand[frend->n_cand-1].alt;
bearing = frend->cand[frend->n_cand-1].bearing;
break;
}
tracelog(trace, "No new candidates found at tick %d. Backtracking "
"again.\n", tick);
}
if (alt) {
row += bearings[bearing].drow;
col += bearings[bearing].dcol;
}
add_course_elem(p, row, col, alt, bearing, cleared_exit,
trace ? tick : 0);
tick++;
if (row == target.row && col == target.col && alt == target.alt) {
// We've reached the target. Clean-up and return.
if (p->target_airport) {
if (!p->isjet) {
add_course_elem(p, row, col, alt, bearing, cleared_exit,
trace ? tick : 0);
tick++;
}
add_course_elem(p, -1, -1, -2, -1, cleared_exit,
trace ? tick : 0);
p->end_tm = tick;
} else {
// For an exit, the plane disappears at reaching it.
p->end_tm = tick-1;
p->end->at_exit = true;
}
free_framelist(frstart);
if (!quiet) {
struct record *rec = p->isjet ? &rec_jet : &rec_prop;
if (steps > rec->steps || moves > rec->moves) {
if (steps > rec->steps)
rec->steps = steps;
if (moves > rec->moves)
rec->moves = moves;
fprintf(logff, "New record long route: plane '%c' at time "
"%d in %d steps/%d moves.\n",
p->id, frame_no, steps, moves);
log_course(p);
log_all_courses();
}
}
return;
}
if (!cleared_exit && alt > 1 && ((row > 2 && row < board_height-3 &&
col > 2 && col < board_width-3) || alt < 6 || alt == 9)) {
cleared_exit = true;
}
make_new_fr(&frend);
}
}
void geocacheMenu(void)
{
int pressedKey = mySwitch.checkKeypad();
switch (menuState)
{
case MENU0:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
tft.print(monGPSClk);
setCursorTFT(3,0);
tft.print(downldWays);
setCursorTFT(4,0);
tft.print(goToWaypts);
setCursorTFT(5,0);
tft.print(setActWays);
setCursorTFT(6,0);
tft.print(showWaypts);
menuState = MENU0B;
break;
case MENU0B:
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(monGPSLocn);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
clearLine(1);
clearLine(2);
clearLine(3);
clearLine(4);
clearLine(5);
clearLine(6);
menuState = MENU0C;
}
else if (pressedKey == DOWN)
menuState = MENU1;
}
break;
case MENU0C:
if (pressedKey != NOKEY)
menuState = MENU0;
readGPS();
break;
case MENU1:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(monGPSClk);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(3,0);
tft.print(downldWays);
setCursorTFT(4,0);
tft.print(goToWaypts);
setCursorTFT(5,0);
tft.print(setActWays);
setCursorTFT(6,0);
tft.print(showWaypts);
menuState = MENU1B;
break;
case MENU1B:
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
clearLine(0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(monGPSClk);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
clearLine(1);
clearLine(2);
clearLine(3);
clearLine(4);
clearLine(5);
clearLine(6);
menuState = MENU1C;
}
else if (pressedKey == UP)
menuState = MENU0;
else if (pressedKey == DOWN)
menuState = MENU2;
}
break;
case MENU1C:
if (pressedKey != NOKEY)
{
tft.fillScreen(ST7735_BLACK);
menuState = MENU1;
}
GPSClock();
break;
case MENU2:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.print(monGPSClk);
setCursorTFT(3,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(downldWays);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(4,0);
tft.print(goToWaypts);
setCursorTFT(5,0);
tft.print(setActWays);
setCursorTFT(6,0);
tft.print(showWaypts);
menuState = MENU2B;
break;
case MENU2B:
{
int charIn;
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
rxCount = 0;
clearLine(2);
clearLine(3);
clearLine(4);
clearLine(5);
clearLine(6);
clearLine(1);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(downldWays);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(2,0);
menuState = MENU2C;
while (Serial.available()) // flush the serial read port
charIn = Serial.read();
Serial.println("<DL>"); // signals host that it's time to load waypoints
}
else if (pressedKey == UP)
menuState = MENU1;
else if (pressedKey == DOWN)
menuState = MENU3;
}
}
break;
case MENU2C:
{
int charIn;
int errorCode;
if (Serial.available())
{
charIn = Serial.read();
if (charIn == '\n')
{
tft.print(".");
rxBuffer[rxCount] = 0;
errorCode = parseRxBuffer();
if (errorCode == -1)
{
clearLine(2);
tft.print("Error: Missing equal");
}
else if (errorCode == -2)
{
clearLine(2);
tft.print("Error: Bad waypt num");
}
rxCount = 0;
Serial.write('A');
if ((rxBuffer[0] == '1') && (rxBuffer[1]=='9'))
{
clearLine(3);
tft.print("Download Completed ");
EEPROM_writeAnything(0, myStoreVals);
Serial.println("</SERIN>"); // signals host that it's time to load waypoints
delay(2000); // 2 second message
menuState = MENU2;
}
}
else
{
rxBuffer[rxCount++] = charIn;
}
}
if (pressedKey == UP)
{
Serial.println("</DL>"); // signals host that it's time to load waypoints
clearLine(2);
tft.print("Download Terminated ");
delay(2000); // 2 second message
menuState = MENU2;
}
}
break;
case MENU3:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.print(monGPSClk);
setCursorTFT(3,0);
tft.print(downldWays);
setCursorTFT(4,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(goToWaypts);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(5,0);
tft.print(setActWays);
setCursorTFT(6,0);
tft.print(showWaypts);
menuState = MENU3B;
break;
case MENU3B:
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
quietReadGPS();
delay(500);
menuState = MENU3C;
}
else if (pressedKey == UP)
menuState = MENU2;
else if (pressedKey == DOWN)
menuState = MENU4;
}
break;
case MENU3C:
tft.fillScreen(ST7735_BLACK);
tft.drawCircle(64,110,45,ST7735_WHITE);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print("Going to Waypoint ");
setCursorTFT(1,18);
tft.print(currentWayPoint, DEC);
setCursorTFT(2,0);
tft.print("Bearing =");
setCursorTFT(3,0);
tft.print("Distance =");
setCursorTFT(4,0);
tft.print("Direction =");
setCursorTFT(5,0);
tft.print("Satellites =");
quietReadGPS();
setCursorTFT(2,10);
tft.print(" ");
setCursorTFT(2,10);
bearing = calc_bearing(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
tft.print(bearing);
lastLat = fLat2;
setCursorTFT(3,11);
tft.print(" ");
setCursorTFT(3,11);
tft.print(calc_dist(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]));
lastLon = fLon2;
setCursorTFT(4,12);
tft.print(GPS.angle-bearing);
lastAngle = GPS.angle;
lastBearing = bearing;
setCursorTFT(5,13);
tft.print(GPS.satellites);
lastSats = GPS.satellites;
menuState = MENU3D;
break;
case MENU3D:
{
if (pressedKey != NOKEY)
{
menuState = MENU3;
break;
}
if (quietReadGPS() != 0)
{
break;
}
float distCalc = calc_dist(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
bearing = calc_bearing(fLat2,fLon2,myStoreVals.lats[currentWayPoint],myStoreVals.lons[currentWayPoint]);
if ((fLat2 != lastLat) || (fLon2 != lastLon))
{
setCursorTFT(2,10);
tft.print(" ");
setCursorTFT(2,10);
tft.print(bearing);
setCursorTFT(3,11);
tft.print(" ");
setCursorTFT(3,11);
tft.print(distCalc);
lastLat = fLat2;
lastLon = fLon2;
}
if ((GPS.angle != lastAngle) || (bearing != lastBearing))
{
setCursorTFT(4,12);
tft.print(" ");
setCursorTFT(4,12);
tft.print(GPS.angle-bearing);
drawVector(GPS.angle-bearing);
lastAngle = GPS.angle;
lastBearing = bearing;
}
if (lastSats != GPS.satellites)
{
setCursorTFT(5,13);
tft.print(GPS.satellites);
tft.print(" ");
}
}
break;
case MENU4:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.print(monGPSClk);
setCursorTFT(3,0);
tft.print(downldWays);
setCursorTFT(4,0);
tft.print(goToWaypts);
setCursorTFT(5,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(setActWays);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(6,0);
tft.print(showWaypts);
menuState = MENU4B;
break;
case MENU4B:
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(setActWays);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
setCursorTFT(1,0);
tft.print(currentWayPoint, DEC);
menuState = MENU4C;
}
else if (pressedKey == UP)
{
menuState = MENU3;
}
else if (pressedKey == DOWN)
{
menuState = MENU5;
}
}
break;
case MENU4C:
{
if (pressedKey != NOKEY)
{
switch(pressedKey)
{
case SELECT:
myStoreVals.myCurrentWayPoint = currentWayPoint;
EEPROM_writeAnything(0, myStoreVals);
menuState = MENU4;
break;
case UP:
if (currentWayPoint < 19)
currentWayPoint++;
delay(250);
break;
case DOWN:
if (currentWayPoint > 0)
currentWayPoint--;
delay(250);
break;
}
clearLine(1);
tft.print(currentWayPoint, DEC);
}
}
break;
case MENU5:
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.print(menuHeader);
setCursorTFT(1,0);
tft.print(monGPSLocn);
setCursorTFT(2,0);
tft.print(monGPSClk);
setCursorTFT(3,0);
tft.print(downldWays);
setCursorTFT(4,0);
tft.print(goToWaypts);
setCursorTFT(5,0);
tft.print(setActWays);
setCursorTFT(6,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(showWaypts);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
menuState = MENU5B;
break;
case MENU5B:
if (pressedKey != NOKEY)
{
if ((pressedKey == SELECT) || (pressedKey == RIGHT))
{
tft.fillScreen(ST7735_BLACK);
setCursorTFT(0,0);
tft.setTextColor(ST7735_WHITE,ST7735_BLUE);
tft.print(showWaypts);
tft.setTextColor(ST7735_WHITE,ST7735_BLACK);
for (int row = 0; row < 15; row++)
{
setCursorTFT(row+1,0);
tft.print(row);
tft.print(",");
tft.print(myStoreVals.lats[row],4);
tft.print(",");
tft.print(myStoreVals.lons[row],4);
}
menuState = MENU5C;
}
else if (pressedKey == UP)
{
menuState = MENU4;
}
}
break;
case MENU5C:
{
if (pressedKey != NOKEY)
{
switch(pressedKey)
{
case SELECT:
case UP:
tft.fillScreen(ST7735_BLACK);
menuState = MENU5;
break;
}
}
break;
}
}
}