void LegendManager::procDirectDebugInstruction(StringBuilder *input) {
char* str = input->position(0);
uint8_t temp_byte = 0;
if (*(str) != 0) {
temp_byte = atoi((char*) str+1);
}
StringBuilder parse_mule;
switch (*(str)) {
case 'v':
parse_mule.concat(str);
local_log.concatf("parse_mule split (%s) into %d positions.\n", str, parse_mule.split(","));
parse_mule.drop_position(0);
if (temp_byte < 17) {
if (parse_mule.count() > 0) {
int temp_int = parse_mule.position_as_int(0);
iius[temp_byte].setVerbosity(temp_int);
}
local_log.concatf("Verbosity on IIU %d is %d.\n", temp_byte, iius[temp_byte].getVerbosity());
}
break;
case 'i':
if (1 == temp_byte) {
local_log.concatf("The IIU preallocated measurements are stored at %p.\n", (uintptr_t) __prealloc);
}
else if (2 == temp_byte) {
if (operating_legend) {
operating_legend->printDebug(&local_log);
}
}
else {
int8_t old_verbosity = getVerbosity();
if (temp_byte && (temp_byte < 7)) setVerbosity(temp_byte);
printDebug(&local_log);
if (temp_byte && (temp_byte < 7)) setVerbosity(old_verbosity);
}
break;
case '-':
if (operating_legend) {
operating_legend->copy_frame();
local_log.concat("Frame copied.\n");
operating_legend->printDataset(&local_log);
}
break;
case '+':
if (temp_byte < 17) {
iius[temp_byte].dumpPointers(&local_log);
}
break;
// IMU DEBUG //////////////////////////////////////////////////////////////////
case 'c':
if (temp_byte < 17) {
iius[temp_byte].printDebug(&local_log);
}
break;
// IMU STATE CONTROL //////////////////////////////////////////////////////////
case 'g':
if (255 == temp_byte) {
local_log.concat("Syncing all IIUs...\n");
for (uint8_t i = 0; i < 17; i++) {
iius[i].sync();
}
}
else if (temp_byte < 17) {
iius[temp_byte].sync();
local_log.concatf("Syncing IIU %d.\n", temp_byte);
}
break;
case 's':
parse_mule.concat(str);
parse_mule.split(",");
parse_mule.drop_position(0);
if (parse_mule.count() > 0) {
int temp_int = parse_mule.position_as_int(0);
if (255 == temp_byte) {
for (uint8_t i = 0; i < 17; i++) {
iius[i].setOperatingState(temp_int);
}
}
else if (temp_byte < 17) {
local_log.concatf("Setting the state of IMU %d to %d\n", temp_byte, temp_int);
iius[temp_byte].setOperatingState(temp_int);
}
}
break;
case 'k':
if ((temp_byte < 6) && (temp_byte >= 0)) {
ManuvrMsg *event = Kernel::returnEvent(DIGITABULUM_MSG_IMU_INIT);
event->addArg((uint8_t) temp_byte); // Set the desired init stage.
event->priority(0);
raiseEvent(event);
local_log.concatf("Broadcasting IMU_INIT for stage %u...\n", temp_byte);
}
else {
local_log.concatf("Illegal INIT stage: %u\n", temp_byte);
}
break;
case 'r':
if (255 == temp_byte) {
local_log.concat("Reseting all IIUs...\n");
for (uint8_t i = 0; i < 17; i++) {
iius[i].reset();
}
}
else if (temp_byte < 17) {
local_log.concatf("Resetting IIU %d.\n", temp_byte);
iius[temp_byte].reset();
}
break;
case 'T':
case 't':
if (temp_byte < 17) {
ManuvrMsg *event = Kernel::returnEvent((*(str) == 'T') ? DIGITABULUM_MSG_IMU_DOUBLE_TAP : DIGITABULUM_MSG_IMU_TAP);
event->setOriginator((EventReceiver*) this);
event->addArg((uint8_t) temp_byte);
Kernel::staticRaiseEvent(event);
local_log.concatf("Sent %stap event for IMU %d.\n", ((*(str) == 'T') ? "double ":""), temp_byte);
}
break;
case 'q':
if (temp_byte < 17) {
ManuvrMsg *event = Kernel::returnEvent(DIGITABULUM_MSG_IMU_QUAT_CRUNCH);
event->specific_target = (EventReceiver*) this;
event->addArg((uint8_t) temp_byte);
Kernel::staticRaiseEvent(event);
local_log.concatf("Running quat on IIU %d.\n", temp_byte);
}
break;
// IMU DATA ///////////////////////////////////////////////////////////////////
case 'j':
switch (*(str+1)) {
case '0':
reflection_acc.x = -1;
reflection_acc.y = 1;
reflection_acc.z = -1;
reflection_gyr.set(-1, 1, -1);
reflection_mag.x = 1;
reflection_mag.y = 1;
reflection_mag.z = -1;
break;
case 'm':
if (*(str+2) == 'x') reflection_mag.x *= -1;
else if (*(str+2) == 'y') reflection_mag.y *= -1;
else if (*(str+2) == 'z') reflection_mag.z *= -1;
break;
case 'a':
if (*(str+2) == 'x') reflection_acc.x *= -1;
else if (*(str+2) == 'y') reflection_acc.y *= -1;
else if (*(str+2) == 'z') reflection_acc.z *= -1;
break;
case 'g':
if (*(str+2) == 'x') reflection_gyr.x *= -1;
else if (*(str+2) == 'y') reflection_gyr.y *= -1;
else if (*(str+2) == 'z') reflection_gyr.z *= -1;
break;
}
local_log.concatf("Reflection vectors\n\tMag (%d, %d, %d)\n\tAcc (%d, %d, %d)\n\tGyr (%d, %d, %d)\n", reflection_mag.x, reflection_mag.y, reflection_mag.z, reflection_acc.x, reflection_acc.y, reflection_acc.z, reflection_gyr.x, reflection_gyr.y, reflection_gyr.z);
break;
case '[':
case ']':
if (255 == temp_byte) {
local_log.concatf("%sabling spherical abberation correction on all IIUs.\n", ((*(str) == ']') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].correctSphericalAbberation((*(str) == ']'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].correctSphericalAbberation((*(str) == ']'));
local_log.concatf("%sabling spherical abberation correction on IIU %d.\n", ((*(str) == ']') ? "En":"Dis"), temp_byte);
}
break;
case 'u':
case 'U':
if (255 == temp_byte) {
local_log.concatf("%sabling (clean-mag-is-zero) on all IIUs.\n", ((*(str) == 'U') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].cleanMagZero((*(str) == 'U'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].cleanMagZero((*(str) == 'U'));
local_log.concatf("%sabling (clean-mag-is-zero) on IIU %d.\n", ((*(str) == 'U') ? "En":"Dis"), temp_byte);
}
break;
case 'w':
case 'W':
if (255 == temp_byte) {
local_log.concatf("%sabling mag data scrutiny on all IIUs.\n", ((*(str) == 'Z') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].dropObviousBadMag((*(str) == 'Z'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].dropObviousBadMag((*(str) == 'Z'));
local_log.concatf("%sabling mag data scrutiny on IIU %d.\n", ((*(str) == 'Z') ? "En":"Dis"), temp_byte);
}
break;
case 'z':
case 'Z':
if (255 == temp_byte) {
local_log.concatf("%sabling autoscale on all IIUs.\n", ((*(str) == 'Z') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].enableAutoscale((*(str) == 'Z'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].enableAutoscale((*(str) == 'Z'));
local_log.concatf("%sabling autoscale on IIU %d.\n", ((*(str) == 'Z') ? "En":"Dis"), temp_byte);
}
break;
case 'n':
case 'N':
if (255 == temp_byte) {
local_log.concatf("%sabling range-binding on all IIUs.\n", ((*(str) == 'N') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].rangeBind((*(str) == 'N'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].rangeBind((*(str) == 'N'));
local_log.concatf("%sabling range-binding on IIU %d.\n", ((*(str) == 'N') ? "En":"Dis"), temp_byte);
}
break;
case 'h':
case 'H':
if (255 == temp_byte) {
local_log.concatf("%sabling quats on all IIUs.\n", ((*(str) == 'H') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].processQuats((*(str) == 'H'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].processQuats((*(str) == 'H'));
local_log.concatf("%sabling quats on IIU %d.\n", ((*(str) == 'H') ? "En":"Dis"), temp_byte);
}
break;
case 'x':
case 'X':
if (255 == temp_byte) {
local_log.concatf("%sabling gyro error compensation on all IIUs.\n", ((*(str) == 'X') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].nullGyroError((*(str) == 'X'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].nullGyroError((*(str) == 'X'));
local_log.concatf("%sabling gyro error compensation on IIU %d.\n", ((*(str) == 'X') ? "En":"Dis"), temp_byte);
}
break;
case 'm':
case 'M':
if (255 == temp_byte) {
local_log.concatf("%sabling gravity nullification on all IIUs.\n", ((*(str) == 'M') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].nullifyGravity((*(str) == 'M'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].nullifyGravity((*(str) == 'M'));
local_log.concatf("%sabling gravity nullification on IIU %d.\n", ((*(str) == 'M') ? "En":"Dis"), temp_byte);
}
break;
case 'y':
case 'Y':
if (255 == temp_byte) {
local_log.concatf("%sabling bearing nullification on all IIUs.\n", ((*(str) == 'Y') ? "En":"Dis"));
for (uint8_t i = 0; i < 17; i++) {
iius[i].nullifyBearing((*(str) == 'Y'));
}
}
else if (temp_byte < 17) {
iius[temp_byte].nullifyBearing((*(str) == 'Y'));
local_log.concatf("%sabling bearing nullification on IIU %d.\n", ((*(str) == 'Y') ? "En":"Dis"), temp_byte);
}
break;
case 'Q':
local_log.concatf("Madgwick iterations to %d on all IIUs.\n", temp_byte);
for (uint8_t i = 0; i < 17; i++) {
iius[i].madgwickIterations(temp_byte);
}
break;
case ',':
IIU::max_quats_per_event = temp_byte;
local_log.concatf("IIU class now runs a maximum of %u quats per event.\n", IIU::max_quats_per_event);
break;
case 'b':
for (uint8_t i = 0; i < 17; i++) {
iius[i].beta = (float)temp_byte * 0.1;
}
local_log.concatf("Beta value is now %f.\n", (double) (temp_byte * 0.1f));
break;
case 'L':
for (uint8_t i = 0; i < 17; i++) {
iius[i].setSampleRateProfile(temp_byte);
}
local_log.concatf("Moving to sample rate profile %d.\n", temp_byte);
break;
case 'o':
for (uint8_t i = 0; i < 17; i++) {
iius[i].setGyroBaseFiler(temp_byte);
}
local_log.concatf("Setting GYR base filter to %d.\n", temp_byte);
break;
case 'O':
for (uint8_t i = 0; i < 17; i++) {
iius[i].setAccelBaseFiler(temp_byte);
}
local_log.concatf("Setting ACC base filter to %d.\n", temp_byte);
break;
case 'a':
if (255 == temp_byte) {
refreshIMU();
}
else if (17 > temp_byte) {
refreshIMU(temp_byte);
}
break;
case 'd':
switch (temp_byte) {
case 255:
event_legend_frame_ready.fireNow(); // Fire a single frame transmission.
local_log.concat("We are manually firing the IMU frame broadcasts schedule.\n");
break;
case 254:
event_legend_frame_ready.enableSchedule(true); // Enable the periodic read.
local_log.concat("Enabled frame broadcasts.\n");
break;
#if defined(__MANUVR_DEBUG)
case 253:
event_legend_frame_ready.printDebug(&local_log);
break;
#endif
case 252:
send_map_event();
local_log.concat("We are manually firing the IMU frame broadcasts schedule.\n");
break;
default:
if (temp_byte) {
event_legend_frame_ready.alterSchedulePeriod(temp_byte*10);
local_log.concatf("Set periodic frame broadcast to once every %dms.\n", temp_byte*10);
}
else {
event_legend_frame_ready.enableSchedule(false); // Disable the periodic read.
local_log.concat("Disabled frame broadcasts.\n");
}
break;
}
break;
case 'f':
switch (temp_byte) {
case 255:
event_iiu_read.fireNow();
local_log.concat("We are manually firing the IMU read schedule.\n");
break;
case 254:
event_iiu_read.enableSchedule(true);
local_log.concat("Enabled periodic readback.\n");
break;
default:
if (temp_byte) {
event_iiu_read.alterSchedulePeriod(temp_byte*10);
local_log.concatf("Set periodic read schedule to once every %dms.\n", temp_byte*10);
}
else {
event_iiu_read.enableSchedule(false); // Disable the periodic read.
local_log.concat("Disabled periodic readback.\n");
}
break;
}
break;
case 'p':
{
parse_mule.concat(str);
parse_mule.split(",");
parse_mule.drop_position(0);
uint8_t start = (temp_byte < 17) ? temp_byte : 0;
uint8_t stop = (temp_byte < 17) ? temp_byte+1 : 17;
int temp_int = (parse_mule.count() > 0) ? parse_mule.position_as_int(0) : 255;
for (uint8_t i = start; i < stop; i++) {
if (255 != temp_int) { // The user wants to make a change..
iius[i].enableProfiling(temp_int ? true:false);
}
local_log.concatf("Profiling IIU %d: %sabled.\n", i, (iius[i].enableProfiling() ? "en":"dis"));
}
}
break;
case 'e':
if (temp_byte < 17) {
iius[temp_byte].dumpPreformedElements(&local_log);
}
break;
default:
EventReceiver::procDirectDebugInstruction(input);
break;
}
flushLocalLog();
}
int8_t LegendManager::notify(ManuvrMsg* active_event) {
int8_t return_value = 0;
uint8_t temp_uint_8 = 0;
/* Some class-specific set of conditionals below this line. */
switch (active_event->eventCode()) {
case DIGITABULUM_MSG_IMU_READ:
iius[last_imu_read].readSensor();
return_value++;
break;
case MANUVR_MSG_SESS_ESTABLISHED:
event_legend_frame_ready.delaySchedule(1100); // Enable the periodic frame broadcast.
{
ManuvrMsg *event = Kernel::returnEvent(DIGITABULUM_MSG_IMU_INIT);
event->addArg((uint8_t) 4); // Set the desired init stage.
event->priority(0);
raiseEvent(event);
}
event_iiu_read.delaySchedule(1000); // Enable the periodic read after letting the dust settle.
return_value++;
break;
case MANUVR_MSG_SESS_HANGUP:
event_legend_frame_ready.enableSchedule(false);
for (uint8_t i = 0; i < LEGEND_DATASET_IIU_COUNT; i++) {
ManuvrMsg *event = Kernel::returnEvent(DIGITABULUM_MSG_IMU_INIT);
event->addArg((uint8_t) 4); // Set the desired init stage.
event->priority(0);
raiseEvent(event);
}
event_iiu_read.enableSchedule(false); // Disable the periodic read.
return_value++;
break;
case DIGITABULUM_MSG_IMU_INIT:
/* This is a request (probably from elsewhere in this class) to move one-or-more
IMUs into the given INIT stage. The argument forms are...
None A request to move all IMUs into the minimum meaningful INIT stage (INIT-1).
uint8 A request to move all IMUs into the given INIT stage.
*/
if (0 == active_event->argCount()) {
if (last_imu_read > 16) {
if (getVerbosity() > 1) local_log.concat("MSG_IMU_INIT: last_imu_read > 16.\n");
}
else {
iius[last_imu_read].init();
return_value++;
}
}
else if (0 == active_event->getArgAs(&temp_uint_8)) {
// If the arg was present, we interpret this as a specified INIT stage...
if (temp_uint_8 > 16) {
if (getVerbosity() > 1) local_log.concat("MSG_IMU_INIT had an IMU idx > 16.\n");
}
else {
iius[last_imu_read].state_pass_through(temp_uint_8);
return_value++;
}
}
break;
case DIGITABULUM_MSG_IMU_MAP_STATE:
//if (0 == active_event->argCount()) {
// No args means a request. Send it.
// send_map_event();
// return_value++;
//}
break;
case DIGITABULUM_MSG_CPLD_RESET_COMPLETE:
if (getVerbosity() > 3) local_log.concatf("Initializing IMUs...\n");
// Range-bind everything....
for (uint8_t i = 0; i < 17; i++) iius[i].rangeBind(true);
// Fire the event to put the IMUs into INIT-1.
//raiseEvent(Kernel::returnEvent(DIGITABULUM_MSG_IMU_INIT));
return_value++;
break;
case DIGITABULUM_MSG_IMU_TAP:
if (0 == active_event->argCount()) {
// Somthing wants the thresholds for all configured taps.
}
else {
// Otherwise, it means we've emitted the event. No need to respond.
}
break;
case DIGITABULUM_MSG_IMU_QUAT_CRUNCH:
if (0 == active_event->getArgAs(&temp_uint_8)) {
if (temp_uint_8 > 16) {
if (getVerbosity() > 1) local_log.concat("QUAT_CRUNCH had an IMU idx > 16.\n");
}
else {
iius[temp_uint_8].MadgwickQuaternionUpdate();
}
return_value++;
}
else {
if (getVerbosity() > 2) local_log.concatf("QUAT_CRUNCH handler (IIU %u) got a bad return from an Arg..\n", temp_uint_8);
}
break;
default:
return_value += EventReceiver::notify(active_event);
break;
}
flushLocalLog();
return return_value;
}