/*
* Updates the local state if warranted, but always returns local state.
*/
ChargeState PMU::getChargeState() {
uint8_t idx = 0;
bool s1 = readPin(_stat1_pin);
bool s2 = readPin(_stat2_pin);
_er_set_flag(DIGITAB_PMU_FLAG_STAT1, s1);
_er_set_flag(DIGITAB_PMU_FLAG_STAT2, s2);
idx += s1 ? 1 : 0;
idx += s2 ? 2 : 0;
switch (idx) {
case 0:
_charge_state = ChargeState::TEST;
break;
case 1:
_charge_state = ChargeState::FULL;
break;
case 2:
_charge_state = ChargeState::CHARGING;
break;
case 3:
// Both pins high. No inference possible.
default:
break;
}
return _charge_state;
}
void PMU::gpioSetup() {
/* These Port B pins are inputs:
*
* # Default Purpose
* -----------------------------------------------
* 0 0 CHG_STAT_1
* 1 0 CHG_STAT_2
*/
// This will cause interrupts to be enabled for these pins.
setPinFxn(_stat1_pin, CHANGE_PULL_UP, mcp73833_stat1_isr);
setPinFxn(_stat2_pin, CHANGE_PULL_UP, mcp73833_stat2_isr);
_er_set_flag(DIGITAB_PMU_FLAG_STAT1, readPin(_stat1_pin));
_er_set_flag(DIGITAB_PMU_FLAG_STAT2, readPin(_stat2_pin));
}
/**
* If we find ourselves in this fxn, it means an event that this class built (the argument)
* has been serviced and we are now getting the chance to see the results. The argument
* to this fxn will never be NULL.
*
* Depending on class implementations, we might choose to handle the completed Event differently. We
* might add values to event's Argument chain and return RECYCLE. We may also free() the event
* ourselves and return DROP. By default, we will return REAP to instruct the Kernel
* to either free() the event or return it to it's preallocate queue, as appropriate. If the event
* was crafted to not be in the heap in its own allocation, we will return DROP instead.
*
* @param event The event for which service has been completed.
* @return A callback return code.
*/
int8_t LegendManager::callback_proc(ManuvrMsg* event) {
/* Setup the default return code. If the event was marked as mem_managed, we return a DROP code.
Otherwise, we will return a REAP code. Downstream of this assignment, we might choose differently. */
int8_t return_value = (0 == event->refCount()) ? EVENT_CALLBACK_RETURN_REAP : EVENT_CALLBACK_RETURN_DROP;
/* Some class-specific set of conditionals below this line. */
switch (event->eventCode()) {
case DIGITABULUM_MSG_IMU_READ:
switch (last_imu_read) {
case 0:
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
last_imu_read++;
return EVENT_CALLBACK_RETURN_RECYCLE;
case 16:
last_imu_read = 0;
break;
default:
if (getVerbosity() > 2) local_log.concat("LegendManager::callback_proc(IMU_READ): Bad arg\n");
last_imu_read = 0;
break;
}
break;
case DIGITABULUM_MSG_IMU_INIT:
switch (last_imu_read) {
case 0:
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
last_imu_read++;
if (getVerbosity() > 6) local_log.concat("LegendManager::callback_proc(IMU_INIT): RECYCLING\n");
// We still have IMUs left to deal with. Recycle the event...
return EVENT_CALLBACK_RETURN_RECYCLE;
case 16:
last_imu_read = 0;
if (getVerbosity() > 6) local_log.concat("LegendManager::callback_proc(IMU_INIT): DROPPING\n");
break;
default:
if (getVerbosity() > 2) local_log.concat("LegendManager::callback_proc(IMU_READ): Bad arg\n");
last_imu_read = 0;
break;
}
break;
case DIGITABULUM_MSG_IMU_LEGEND:
// We take this as an indication that our notice of altered Legend was sent.
_er_set_flag(LEGEND_MGR_FLAGS_LEGEND_SENT);
break;
case DIGITABULUM_MSG_IMU_MAP_STATE:
*(_ptr_sequence) = *(_ptr_sequence) + 1;
if (operating_legend && _er_flag(LEGEND_MGR_FLAGS_LEGEND_SENT)) {
operating_legend->copy_frame();
Kernel::staticRaiseEvent(&event_legend_frame_ready);
return 0;
}
break;
case DIGITABULUM_MSG_IMU_QUAT_CRUNCH:
{
uint8_t temp_uint_8;
if (0 == event->getArgAs(&temp_uint_8)) {
if (iius[temp_uint_8 % 17].has_quats_left()) {
return_value = EVENT_CALLBACK_RETURN_RECYCLE;
}
}
else {
local_log.concat("LegendManager::callback_proc(): QUAT crunch had no argument?!.\n");
}
}
break;
default:
if (getVerbosity() > 5) {
local_log.concat("LegendManager::callback_proc(): Default case.\n");
#if defined(__MANUVR_DEBUG)
event->printDebug(&local_log);
#endif
Kernel::log(&local_log);
}
break;
}
flushLocalLog();
return return_value;
}
/*******************************************************************************
* ___ _ ___ _ _ _ _
* / __| |__ _ ______ | _ ) ___(_) |___ _ _ _ __| |__ _| |_ ___
* | (__| / _` (_-<_-< | _ \/ _ \ | / -_) '_| '_ \ / _` | _/ -_)
* \___|_\__,_/__/__/ |___/\___/_|_\___|_| | .__/_\__,_|\__\___|
* |_|
* Constructors/destructors, class initialization functions and so-forth...
*******************************************************************************/
LegendManager::LegendManager(BusAdapter<SPIBusOp>* bus) : EventReceiver() {
_bus = (CPLDDriver*) bus; // TODO: Make this cast unnecessary.
setReceiverName("ManuMgmt");
INSTANCE = this;
reflection_mag.x = 1;
reflection_mag.y = 1;
reflection_mag.z = -1;
reflection_acc.x = -1;
reflection_acc.y = 1;
reflection_acc.z = -1;
reflection_gyr.x = -1;
reflection_gyr.y = 1;
reflection_gyr.z = -1;
_preformed_read_a.shouldReap(false);
_preformed_read_a.devRegisterAdvance(true);
_preformed_read_a.set_opcode(BusOpcode::RX);
_preformed_read_a.callback = (BusOpCallback*) this;
// Starting from the first accelerometer...
// Read 6 bytes...
// ...across 17 sensors...
// ...from this base address...
_preformed_read_a.setParams(CPLD_REG_IMU_DM_P_I|0x80, 6, 17, LSM9DS1_A_DATA_X);
// ...and drop the results here.
_preformed_read_a.buf = (uint8_t*) __frame_buf_a;
_preformed_read_a.buf_len = 102;
_preformed_read_g.shouldReap(false);
_preformed_read_g.devRegisterAdvance(true);
_preformed_read_g.set_opcode(BusOpcode::RX);
_preformed_read_g.callback = (BusOpCallback*) this;
// Starting from the first gyro...
// Read 6 bytes...
// ...across 17 sensors...
// ...from this base address...
_preformed_read_g.setParams(CPLD_REG_IMU_DM_P_I|0x80, 6, 17, LSM9DS1_G_DATA_X);
// ...and drop the results here.
_preformed_read_g.buf = (uint8_t*) __frame_buf_g;
_preformed_read_g.buf_len = 102;
_preformed_read_m.shouldReap(false);
_preformed_read_m.devRegisterAdvance(true);
_preformed_read_m.set_opcode(BusOpcode::RX);
_preformed_read_m.callback = (BusOpCallback*) this;
// Starting from the first magnetometer...
// Read 6 bytes...
// ...across 17 sensors...
// ...from this base address...
_preformed_read_m.setParams(CPLD_REG_IMU_DM_P_M|0x80, 6, 17, LSM9DS1_M_DATA_X);
// ...and drop the results here.
_preformed_read_m.buf = (uint8_t*) __frame_buf_m;
_preformed_read_m.buf_len = 102;
_preformed_fifo_read.shouldReap(false);
_preformed_fifo_read.devRegisterAdvance(true);
_preformed_fifo_read.set_opcode(BusOpcode::RX);
_preformed_fifo_read.callback = (BusOpCallback*) this;
// Starting from the first inertial...
// Read 1 byte...
// ...across 17 sensors...
// ...from this base address...
_preformed_fifo_read.setParams(CPLD_REG_IMU_DM_P_I|0x80, 1, 17, LSM9DS1_AG_FIFO_SRC);
// ...and drop the results here.
_preformed_fifo_read.buf = (uint8_t*) __fifo_levels;
_preformed_fifo_read.buf_len = 17;
/* Populate all the static preallocation slots for measurements. */
for (uint16_t i = 0; i < PREALLOCATED_IIU_MEASUREMENTS; i++) {
__prealloc[i].wipe();
preallocd_measurements.insert(&__prealloc[i]);
}
// Zero the ManuLegend.
for (int i = 0; i < LEGEND_MGR_MAX_DATASET_SIZE; i++) {
*(__dataset + i) = 0;
}
// Global frame data for MAP_STATE.
_ptr_sequence = (uint32_t*) (__dataset + LEGEND_DATASET_OFFSET_SEQUENCE/4);
_ptr_delta_t = (float*) (__dataset + LEGEND_DATASET_OFFSET_DELTA_T/4);
*(_ptr_sequence) = 0;
_er_set_flag(LEGEND_MGR_FLAGS_LEGEND_STABLE, true); // Ick....
operating_legend = new ManuLegend();
operating_legend->sensorEnabled(true);
//operating_legend->accNullGravity(true);
operating_legend->accRaw(true);
operating_legend->gyro(true);
operating_legend->mag(true);
operating_legend->orientation(true);
operating_legend->temperature(true);
if (operating_legend->finallize()) {
local_log.concat("ManuLegend failed to finallize().\n");
}
reconfigure_data_map();
setLegend(operating_legend);
}
