//******************************************************************************
void ModesTask::handle(glo_modes_t new_modes)
{
// First verify the robot mode is valid.
if (new_modes.main_mode >= NUM_MAIN_MODES)
{
assert_always_msg(ASSERT_CONTINUE, "No mode with ID %d", (int)new_modes.main_mode);
new_modes.main_mode = MAIN_MODE_BALANCE;
}
// Then verify the sub mode is valid if necessary.
if ((new_modes.main_mode == MAIN_MODE_EXPERIMENT) &&
(new_modes.sub_mode >= NUM_EXPERIMENTS))
{
assert_always_msg(ASSERT_CONTINUE, "No experiment with ID %d", (int)new_modes.sub_mode);
new_modes.sub_mode = EXPERIMENT_NONE_SELECTED;
}
bool main_mode_changed = modes_.main_mode != new_modes.main_mode;
bool sub_mode_changed = modes_.sub_mode != new_modes.sub_mode;
if (main_mode_changed || sub_mode_changed)
{
changeState(STATE_STOPPED);
reset();
}
// Don't let state change from outside task.
new_modes.state = modes_.state;
modes_ = new_modes;
glo_modes.publish(&modes_);
}
//******************************************************************************
void TelemetryReceiveTask::newMessageCallback(uint8_t object_id, uint16_t instance, void * glob_data)
{
// Received a new glob over the serial port so need to decide how to handle it.
switch (object_id)
{
case GLO_ID_REQUEST:
receive_task.handle(*((glo_request_t *)glob_data), instance);
break;
default:
assert_always_msg(ASSERT_CONTINUE, "Received unhandled glob with id: %d", object_id);
break;
}
}
//******************************************************************************
void ComplementaryFilterTask::initialize(void)
{
uint32_t mpu_fail_count = 0;
while (mpu_.initialize() != 0)
{
// Sensor failed to initialize properly. Wait then try again.
sys_timer.busyWait(0.5);
if (++mpu_fail_count == 3)
{
assert_always_msg(ASSERT_CONTINUE, "MPU6000 failed to initialize.");
}
}
}
void error(char* s) { assert_always_msg(0, s); }
