void
CannaLooper::_HandleLocationReply(BMessage* msg)
{
BPoint where = B_ORIGIN;
float height = 0.0;
int32 start;
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_LOCATION_REQUEST received\n"));
start = fCanna->GetRevStartPositionInChar();
#ifdef DEBUG
type_code type;
int32 count;
msg->GetInfo("be:location_reply", &type, &count);
SERIAL_PRINT(("CannaLooper: LOCATION_REPLY has %d locations.\n", count));
SERIAL_PRINT(("CannaLooper: RevStartPosition is %d.\n", start));
#endif
msg->FindPoint("be:location_reply", start, &where);
msg->FindFloat("be:height_reply", start, &height);
BMessage m(KOUHO_WINDOW_SHOWAT);
m.AddPoint("position", where);
m.AddFloat("height", height);
fKouhoWindow->PostMessage(fCanna->GenerateKouhoString());
fKouhoWindow->PostMessage(&m);
}
void
CannaLooper::Quit()
{
// delete palette here
SERIAL_PRINT(("CannaLooper: destructor called.\n"));
delete fCanna;
if (fKouhoWindow != NULL) {
SERIAL_PRINT(("CannaLooper: Sending QUIT to kouho window...\n"));
fKouhoWindow->Lock();
fKouhoWindow->Quit();
}
if (fPaletteWindow) {
SERIAL_PRINT(("CannaLooper: Sending QUIT to palette...\n"));
fPaletteWindow->Lock();
fPaletteWindow->Quit();
}
fOwner->SetMenu(NULL, BMessenger());
delete fMenu;
BLooper::Quit();
}
void last_hopper_waypoint(int h) {
Hopper& hopper = hoppers[h];
Target& last_waypoint = hopper_waypoints[h][0];
int hx = boundaries[hopper.index].x;
int hy = boundaries[hopper.index].y;
last_waypoint.theta = atan2(hy - last_waypoint.y, hx - last_waypoint.x);
SERIAL_PRINT("last waypoint: ");
SERIAL_PRINT(hopper_waypoints[h][0].x);
SERIAL_PRINT(' ');
SERIAL_PRINT(hopper_waypoints[h][0].y);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(hopper_waypoints[h][0].theta * RADS);
}
void
CannaLooper::_ForceKakutei()
{
SERIAL_PRINT(( "CannaLooper: _ForceKakutei() called\n" ));
uint32 result = fCanna->Kakutei();
SERIAL_PRINT(("CannaLooper: returned from Kakutei(). result = %d\n",
result));
if (result != NOTHING_TO_KAKUTEI)
_ProcessResult(result);
else
SendInputStopped();
}
BottomlineWindow::BottomlineWindow()
: BWindow(BRect(0, 0, 350, 16), "",
kLeftTitledWindowLook,
B_FLOATING_ALL_WINDOW_FEEL,
B_NOT_V_RESIZABLE | B_NOT_CLOSABLE | B_NOT_ZOOMABLE | B_NOT_MINIMIZABLE
| B_AVOID_FOCUS | B_WILL_ACCEPT_FIRST_CLICK)
{
BRect textRect = Bounds();
textRect.OffsetTo(B_ORIGIN);
textRect.InsetBy(2,2);
fTextView = new BTextView(Bounds(), "", textRect, be_plain_font,
NULL, B_FOLLOW_ALL, B_WILL_DRAW | B_FRAME_EVENTS);
AddChild(fTextView);
fTextView->SetText("");
BRect screenFrame = (BScreen(B_MAIN_SCREEN_ID).Frame());
BPoint pt;
pt.x = 100;
pt.y = screenFrame.Height()*2/3 - Bounds().Height()/2;
MoveTo(pt);
Show();
SERIAL_PRINT(("BottomlineWindow created\n"));
}
void KouhoWindow::ShowAt( BPoint revpoint, float height )
{
BRect screenrect;
BPoint point;
int32 kouhowidth, kouhoheight;
kouhowidth = Frame().IntegerWidth();
kouhoheight = Frame().IntegerHeight();
screenrect = BScreen( this ).Frame();
#ifdef DEBUG
SERIAL_PRINT(( "KouhoWindow: ShowAt activated. point x= %f, y= %f, height= %f", revpoint.x, revpoint.y, height ));
#endif
//adjust to preferred position - considering window border & index
//revpoint.y += WINDOW_BORDER_WIDTH;
point.y = revpoint.y + height + WINDOW_BORDER_WIDTH_V;
point.x = revpoint.x - indexView->Frame().IntegerWidth()
- INDEXVIEW_SIDE_MARGIN;
if ( point.y + kouhoheight > screenrect.bottom )
point.y = revpoint.y - kouhoheight - WINDOW_BORDER_WIDTH_V;
// else
// point.y = revpoint.y + height;
if ( point.x + kouhowidth > screenrect.right )
point.x = point.x - (screenrect.right - (point.x + kouhowidth ));
// point.x = revpoint.x
// - ( revpoint.x + kouhowidth + WINDOW_BORDER_WIDTH - screenrect.right );
// else
// point.x = revpoint.x;
MoveTo( point );
ShowWindow();
}
CannaMethod::~CannaMethod()
{
BLooper *looper = NULL;
cannaLooper.Target( &looper );
if ( looper != NULL )
{
#ifdef DEBUG
SERIAL_PRINT(( "CannaIM:Locking CannaLooper...\n" ));
#endif
if ( looper->Lock() )
#ifdef DEBUG
SERIAL_PRINT(( "CannaIM:CannaLooper locked. Calling Quit().\n" ));
#endif
looper->Quit();
}
WriteSettings();
}
void
CannaLooper::SendInputStopped()
{
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_STOPPED);
EnqueueMessage(msg);
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_STOPPED has been sent\n"));
}
CannaMethod::CannaMethod()
: BInputServerMethod("Canna", (const uchar*)kCannaIcon)
{
#ifdef DEBUG
SERIAL_PRINT(( "CannaIM:Constructor called.\n" ));
#endif
SetMenu(NULL, BMessenger());
ReadSettings();
}
void
CannaLooper::SendInputStarted()
{
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_STARTED);
msg->AddMessenger("be:reply_to", BMessenger(NULL, this));
EnqueueMessage(msg);
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_STARTED has been sent\n"));
}
KouhoWindow::KouhoWindow( BFont *font, BLooper *looper )
:BWindow( DUMMY_RECT,
"kouho", B_MODAL_WINDOW_LOOK,
B_FLOATING_ALL_WINDOW_FEEL,
B_NOT_RESIZABLE | B_NOT_CLOSABLE |
B_NOT_ZOOMABLE | B_NOT_MINIMIZABLE | B_AVOID_FOCUS |
B_NOT_ANCHORED_ON_ACTIVATE )
{
float fontHeight;
BRect frame;
BFont indexfont;
cannaLooper = looper;
kouhoFont = font;
font_family family;
font_style style;
strcpy( family, "Haru" );
strcpy( style, "Regular" );
indexfont.SetFamilyAndStyle( family, style );
indexfont.SetSize( 10 );
#ifdef DEBUG
SERIAL_PRINT(( "kouhoWindow: Constructor called.\n" ));
#endif
//setup main pane
indexWidth = indexfont.StringWidth( "W" ) + INDEXVIEW_SIDE_MARGIN * 2;
minimumWidth = indexfont.StringWidth( "ギリシャ 100/100" );
frame = Bounds();
frame.left = indexWidth + 2;
frame.bottom -= INFOVIEW_HEIGHT;
kouhoView = new KouhoView( frame );
BRect screenrect = BScreen( this ).Frame();
kouhoView->SetTextRect( screenrect ); //big enough
kouhoView->SetFontAndColor( kouhoFont );
kouhoView->SetWordWrap( false );
AddChild( kouhoView );
fontHeight = kouhoView->LineHeight();
frame = Bounds();
frame.right = indexWidth;
frame.bottom = frame.bottom - INFOVIEW_HEIGHT + 1;
indexView = new KouhoIndexView( frame, fontHeight );
indexView->SetFont( &indexfont );
AddChild( indexView );
frame = Bounds();
frame.top = frame.bottom - INFOVIEW_HEIGHT + 1;
infoView = new KouhoInfoView( frame );
infoView->SetFont( &indexfont );
infoView->SetAlignment( B_ALIGN_RIGHT );
AddChild( infoView );
}
void
BottomlineWindow::HandleInputMethodEvent(BMessage* event, EventList& newEvents)
{
CALLED();
PostMessage(event, fTextView);
const char* string;
bool confirmed;
int32 opcode;
if (event->FindInt32("be:opcode", &opcode) != B_OK
|| opcode != B_INPUT_METHOD_CHANGED
|| event->FindBool("be:confirmed", &confirmed) != B_OK
|| !confirmed
|| event->FindString("be:string", &string) != B_OK)
return;
SERIAL_PRINT(("IME : %i, %s\n", opcode, string));
SERIAL_PRINT(("IME : confirmed\n"));
int32 length = strlen(string);
int32 offset = 0;
int32 nextOffset = 0;
while (offset < length) {
// this is supposed to go to the next UTF-8 character
for (++nextOffset; (string[nextOffset] & 0xC0) == 0x80; ++nextOffset)
;
BMessage *newEvent = new BMessage(B_KEY_DOWN);
if (newEvent != NULL) {
newEvent->AddInt32("key", 0);
newEvent->AddInt64("when", system_time());
BString bytes(string + offset, nextOffset - offset);
newEvent->AddString("bytes", bytes);
newEvent->AddInt32("raw_char", 0xa);
newEvents.AddItem(newEvent);
}
offset = nextOffset;
}
}
void
CannaLooper::_HandleKeyDown(BMessage* msg)
{
uint32 modifier;
int32 key;
msg->FindInt32("modifiers", (int32*)&modifier);
msg->FindInt32("key", &key);
if ((modifier & B_COMMAND_KEY) != 0) {
EnqueueMessage(DetachCurrentMessage());
return;
}
char character;
msg->FindInt8("byte", (int8*)&character);
// The if clause below is to avoid processing key input which char code
// is 0x80 or more.
// if mikakutei string exists, dispose current message.
// Otherwise, send it to application as usual B_KEY_DOWN message.
if ((character & 0x80) != 0) {
if (fCanna->MikakuteiLength() != 0)
delete DetachCurrentMessage();
else
EnqueueMessage(DetachCurrentMessage());
return;
}
SERIAL_PRINT(("CannaLooper: HandleKeyDown() calling "
"CannaInterface::KeyIn()...\n", result));
uint32 result = fCanna->KeyIn(character, modifier, key);
SERIAL_PRINT(("CannaLooper: HandleKeyDown() received result = %d from "
"CannaInterface.\n", result));
_ProcessResult(result);
}
status_t
CannaMethod::InitCheck()
{
#ifdef DEBUG
SERIAL_PRINT(( "CannaIM:InitCheck() called.\n" ));
#endif
CannaLooper *looper;
status_t err;
looper = new CannaLooper( this );
looper->Lock();
err = looper->Init();
looper->Unlock();
cannaLooper = BMessenger( NULL, looper );
#ifdef DEBUG
if ( err != B_NO_ERROR )
SERIAL_PRINT(( "CannaLooper::InitCheck() failed.\n" ));
else
SERIAL_PRINT(( "CannaLooper::InitCheck() success.\n" ));
#endif
return err;
}
void passive_red_line_correct() {
// only correct to red line if not backing up
if (abs(y - RENDEZVOUS_Y) < GRID_WIDTH*0.5 && layers[active_layer].speed > 0) {
digitalWrite(bottom_led, HIGH);
if (on_line(CENTER)) cycles_on_red_line = 0;
else if (on_line(RED) && !on_line(CENTER)) {
++cycles_on_red_line;
// navigate trying to get back to red line and x is far enough forward
if (seeking_red_line && RENDEZVOUS_X - x < 3*RENDEZVOUS_CLOSE) {
waypoint(LAYER_NAV);
}
}
else if (!on_line(RED)) {
// not false alarm
if (cycles_on_red_line >= CYCLES_CROSSING_LINE && current_distance() - last_correct_distance > DISTANCE_CENTER_TO_RED_ALLOWANCE) {
SERIAL_PRINT("RC");
SERIAL_PRINTLN(current_distance() - last_correct_distance);
// direction and signs are taken care of by sin and cos
float offset_y = DISTANCE_CENTER_TO_RED * sin(theta);
// avoid correcting when parallel to horizontal line
int offset_x = abs((int)x) % GRID_WIDTH;
if (abs(offset_y) > NEED_TO_HOPPER_CORRECT && (offset_x < INTERSECTION_TOO_CLOSE*0.5 || offset_x > GRID_WIDTH - INTERSECTION_TOO_CLOSE*0.5) && parallel_to_horizontal()) {
SERIAL_PRINTLN("-RC");
cycles_on_red_line = 0;
last_correct_distance = current_distance();
return;
}
y = RENDEZVOUS_Y;
// between [-180,0] left while going left
// x += DISTANCE_CENTER_TO_RED * cos(theta);
if (theta < 0) offset_y -= HALF_LINE_WIDTH;
else offset_y += HALF_LINE_WIDTH;
y += offset_y;
last_red_line_distance = current_distance();
last_correct_distance = last_red_line_distance;
if (side_of_board == SIDE_RIGHT) side_of_board = SIDE_LEFT;
else if (side_of_board == SIDE_LEFT) side_of_board = SIDE_RIGHT;
}
cycles_on_red_line = 0;
}
}
else {
digitalWrite(bottom_led, LOW);
cycles_on_red_line = 0;
}
}
void follow_hopper_waypoints(byte h) {
Hopper& hopper = hoppers[h];
if (hopper.waypoint == 0) {
SERIAL_PRINT("No waypoints:");
SERIAL_PRINTLN(h);
return;
}
add_target(hopper_waypoints[h][0].x, hopper_waypoints[h][0].y, hopper_waypoints[h][0].theta, TARGET_GET, true);
for (byte w = 1; w < hopper.waypoint; ++w) {
add_target(hopper_waypoints[h][w].x, hopper_waypoints[h][w].y, ANY_THETA);
}
}
void
CannaMethod::ReadSettings()
{
BMessage pref;
BFile preffile( CANNAIM_SETTINGS_FILE, B_READ_ONLY );
if ( preffile.InitCheck() == B_NO_ERROR && pref.Unflatten( &preffile ) == B_OK )
{
pref.FindBool( "arrowkey", &gSettings.convert_arrowkey );
pref.FindPoint( "palette", &gSettings.palette_loc );
pref.FindPoint( "standalone", &gSettings.standalone_loc );
#ifdef DEBUG
SERIAL_PRINT(( "CannaMethod: ReadSettings() success. arrowkey=%d, palette_loc=%d,%d standalone_loc=%d, %d\n", gSettings.convert_arrowkey, gSettings.palette_loc.x, gSettings.palette_loc.y, gSettings.standalone_loc.x, gSettings.standalone_loc.y ));
#endif
return;
}
//set default values
#ifdef DEBUG
SERIAL_PRINT(( "CannaMethod: ReadSettings() failed.\n" ));
#endif
gSettings.convert_arrowkey = true;
gSettings.palette_loc.Set( 800, 720 );
gSettings.standalone_loc.Set( 256, 576 );
}
void CannaMethod::WriteSettings()
{
BMessage pref;
BFile preffile( CANNAIM_SETTINGS_FILE,
B_WRITE_ONLY | B_CREATE_FILE | B_ERASE_FILE );
if ( preffile.InitCheck() == B_NO_ERROR )
{
pref.AddBool( "arrowkey", gSettings.convert_arrowkey );
pref.AddPoint( "palette", gSettings.palette_loc );
pref.AddPoint( "standalone", gSettings.standalone_loc );
pref.Flatten( &preffile );
#ifdef DEBUG
SERIAL_PRINT(( "CannaMethod: WriteSettings() success. arrowkey=%d, palette_loc=%d,%d standalone_loc=%d, %d\n", gSettings.convert_arrowkey, gSettings.palette_loc.x, gSettings.palette_loc.y, gSettings.standalone_loc.x, gSettings.standalone_loc.y ));
#endif
}
}
// return from hopper to rendezvous in reverse direction
void return_from_hopper() {
byte h;
if (active_hopper == HOPPER1) h = 0;
else if (active_hopper == HOPPER2) h = 2;
else if (active_hopper == HOPPER3) h = 3;
else if (active_hopper == HOPPER4) h = 4;
Hopper& hopper = hoppers[h];
if (hopper.waypoint == 0) {
SERIAL_PRINT("No waypoints:");
SERIAL_PRINTLN(active_hopper);
return;
}
add_target(RENDEZVOUS_X, RENDEZVOUS_Y, 0, TARGET_PUT);
// skip the last target
for (byte w = hoppers[h].waypoint - 1; w > 0; --w) {
add_target(hopper_waypoints[h][w].x, hopper_waypoints[h][w].y);
}
}
void *
hoardSbrk(long size)
{
assert(size > 0);
CTRACE(("sbrk: size = %ld\n", size));
// align size request
size = (size + hoardHeap::ALIGNMENT - 1) & ~(hoardHeap::ALIGNMENT - 1);
// choose correct protection flags
uint32 protection = B_READ_AREA | B_WRITE_AREA;
if (__gABIVersion < B_HAIKU_ABI_GCC_2_HAIKU)
protection |= B_EXECUTE_AREA;
hoardLock(sHeapLock);
// find chunk in free list
free_chunk *chunk = sFreeChunks, *last = NULL;
for (; chunk != NULL; chunk = chunk->next) {
CTRACE((" chunk %p (%ld)\n", chunk, chunk->size));
if (chunk->size < (size_t)size) {
last = chunk;
continue;
}
// this chunk is large enough to satisfy the request
SERIAL_PRINT(("HEAP-%ld: found free chunk to hold %ld bytes\n",
find_thread(NULL), size));
void *address = (void *)chunk;
if (chunk->size > (size_t)size + sizeof(free_chunk)) {
// divide this chunk into smaller bits
size_t newSize = chunk->size - size;
free_chunk *next = chunk->next;
chunk = (free_chunk *)((addr_t)chunk + size);
chunk->next = next;
chunk->size = newSize;
if (last != NULL) {
last->next = next;
insert_chunk(chunk);
} else
sFreeChunks = chunk;
} else {
chunk = chunk->next;
if (last != NULL)
last->next = chunk;
else
sFreeChunks = chunk;
}
hoardUnlock(sHeapLock);
return address;
}
// There was no chunk, let's see if the area is large enough
size_t oldHeapSize = sFreeHeapSize;
sFreeHeapSize += size;
// round to next heap increment aligned size
size_t incrementAlignedSize = (sFreeHeapSize + kHeapIncrement - 1)
& ~(kHeapIncrement - 1);
if (incrementAlignedSize <= sHeapAreaSize) {
SERIAL_PRINT(("HEAP-%ld: heap area large enough for %ld\n",
find_thread(NULL), size));
// the area is large enough already
hoardUnlock(sHeapLock);
return (void *)(sFreeHeapBase + oldHeapSize);
}
// We need to grow the area
SERIAL_PRINT(("HEAP-%ld: need to resize heap area to %ld (%ld requested)\n",
find_thread(NULL), incrementAlignedSize, size));
status_t status = resize_area(sHeapArea, incrementAlignedSize);
if (status != B_OK) {
// Either the system is out of memory or another area is in the way and
// prevents ours from being resized. As a special case of the latter
// the user might have mmap()ed something over malloc()ed memory. This
// splits the heap area in two, the first one retaining the original
// area ID. In either case, if there's still memory, it is a good idea
// to try and allocate a new area.
sFreeHeapSize = oldHeapSize;
if (status == B_NO_MEMORY) {
hoardUnlock(sHeapLock);
return NULL;
}
size_t newHeapSize = (size + kHeapIncrement - 1) / kHeapIncrement
* kHeapIncrement;
// First try at the location directly after the current heap area, if
// that is still in the reserved memory region.
void* base = (void*)(sFreeHeapBase + sHeapAreaSize);
area_id area = -1;
if (sHeapBase != NULL
&& base >= sHeapBase
&& (addr_t)base + newHeapSize
<= (addr_t)sHeapBase + kHeapReservationSize) {
area = create_area("heap", &base, B_EXACT_ADDRESS, newHeapSize,
B_NO_LOCK, protection);
if (area == B_NO_MEMORY) {
hoardUnlock(sHeapLock);
return NULL;
}
}
// If we don't have an area yet, try again with a free location
// allocation.
if (area < 0) {
base = (void*)(sFreeHeapBase + sHeapAreaSize);
area = create_area("heap", &base, B_RANDOMIZED_BASE_ADDRESS,
newHeapSize, B_NO_LOCK, protection);
}
if (area < 0) {
hoardUnlock(sHeapLock);
return NULL;
}
// We have a new area, so make it the new heap area.
sHeapArea = area;
sFreeHeapBase = (addr_t)base;
sHeapAreaSize = newHeapSize;
sFreeHeapSize = size;
oldHeapSize = 0;
} else
sHeapAreaSize = incrementAlignedSize;
hoardUnlock(sHeapLock);
return (void *)(sFreeHeapBase + oldHeapSize);
}
void PrintValueComma(char val) {
SERIAL_PRINT(val);
comma();
}
void PrintValueComma(unsigned long val)
{
SERIAL_PRINT(val);
comma();
}
void PrintValueComma(byte val)
{
SERIAL_PRINT(val);
comma();
}
void PrintValueComma(long int val)
{
SERIAL_PRINT(val);
comma();
}
void sendSerialTelemetry() {
switch (queryType) {
case '=': // Reserved debug command to view any variable from Serial Monitor
break;
case 'a': // Send roll and pitch rate mode PID values
PrintPID(RATE_XAXIS_PID_IDX);
PrintPID(RATE_YAXIS_PID_IDX);
PrintValueComma(rotationSpeedFactor);
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'b': // Send roll and pitch attitude mode PID values
PrintPID(ATTITUDE_XAXIS_PID_IDX);
PrintPID(ATTITUDE_YAXIS_PID_IDX);
PrintPID(ATTITUDE_GYRO_XAXIS_PID_IDX);
PrintPID(ATTITUDE_GYRO_YAXIS_PID_IDX);
SERIAL_PRINTLN(windupGuard);
queryType = 'X';
break;
case 'c': // Send yaw PID values
PrintPID(ZAXIS_PID_IDX);
PrintPID(HEADING_HOLD_PID_IDX);
SERIAL_PRINTLN((int)headingHoldConfig);
queryType = 'X';
break;
case 'd': // Altitude Hold
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
PrintPID(BARO_ALTITUDE_HOLD_PID_IDX);
PrintValueComma(PID[BARO_ALTITUDE_HOLD_PID_IDX].windupGuard);
PrintValueComma(altitudeHoldBump);
PrintValueComma(altitudeHoldPanicStickMovement);
PrintValueComma(minThrottleAdjust);
PrintValueComma(maxThrottleAdjust);
#if defined AltitudeHoldBaro
PrintValueComma(baroSmoothFactor);
#else
PrintValueComma(0);
#endif
PrintPID(ZDAMPENING_PID_IDX);
#else
PrintDummyValues(10);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'e': // miscellaneous config values
PrintValueComma(aref);
SERIAL_PRINTLN(minArmedThrottle);
queryType = 'X';
break;
case 'f': // Send transmitter smoothing values
PrintValueComma(receiverXmitFactor);
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
PrintValueComma(receiverSmoothFactor[axis]);
}
PrintDummyValues(10 - LASTCHANNEL);
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'g': // Send transmitter calibration data
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
Serial.print(receiverSlope[axis], 6);
Serial.print(',');
}
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'h': // Send transmitter calibration data
for (byte axis = XAXIS; axis < LASTCHANNEL; axis++) {
Serial.print(receiverOffset[axis], 6);
Serial.print(',');
}
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'i': // Send sensor data
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
PrintValueComma(gyroRate[axis]);
}
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
PrintValueComma(filteredAccel[axis]);
}
for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
#if defined(HeadingMagHold)
PrintValueComma(getMagnetometerData(axis));
#else
PrintValueComma(0);
#endif
}
SERIAL_PRINTLN();
break;
case 'j': // Send raw mag values
#ifdef HeadingMagHold
PrintValueComma(getMagnetometerRawData(XAXIS));
PrintValueComma(getMagnetometerRawData(YAXIS));
SERIAL_PRINTLN(getMagnetometerRawData(ZAXIS));
#endif
break;
case 'k': // Send accelerometer cal values
SERIAL_PRINT(accelScaleFactor[XAXIS], 6);
comma();
SERIAL_PRINT(runTimeAccelBias[XAXIS], 6);
comma();
SERIAL_PRINT(accelScaleFactor[YAXIS], 6);
comma();
SERIAL_PRINT(runTimeAccelBias[YAXIS], 6);
comma();
SERIAL_PRINT(accelScaleFactor[ZAXIS], 6);
comma();
SERIAL_PRINTLN(runTimeAccelBias[ZAXIS], 6);
queryType = 'X';
break;
case 'l': // Send raw accel values
measureAccelSum();
PrintValueComma((int)(accelSample[XAXIS]/accelSampleCount));
accelSample[XAXIS] = 0;
PrintValueComma((int)(accelSample[YAXIS]/accelSampleCount));
accelSample[YAXIS] = 0;
SERIAL_PRINTLN ((int)(accelSample[ZAXIS]/accelSampleCount));
accelSample[ZAXIS] = 0;
accelSampleCount = 0;
break;
case 'm': // Send magnetometer cal values
#ifdef HeadingMagHold
SERIAL_PRINT(magBias[XAXIS], 6);
comma();
SERIAL_PRINT(magBias[YAXIS], 6);
comma();
SERIAL_PRINTLN(magBias[ZAXIS], 6);
#endif
queryType = 'X';
break;
case 'n': // battery monitor
#ifdef BattMonitor
PrintValueComma(batteryMonitorAlarmVoltage);
PrintValueComma(batteryMonitorThrottleTarget);
PrintValueComma(batteryMonitorGoingDownTime);
#else
PrintDummyValues(3);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'o': // send waypoints
#ifdef UseGPSNavigator
for (byte index = 0; index < MAX_WAYPOINTS; index++) {
PrintValueComma(index);
PrintValueComma(waypoint[index].latitude);
PrintValueComma(waypoint[index].longitude);
PrintValueComma(waypoint[index].altitude);
}
#else
PrintDummyValues(4);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'p': // Send Camera values
#ifdef CameraControl
PrintValueComma(cameraMode);
PrintValueComma(servoCenterPitch);
PrintValueComma(servoCenterRoll);
PrintValueComma(servoCenterYaw);
PrintValueComma(mCameraPitch);
PrintValueComma(mCameraRoll);
PrintValueComma(mCameraYaw);
PrintValueComma(servoMinPitch);
PrintValueComma(servoMinRoll);
PrintValueComma(servoMinYaw);
PrintValueComma(servoMaxPitch);
PrintValueComma(servoMaxRoll);
PrintValueComma(servoMaxYaw);
#ifdef CameraTXControl
PrintValueComma(servoTXChannels);
#endif
#else
#ifdef CameraTXControl
PrintDummyValues(14);
#else
PrintDummyValues(13);
#endif
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'q': // Send Vehicle State Value
SERIAL_PRINTLN(vehicleState);
queryType = 'X';
break;
case 'r': // Vehicle attitude
PrintValueComma(kinematicsAngle[XAXIS]);
PrintValueComma(kinematicsAngle[YAXIS]);
SERIAL_PRINTLN(getHeading());
break;
case 's': // Send all flight data
PrintValueComma(motorArmed);
PrintValueComma(kinematicsAngle[XAXIS]);
PrintValueComma(kinematicsAngle[YAXIS]);
PrintValueComma(getHeading());
#if defined AltitudeHoldBaro || defined AltitudeHoldRangeFinder
#if defined AltitudeHoldBaro
PrintValueComma(getBaroAltitude());
#elif defined AltitudeHoldRangeFinder
PrintValueComma(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] != INVALID_RANGE ? rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX] : 0.0);
#endif
PrintValueComma((int)altitudeHoldState);
#else
PrintValueComma(0);
PrintValueComma(0);
#endif
for (byte channel = 0; channel < 8; channel++) { // Configurator expects 8 values
PrintValueComma((channel < LASTCHANNEL) ? receiverCommand[channel] : 0);
}
for (byte motor = 0; motor < LASTMOTOR; motor++) {
PrintValueComma(motorCommand[motor]);
}
PrintDummyValues(8 - LASTMOTOR); // max of 8 motor outputs supported
#ifdef BattMonitor
PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s
#else
PrintValueComma(0);
#endif
PrintValueComma(flightMode);
SERIAL_PRINTLN();
break;
case 't': // Send processed transmitter values
for (byte axis = 0; axis < LASTCHANNEL; axis++) {
PrintValueComma(receiverCommand[axis]);
}
SERIAL_PRINTLN();
break;
case 'u': // Send range finder values
#if defined (AltitudeHoldRangeFinder)
PrintValueComma(maxRangeFinderRange);
SERIAL_PRINTLN(minRangeFinderRange);
#else
PrintValueComma(0);
SERIAL_PRINTLN(0);
#endif
queryType = 'X';
break;
case 'v': // Send GPS PIDs
#if defined (UseGPSNavigator)
PrintPID(GPSROLL_PID_IDX);
PrintPID(GPSPITCH_PID_IDX);
PrintPID(GPSYAW_PID_IDX);
queryType = 'X';
#else
PrintDummyValues(9);
#endif
SERIAL_PRINTLN();
queryType = 'X';
break;
case 'y': // send GPS info
#if defined (UseGPS)
PrintValueComma(gpsData.state);
PrintValueComma(gpsData.lat);
PrintValueComma(gpsData.lon);
PrintValueComma(gpsData.height);
PrintValueComma(gpsData.course);
PrintValueComma(gpsData.speed);
PrintValueComma(gpsData.accuracy);
PrintValueComma(gpsData.sats);
PrintValueComma(gpsData.fixtime);
PrintValueComma(gpsData.sentences);
PrintValueComma(gpsData.idlecount);
#else
PrintDummyValues(11);
#endif
SERIAL_PRINTLN();
break;
case 'z': // Send all Altitude data
#if defined (AltitudeHoldBaro)
PrintValueComma(getBaroAltitude());
#else
PrintValueComma(0);
#endif
#if defined (AltitudeHoldRangeFinder)
SERIAL_PRINTLN(rangeFinderRange[ALTITUDE_RANGE_FINDER_INDEX]);
#else
SERIAL_PRINTLN(0);
#endif
break;
case '$': // send BatteryMonitor voltage/current readings
#if defined (BattMonitor)
PrintValueComma((float)batteryData[0].voltage/100.0); // voltage internally stored at 10mV:s
#if defined (BM_EXTENDED)
PrintValueComma((float)batteryData[0].current/100.0);
PrintValueComma((float)batteryData[0].usedCapacity/1000.0);
#else
PrintDummyValues(2);
#endif
#else
PrintDummyValues(3);
#endif
SERIAL_PRINTLN();
break;
case '%': // send RSSI
#if defined (UseAnalogRSSIReader) || defined (UseEzUHFRSSIReader) || defined (UseSBUSRSSIReader)
SERIAL_PRINTLN(rssiRawValue);
#else
SERIAL_PRINTLN(0);
#endif
break;
case 'x': // Stop sending messages
break;
case '!': // Send flight software version
SERIAL_PRINTLN(SOFTWARE_VERSION, 1);
queryType = 'X';
break;
case '#': // Send configuration
reportVehicleState();
queryType = 'X';
break;
case '6': // Report remote commands
for (byte motor = 0; motor < LASTMOTOR; motor++) {
PrintValueComma(motorCommand[motor]);
}
SERIAL_PRINTLN();
queryType = 'X';
break;
#if defined(OSD) && defined(OSD_LOADFONT)
case '&': // fontload
if (OFF == motorArmed) {
max7456LoadFont();
}
queryType = 'X';
break;
#endif
}
}
void
CannaLooper::_ProcessResult(uint32 result)
{
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing result = %d\n",
result));
if ((result & GUIDELINE_APPEARED) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"GUIDELINE_APPEARED\n"));
if (fCanna->MikakuteiLength() != 0) {
// usual guideline i.e. kouho
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_LOCATION_REQUEST);
fOwner->EnqueueMessage(msg);
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_LOCATION_REQUEST has "
"been sent\n"));
} else {
// guideline exists, but no mikakutei string - means extend mode
// and such.
SERIAL_PRINT((" GUIDELINE_APPEARED: calling "
"GenerateKouho()...\n"));
fKouhoWindow->PostMessage(fCanna->GenerateKouhoString());
SERIAL_PRINT((" GUIDELINE_APPEARED: posting KouhoMsg to "
"KouhoWindow %x...\n", fKouhoWindow));
fKouhoWindow->PostMessage(KOUHO_WINDOW_SHOW_ALONE);
}
}
if ((result & GUIDELINE_DISAPPEARED) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"GUIDELINE_DISAPPEARED\n"));
fKouhoWindow->PostMessage(KOUHO_WINDOW_HIDE);
}
if ((result & MODE_CHANGED) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"MODE_CHANGED\n"));
BMessage message(PALETTE_WINDOW_BUTTON_UPDATE);
message.AddInt32("mode", fCanna->GetMode());
fPaletteWindow->PostMessage(&message);
SERIAL_PRINT(("CannaLooper: PALETTE_BUTTON_UPDATE has been sent. "
"mode = %d\n", fCanna->GetMode()));
}
if ((result & GUIDELINE_CHANGED) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"GUIDELINE_CHANGED\n"));
fKouhoWindow->PostMessage(fCanna->GenerateKouhoString());
}
if ((result & THROUGH_INPUT) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"THROUGH_INPUT\n"));
EnqueueMessage(DetachCurrentMessage());
}
if ((result & NEW_INPUT_STARTED) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"NEW_INPUT_STARTED\n"));
SendInputStarted();
}
if ((result & KAKUTEI_EXISTS) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"KAKUTEI_EXISTS\n"));
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_CHANGED);
msg->AddString("be:string", fCanna->GetKakuteiStr());
msg->AddInt32("be:clause_start", 0);
msg->AddInt32("be:clause_end", fCanna->KakuteiLength());
msg->AddInt32("be:selection", fCanna->KakuteiLength());
msg->AddInt32("be:selection", fCanna->KakuteiLength());
msg->AddBool("be:confirmed", true);
fOwner->EnqueueMessage(msg);
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_CHANGED (confired) has "
"been sent\n"));
// if both kakutei and mikakutei exist, do not send B_INPUT_STOPPED
if (!(result & MIKAKUTEI_EXISTS))
SendInputStopped();
}
if ((result & MIKAKUTEI_EXISTS) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"MIKAKUTEI_EXISTS\n" ));
int32 start, finish;
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_CHANGED);
msg->AddString("be:string", fCanna->GetMikakuteiStr());
if (fCanna->HasRev()) {
fCanna->GetRevPosition( &start, &finish);
msg->AddInt32("be:clause_start", 0);
msg->AddInt32("be:clause_end", start);
msg->AddInt32("be:clause_start", start);
msg->AddInt32("be:clause_end", finish);
msg->AddInt32("be:clause_start", finish);
msg->AddInt32("be:clause_end", fCanna->MikakuteiLength());
} else {
start = finish = fCanna->MikakuteiLength();
msg->AddInt32("be:clause_start", 0);
msg->AddInt32("be:clause_end", fCanna->MikakuteiLength());
}
msg->AddInt32("be:selection", start);
msg->AddInt32("be:selection", finish);
//msg->AddBool("be:confirmed", false);
fOwner->EnqueueMessage(msg);
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_CHANGED (non-confirmed) "
"has been sent\n"));
}
if ((result & MIKAKUTEI_BECOME_EMPTY) != 0) {
SERIAL_PRINT(("CannaLooper: _ProcessResult() processing "
"MIKAKUTEI_BECOME_EMPTY\n" ));
BMessage* msg = new BMessage(B_INPUT_METHOD_EVENT);
msg->AddInt32("be:opcode", B_INPUT_METHOD_CHANGED);
msg->AddString("be:string", B_EMPTY_STRING);
msg->AddInt32("be:clause_start", 0);
msg->AddInt32("be:clause_end", 0);
msg->AddInt32("be:selection", 0);
msg->AddInt32("be:selection", 0);
msg->AddBool( "be:confirmed", true);
fOwner->EnqueueMessage(msg);
SERIAL_PRINT(( "CannaLooper: B_INPUT_METHOD_CHANGED (NULL, confired) "
"has been sent\n"));
SendInputStopped();
}
}
void comma() {
SERIAL_PRINT(',');
}
void
CannaLooper::MessageReceived(BMessage* msg)
{
SERIAL_PRINT(("CannaLooper: Entering MessageReceived() what=%.4s\n",
(char*)&msg->what));
switch (msg->what) {
case B_KEY_DOWN:
case NUM_SELECTED_FROM_KOUHO_WIN:
_HandleKeyDown(msg);
break;
case B_INPUT_METHOD_EVENT:
uint32 opcode, result;
msg->FindInt32("be:opcode", (int32*)&opcode);
switch (opcode) {
case B_INPUT_METHOD_LOCATION_REQUEST:
_HandleLocationReply(msg);
break;
case B_INPUT_METHOD_STOPPED:
SERIAL_PRINT(("CannaLooper: B_INPUT_METHOD_STOPPED "
"received\n"));
_ForceKakutei();
break;
}
break;
case CANNA_METHOD_ACTIVATED:
SERIAL_PRINT(("CannaLooper: CANNA_METHOD_ACTIVATED received\n"));
_HandleMethodActivated(msg->HasBool("active"));
break;
case MODE_CHANGED_FROM_PALETTE:
_ForceKakutei();
int32 mode;
msg->FindInt32("mode", &mode);
result = fCanna->ChangeMode(mode);
_ProcessResult(result);
break;
case B_ABOUT_REQUESTED:
{
SERIAL_PRINT(("CannaLooper: B_ABOUT_REQUESTED received\n"));
BAlert* panel = new BAlert( "", "Canna Input Method\n"
" by Masao Kawamura 1999\n\n"
"Canna\n"
" Copyright 1992 NEC Corporation, Tokyo, Japan\n"
" Special thanks to T.Murai for porting\n",
"OK");
panel->Go();
break;
}
case RELOAD_INIT_FILE:
_ForceKakutei();
fCanna->Reset();
break;
case ARROW_KEYS_FLIPPED:
{
BMenuItem* item = fMenu->FindItem(ARROW_KEYS_FLIPPED);
gSettings.convert_arrowkey = !gSettings.convert_arrowkey;
item->SetMarked(gSettings.convert_arrowkey);
fCanna->SetConvertArrowKey(gSettings.convert_arrowkey);
break;
}
default:
BLooper::MessageReceived(msg);
break;
}
}
// correct passing a line not too close to an intersection
void passive_correct() {
// still on cool down
if (passive_status < PASSED_NONE) {++passive_status; return;}
if (!far_from_intersection(x, y)) {
if (passive_status > PASSED_NONE) passive_status = PASSED_NONE;
return;
}
// SERIAL_PRINTLN(passive_status, BIN);
// check if center one first activated; halfway there
if (on_line(CENTER) && !(passive_status & CENTER)) {
correct_half_distance = current_distance();
SERIAL_PRINTLN("PH");
}
// activate passive correct when either left or right sensor FIRST ACTIVATES
if ((on_line(LEFT) || on_line(RIGHT)) && passive_status == PASSED_NONE) {
correct_initial_distance = current_distance();
// only look at RIGHT LEFT CENTER
passive_status |= (on_lines & ENCOUNTERED_ALL);
SERIAL_PRINT("PS");
SERIAL_PRINTLN(passive_status, BIN);
// all hit at the same time, don't know heading
if (passive_status == ENCOUNTERED_ALL) hit_first = CENTER;
else if (passive_status & LEFT) hit_first = LEFT;
else hit_first = RIGHT;
// return;
}
if ((passive_status & LEFT) && !on_line(LEFT)) passive_status |= PASSED_LEFT;
if ((passive_status & RIGHT) && !on_line(RIGHT)) passive_status |= PASSED_RIGHT;
// check if encountering any additional lines
passive_status |= (on_lines & ENCOUNTERED_ALL);
// distance from first to center too far, probably too parallel to line
if (passive_status > PASSED_NONE && !(passive_status & CENTER) && (current_distance() - correct_initial_distance > CORRECT_TOO_FAR)) {
passive_status = PASSED_COOL_DOWN;
SERIAL_PRINT("PP");
SERIAL_PRINTLN(current_distance() - correct_initial_distance);
return;
}
// already hit center, see if second half distance is too far from first half distance
else if ((passive_status & CENTER) &&
((current_distance() - correct_half_distance) > // second half distance
(correct_half_distance - correct_initial_distance + CORRECT_CROSSING_TOLERANCE))) { // first half distance plus some room for error
SERIAL_PRINT("PD");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
passive_status = PASSED_COOL_DOWN;
}
// correct at the first encounter of line for each sensor
if ((passive_status & ENCOUNTERED_ALL) == ENCOUNTERED_ALL) {
// correct only if the 2 half distances are about the same
if (abs((current_distance() - correct_half_distance) - (correct_half_distance - correct_initial_distance)) < CORRECT_CROSSING_TOLERANCE) {
// distance since when passive correct was activated
float correct_elapsed_distance = current_distance() - correct_initial_distance;
// always positive
float theta_offset = atan2(correct_elapsed_distance, SIDE_SENSOR_DISTANCE);
// reverse theta correction if direction is backwards
if (layers[get_active_layer()].speed < 0) theta_offset = -theta_offset;
float theta_candidate;
// assume whichever one passed first was the first to hit
if (passive_status & PASSED_LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (passive_status & PASSED_RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
else if (hit_first == LEFT) theta_candidate = (square_heading()*DEGS) + theta_offset;
else if (hit_first == RIGHT) theta_candidate = (square_heading()*DEGS) - theta_offset;
// hit at the same time?
else theta_candidate = (square_heading()*DEGS);
// check how far away correction is from current theta
if (abs(theta - theta_candidate) > THETA_CORRECT_LIMIT) {
SERIAL_PRINT("PO");
SERIAL_PRINTLN(theta_candidate*RADS);
passive_status = PASSED_COOL_DOWN;
return;
}
// else correct to candidate value
theta = theta_candidate;
SERIAL_PRINT('P');
SERIAL_PRINTLN(theta_offset*RADS);
}
// suspicious of an intersection
else {
SERIAL_PRINT("PI");
SERIAL_PRINT(correct_half_distance - correct_initial_distance);
SERIAL_PRINT(' ');
SERIAL_PRINTLN(current_distance() - correct_half_distance);
}
// reset even if not activated on this line (false alarm)
passive_status = PASSED_COOL_DOWN;
}
}
// get ball behaviour, should only occur at hopper approach
void get_ball() {
Layer& get = layers[LAYER_GET];
if (!get.active) return;
if (LAYER_GET == active_layer) {
SERIAL_PRINT(layers[LAYER_GET].speed);
SERIAL_PRINT('|');
SERIAL_PRINTLN(layers[LAYER_GET].angle);
}
// either going forward or backward, always no angle
// go forward until ball is detected or arbitrary additional distance travelled
if (ball_status < CAUGHT_BALL) {
if (caught_ball()) ++ball_status;
get.speed = GET_SPEED;
if (abs(boundaries[active_hopper].theta) < THETA_TOLERANCE) get.angle = 0;
// turn slightly to face hopper
else if (boundaries[active_hopper].theta < 0) get.angle = -GET_TURN;
else get.angle = GET_TURN;
if (ball_status == CAUGHT_BALL) {
// got to the ball, can also correct for position to be near hopper
correct_to_hopper();
// then close servo gate
close_gate();
hard_break(LAYER_GET);
get_initial_distance = tot_distance;
}
}
// after securing ball, drive backwards
else if (ball_status == SECURED_BALL) {
// initial kick to go straight
static int kick_cycle = 0;
if (get.speed > -GET_SPEED) {get.angle = 13; }
else if (get.angle > 0) {
if (kick_cycle <= 0) {
if (get.angle < 7) kick_cycle = 7 - get.angle;
--get.angle;
}
else --kick_cycle;
}
get.speed = -GET_SPEED;
// get.angle = 0;
if (paused) resume_drive(LAYER_GET);
// back up until you hit a line to correct position
if (on_line(CENTER)) {
corrected_while_backing_up = true;
correct_to_grid();
SERIAL_PRINTLN("CWB");
}
// backed up far enough
if (tot_distance - get_initial_distance > 5*GET_DISTANCE) {
// corrected_while_backing_up &&
// after getting ball, return to rendezvous point
corrected_while_backing_up = false;
layers[LAYER_GET].active = false;
close_hoppers();
return_from_hopper();
// hard_break(LAYER_GET, 3);
}
}
// in the middle of closing the gate, wait a couple cycles
else {
++ball_status;
}
}
