void PMU::procDirectDebugInstruction(StringBuilder *input) {
const char* str = (char *) input->position(0);
char c = *str;
int temp_int = 0;
if (input->count() > 1) {
// If there is a second token, we proceed on good-faith that it's an int.
temp_int = input->position_as_int(1);
}
else if (strlen(str) > 1) {
// We allow a short-hand for the sake of short commands that involve a single int.
temp_int = atoi(str + 1);
}
switch (c) {
case 'f':
case 'F':
cpu_scale(*(str) == 'f' ? 0 : 1);
break;
case 'w':
// Find the current wattage draw.
break;
case 'p':
case 'P':
// Start or stop the periodic sensor read.
if (temp_int) {
_periodic_pmu_read.alterSchedulePeriod(temp_int * 10);
local_log.concatf("_periodic_pmu_read set to %d ms period.\n", (temp_int * 10));
}
_periodic_pmu_read.enableSchedule(*(str) == 'P');
local_log.concatf("%s _periodic_pmu_read.\n", (*(str) == 'p' ? "Stopping" : "Starting"));
break;
case 'e':
// Read the present battery voltage.
break;
case 'm': // Set the system-wide power mode.
if (255 != temp_int) {
ManuvrMsg* event = Kernel::returnEvent(MANUVR_MSG_SYS_POWER_MODE);
event->addArg((uint8_t) temp_int);
EventReceiver::raiseEvent(event);
local_log.concatf("Power mode is now %d.\n", temp_int);
}
else {
}
break;
default:
EventReceiver::procDirectDebugInstruction(input);
break;
}
flushLocalLog();
}
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;
}
void MGC3130::dispatchGestureEvents() {
ManuvrMsg* event = nullptr;
if (isPositionDirty()) {
// We don't want to spam the Kernel. We need to rate-limit.
if ((millis() - MGC3130_MINIMUM_NUANCE_PERIOD) > last_nuance_sent) {
if (!position_asserted()) {
// If we haven't asserted the position gesture yet, do so now.
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_RECOGNIZED);
event->addArg((uint32_t) 1);
raiseEvent(event);
position_asserted(true);
}
last_nuance_sent = millis();
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_NUANCE);
event->addArg((uint32_t) 1);
event->addArg((uint16_t) _pos_x);
event->addArg((uint16_t) _pos_y);
event->addArg((uint16_t) _pos_z);
raiseEvent(event);
_pos_x = -1;
_pos_y = -1;
_pos_z = -1;
}
}
else if (position_asserted()) {
// We need to disassert the position gesture.
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_DISASSERT);
event->addArg((uint32_t) 1);
raiseEvent(event);
position_asserted(false);
_pos_x = -1;
_pos_y = -1;
_pos_z = -1;
}
if (0 < wheel_position) {
// We don't want to spam the Kernel. We need to rate-limit.
if ((millis() - MGC3130_MINIMUM_NUANCE_PERIOD) > last_nuance_sent) {
if (!airwheel_asserted()) {
// If we haven't asserted the airwheel gesture yet, do so now.
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_RECOGNIZED);
event->addArg((uint32_t) 2);
raiseEvent(event);
airwheel_asserted(true);
}
last_nuance_sent = millis();
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_NUANCE);
event->addArg((uint32_t) 2);
event->addArg((int32_t) wheel_position);
raiseEvent(event);
wheel_position = 0;
}
}
else if (airwheel_asserted()) {
// We need to disassert the airwheel gesture.
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_DISASSERT);
event->addArg((uint32_t) 2);
raiseEvent(event);
airwheel_asserted(false);
wheel_position = 0;
}
if (0 < last_tap) {
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_ONE_SHOT);
event->addArg(getTouchTapString(last_tap));
raiseEvent(event);
last_tap = 0;
}
if (0 < last_double_tap) {
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_ONE_SHOT);
event->addArg(getTouchTapString(last_double_tap));
raiseEvent(event);
last_double_tap = 0;
}
if (0 < last_swipe) {
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_ONE_SHOT);
event->addArg(getTouchTapString(last_swipe));
raiseEvent(event);
last_swipe = 0;
}
if (isTouchDirty()) {
event = Kernel::returnEvent(MANUVR_MSG_GESTURE_ONE_SHOT);
event->addArg(getTouchTapString(last_touch));
raiseEvent(event);
last_touch_noted = last_touch;
}
if (special) {
// TODO: Not sure how to deal with this yet...
#ifdef MANUVR_DEBUG
if (getVerbosity() > 3) {
local_log.concatf("MGC3130 special code 0x08\n", special);
Kernel::log(&local_log);
}
#endif
special = 0;
}
if (last_event) {
// TODO: Not sure how to deal with this yet...
#ifdef MANUVR_DEBUG
if (getVerbosity() > 3) {
local_log.concatf("MGC3130 last_event 0x08\n", last_event);
Kernel::log(&local_log);
}
#endif
last_event = 0;
}
}