/**
* Function to test mesh functionality. We have to figure out if we have to enable the radio first, and that kind of
* thing.
*/
void CMesh::init() {
LOGi("For now we are testing the meshing functionality.");
nrf_gpio_pin_clear(PIN_GPIO_LED0);
rbc_mesh_init_params_t init_params;
init_params.access_addr = 0xA541A68F;
init_params.adv_int_ms = 100;
init_params.channel = 38;
init_params.handle_count = 2;
init_params.packet_format = RBC_MESH_PACKET_FORMAT_ORIGINAL;
init_params.radio_mode = RBC_MESH_RADIO_MODE_BLE_1MBIT;
LOGi("Call rbc_mesh_init");
uint8_t error_code;
// checks if softdevice is enabled etc.
error_code = rbc_mesh_init(init_params);
APP_ERROR_CHECK(error_code);
LOGi("Call rbc_mesh_value_enable. Todo. Do this on send() and receive()!");
error_code = rbc_mesh_value_enable(1);
APP_ERROR_CHECK(error_code);
error_code = rbc_mesh_value_enable(2);
APP_ERROR_CHECK(error_code);
}
// Setup will calibrate our compass by finding maximum/minimum magnetic readings
void Compass::calibrate()
{
_calibrated = false;
// The highest possible magnetic value to read in any direction is 2047
// The lowest possible magnetic value to read in any direction is -2047
LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767};
unsigned char index;
// Initiate the Wire library and join the I2C bus as a master
Wire.begin();
// Initiate LSM303
_compass.init();
// Enables accelerometer and magnetometer
_compass.enableDefault();
_compass.writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
_compass.writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ); // 220 Hz compass update rate
delay(500);
LOGi("starting calibration");
// To calibrate the magnetometer, the Zumo spins to find the max/min
// magnetic vectors. This information is used to correct for offsets
// in the magnetometer data.
drive(SPEED, -SPEED);
for(index = 0; index < CALIBRATION_SAMPLES; index ++)
{
// Take a reading of the magnetic vector and store it in compass.m
_compass.read();
running_min.x = min(running_min.x, _compass.m.x);
running_min.y = min(running_min.y, _compass.m.y);
running_max.x = max(running_max.x, _compass.m.x);
running_max.y = max(running_max.y, _compass.m.y);
delay(50);
}
drive_stop();
LOGi("max.x %d", running_max.x);
LOGi("max.y %d", running_max.y);
LOGi("min.x %d", running_min.x);
LOGi("min.y %d", running_min.y);
// Set _calibrated values to compass.m_max and compass.m_min
_compass.m_max.x = running_max.x;
_compass.m_max.y = running_max.y;
_compass.m_min.x = running_min.x;
_compass.m_min.y = running_min.y;
_calibrated = true;
}
Storage::Storage() {
LOGi("Storage create");
// call once before using any other API calls of the persistent storage module
BLE_CALL(pstorage_init, ());
for (int i = 0; i < NR_CONFIG_ELEMENTS; i++) {
LOGi("Init %i bytes persistent storage (FLASH) for id %d", config[i].storage_size, config[i].id);
initBlocks(config[i].storage_size, config[i].handle);
}
}
void Tracker::stopTracking() {
if (!_stack) {
LOGe(STR_ERR_FORGOT_TO_ASSIGN_STACK);
return;
}
_tracking = false;
if (_stack->isScanning()) {
LOGi(FMT_STOP, "tracking");
_stack->stopScanning();
} else {
LOGi(STR_ERR_ALREADY_STOPPED);
}
}
void MeshControl::handleEvent(EventType evt, void* p_data, uint16_t length) {
switch(evt) {
case EVT_POWER_ON:
case EVT_POWER_OFF: {
assert(length < MAX_EVENT_MESH_MESSAGE_DATA_LENGTH, "event data is too long");
LOGi("handle power event");
mesh_message_t msg;
uint8_t targetAddress[BLE_GAP_ADDR_LEN] = {0x03, 0x24, 0x79, 0x56, 0xD6, 0xC6};
memcpy(msg.targetAddress, &targetAddress, BLE_GAP_ADDR_LEN);
msg.evtMsg.type = evt;
memcpy(msg.evtMsg.data, p_data, length);
CMesh::getInstance().send(EVENT_CHANNEL, (uint8_t*)&msg, sizeof(msg));
// EventMeshPackage msg;
// msg.evt = evt;
// msg.p_data = (uint8_t*)p_data;
// CMesh::getInstance().send(EVENT_CHANNEL, (uint8_t*)&msg, length + 1);
break;
}
default:
break;
}
}
void IndoorLocalizationService::createCharacteristics() {
LOGi(FMT_SERVICE_INIT, BLE_SERVICE_INDOOR_LOCALIZATION);
#if CHAR_RSSI==1
LOGi(FMT_CHAR_ADD, STR_CHAR_RSSI);
addRssiCharacteristic();
#else
LOGi(FMT_CHAR_SKIP, STR_CHAR_RSSI);
#endif
#if CHAR_SCAN_DEVICES==1
LOGi(FMT_CHAR_ADD, STR_CHAR_SCAN_DEVICES);
addScanControlCharacteristic();
addScannedDeviceListCharacteristic();
#else
LOGi(FMT_CHAR_SKIP, STR_CHAR_SCAN_DEVICES);
#endif
#if CHAR_TRACK_DEVICES==1
LOGi(FMT_CHAR_ADD, STR_CHAR_TRACKED_DEVICE);
addTrackedDeviceListCharacteristic();
addTrackedDeviceCharacteristic();
#else
LOGi(FMT_CHAR_SKIP, STR_CHAR_TRACKED_DEVICE);
#endif
addCharacteristicsDone();
}
/**
* Event handler on receiving a message from
*/
void rbc_mesh_event_handler(rbc_mesh_event_t* evt)
{
TICK_PIN(28);
//nrf_gpio_gitpin_toggle(PIN_GPIO_LED1);
switch (evt->event_type)
{
case RBC_MESH_EVENT_TYPE_CONFLICTING_VAL:
LOGd("conflicting value");
break;
case RBC_MESH_EVENT_TYPE_NEW_VAL:
LOGd("new value");
break;
case RBC_MESH_EVENT_TYPE_UPDATE_VAL:
LOGd("update value");
break;
case RBC_MESH_EVENT_TYPE_INITIALIZED:
LOGd("initialized");
break;
}
switch (evt->event_type)
{
// case RBC_MESH_EVENT_TYPE_CONFLICTING_VAL:
case RBC_MESH_EVENT_TYPE_NEW_VAL:
case RBC_MESH_EVENT_TYPE_UPDATE_VAL: {
if (evt->value_handle > 2)
break;
//if (evt->data[0]) {
LOGi("Got data ch: %i, val: %i, len: %d, orig_addr:", evt->value_handle, evt->data[0], evt->data_len);
// BLEutil::printArray(evt->originator_address.addr, 6);
MeshControl &meshControl = MeshControl::getInstance();
meshControl.process(evt->value_handle, evt->data, evt->data_len);
//}
//led_config(evt->value_handle, evt->data[0]);
break;
}
default:
LOGi("Default: %i", evt->event_type);
break;
}
}
void Tracker::handleTrackedDeviceCommand(buffer_ptr_t buffer, uint16_t size) {
TrackedDevice dev;
//TODO: should we check the result of assign() ?
dev.assign(buffer, size);
if (dev.getRSSI() > 0) {
#ifdef PRINT_TRACKER_VERBOSE
LOGi("Remove tracked device");
#endif
#ifdef PRINT_DEBUG
dev.print();
#endif
Tracker::getInstance().removeTrackedDevice(dev);
} else {
#ifdef PRINT_TRACKER_VERBOSE
LOGi("Add tracked device");
#endif
#ifdef PRINT_DEBUG
dev.print();
#endif
Tracker::getInstance().addTrackedDevice(dev);
}
TrackedDeviceList* trackedDeviceList = Tracker::getInstance().getTrackedDevices();
#ifdef PRINT_DEBUG
LOGi("currently tracking devices:");
trackedDeviceList->print();
#endif
if (trackedDeviceList->getSize() > 0) {
Tracker::getInstance().startTracking();
} else {
Tracker::getInstance().stopTracking();
}
}
void Tracker::startTracking() {
if (!_stack) {
LOGe(STR_ERR_FORGOT_TO_ASSIGN_STACK);
return;
}
if (!_tracking) {
//! Set to some value initially
_trackIsNearby = false;
_tracking = true;
if (!_stack->isScanning()) {
LOGi(FMT_START, "tracking");
_stack->startScanning();
}
Timer::getInstance().start(_appTimerId, HZ_TO_TICKS(TRACKER_UPATE_FREQUENCY), this);
} else {
LOGi(FMT_ALREADY, "tracking");
}
}
bool TrackedDeviceList::add(const uint8_t* adrs_ptr, int8_t rssi_threshold) {
for (int i=0; i<getSize(); ++i) {
//! check if it is already in the list, then update
if (memcmp(adrs_ptr, _buffer->list[i].addr, BLE_GAP_ADDR_LEN) == 0) {
_buffer->list[i].rssiThreshold = rssi_threshold;
//_buffer->counters[i] = TDL_COUNTER_INIT; //! Don't update counter
#ifdef PRINT_TRACKEDDEVICES_VERBOSE
LOGi("Updated [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
adrs_ptr[5], adrs_ptr[4], adrs_ptr[3], adrs_ptr[2],
adrs_ptr[1], adrs_ptr[0], rssi_threshold);
#endif
return true;
}
}
if (getSize() >= TRACKDEVICES_MAX_NR_DEVICES) {
return false; //! List is full
}
//! get next index, then increase size by one
int idx = _buffer->size++;
memcpy(_buffer->list[idx].addr, adrs_ptr, BLE_GAP_ADDR_LEN);
_buffer->list[idx].rssiThreshold = rssi_threshold;
_buffer->counters[idx] = TDL_COUNTER_INIT;
// LOGi("Added [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
// _buffer->list[_freeIdx].addr[5], _buffer->list[_freeIdx].addr[4], _buffer->list[_freeIdx].addr[3], _buffer->list[_freeIdx].addr[2],
// _buffer->list[_freeIdx].addr[1], _buffer->list[_freeIdx].addr[0], rssi_threshold);
#ifdef PRINT_TRACKEDDEVICES_VERBOSE
LOGi("Added [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
adrs_ptr[5], adrs_ptr[4], adrs_ptr[3], adrs_ptr[2],
adrs_ptr[1], adrs_ptr[0], rssi_threshold);
#endif
if (memcmp(adrs_ptr, _buffer->list[idx].addr, BLE_GAP_ADDR_LEN)) {
LOGe("Failed to add address");
return false;
}
return true;
}
/** Returns the current time as posix time
* returns 0 when no time was set yet
*/
void Scheduler::setTime(uint32_t time) {
#ifdef PRINT_DEBUG
LOGi("Set time to %i", time);
#endif
_posixTimeStamp = time;
_rtcTimeStamp = RTC::getCount();
#if SCHEDULER_ENABLED==1
_scheduleList->sync(time);
#ifdef PRINT_DEBUG
_scheduleList->print();
#endif
#endif
}
static void pstorage_callback_handler(pstorage_handle_t * handle, uint8_t op_code, uint32_t result, uint8_t * p_data,
uint32_t data_len) {
// we might want to check if things are actually stored, by using this callback
if (result != NRF_SUCCESS) {
LOGd("ERR_CODE: %d (0x%X)", result, result);
APP_ERROR_CHECK(result);
if (op_code == PSTORAGE_LOAD_OP_CODE) {
LOGd("Error with loading data");
}
} else {
LOGi("Opcode %i executed (no error)", op_code);
}
}
void TrackedDeviceList::setTimeout(uint16_t counts) {
if (counts > TDL_COUNTER_INIT) {
return;
}
// Make sure that devices that are not nearby get the new threshold count
for (int i=0; i<getSize(); ++i) {
if (_buffer->counters[i] >= _timeoutCount) {
_buffer->counters[i] = counts;
}
}
_timeoutCount = counts;
LOGi("Set timeout count to %i", counts);
}
bool TrackedDeviceList::add(const uint8_t* adrs_ptr, int8_t rssi_threshold) {
for (int i=0; i<getSize(); ++i) {
// check if it is already in the list, then update
if (memcmp(adrs_ptr, _buffer->list[i].addr, BLE_GAP_ADDR_LEN) == 0) {
_buffer->list[i].rssiThreshold = rssi_threshold;
//_buffer->counters[i] = TDL_COUNTER_INIT; // Don't update counter
LOGi("Updated [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
adrs_ptr[5], adrs_ptr[4], adrs_ptr[3], adrs_ptr[2],
adrs_ptr[1], adrs_ptr[0], rssi_threshold);
return true;
}
}
if (getSize() >= TRACKDEVICES_MAX_NR_DEVICES) {
return false; // List is full
}
// get next index, then increase size by one
int idx = _buffer->size++;
memcpy(_buffer->list[idx].addr, adrs_ptr, BLE_GAP_ADDR_LEN);
_buffer->list[idx].rssiThreshold = rssi_threshold;
_buffer->counters[idx] = TDL_COUNTER_INIT;
// LOGi("Added [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
// _buffer->list[_freeIdx].addr[5], _buffer->list[_freeIdx].addr[4], _buffer->list[_freeIdx].addr[3], _buffer->list[_freeIdx].addr[2],
// _buffer->list[_freeIdx].addr[1], _buffer->list[_freeIdx].addr[0], rssi_threshold);
LOGi("Added [%02X %02X %02X %02X %02X %02X], rssi threshold: %d",
adrs_ptr[5], adrs_ptr[4], adrs_ptr[3], adrs_ptr[2],
adrs_ptr[1], adrs_ptr[0], rssi_threshold);
if (memcmp(adrs_ptr, _buffer->list[idx].addr, BLE_GAP_ADDR_LEN)) {
LOGi("Adding the address did not work, very likely due to memory issues!");
return false;
}
return true;
}
void Tracker::readTrackedDevices() {
buffer_ptr_t buffer;
uint16_t length;
_trackedDeviceList->getBuffer(buffer, length);
State::getInstance().get(STATE_TRACKED_DEVICES, buffer, _trackedDeviceList->getMaxLength());
_trackedDeviceList->resetTimeoutCounters();
if (!_trackedDeviceList->isEmpty()) {
#ifdef PRINT_DEBUG
LOGi("restored tracked devices (%d):", _trackedDeviceList->getSize());
_trackedDeviceList->print();
#endif
// inform listeners (like the indoor localisation service, i.e. trackedDeviceListCharacteristic)
EventDispatcher::getInstance().dispatch(EVT_TRACKED_DEVICES, buffer, length);
}
}
void Storage::getUint32(uint32_t value, uint32_t& target, uint32_t default_value) {
#ifdef PRINT_ITEMS
uint8_t* tmp = (uint8_t*)&value;
LOGi("raw value: %02X %02X %02X %02X", tmp[3], tmp[2], tmp[1], tmp[0]);
#endif
// check if value is equal to INT_MAX (FFFFFFFF) which means that memory is
// unnassigned and value has to be ignored
if (value == INT_MAX) {
LOGd("use default value");
target = default_value;
} else {
target = value;
LOGd("found stored value: %d", target);
}
}
void Storage::getUint16(uint32_t value, uint16_t& target, uint16_t default_value) {
#ifdef PRINT_ITEMS
uint8_t* tmp = (uint8_t*)&value;
LOGi("raw value: %02X %02X %02X %02X", tmp[3], tmp[2], tmp[1], tmp[0]);
#endif
// check if last byte is FF which means that memory is unnassigned
// and value has to be ignored
if (value & (0xFF << 3)) {
LOGd("use default value");
target = default_value;
} else {
target = value;
LOGd("found stored value: %d", target);
}
}
void Scheduler::readScheduleList() {
#if SCHEDULER_ENABLED==1
buffer_ptr_t buffer;
uint16_t length;
_scheduleList->getBuffer(buffer, length);
length = _scheduleList->getMaxLength();
State::getInstance().get(STATE_SCHEDULE, buffer, length);
if (!_scheduleList->isEmpty()) {
#ifdef PRINT_DEBUG
LOGi("restored schedule list (%d):", _scheduleList->getSize());
_scheduleList->print();
#endif
publishScheduleEntries();
}
#endif
}
void TrackedDeviceList::setTimeout(uint16_t counts) {
// if (counts > TDL_COUNTER_INIT) {
// return;
// }
//! Make sure that devices that are not nearby get the new threshold count
for (int i=0; i<getSize(); ++i) {
if (_buffer->counters[i] >= _timeoutCount) {
_buffer->counters[i] = counts;
}
}
_timeoutCount = counts;
#ifdef PRINT_TRACKEDDEVICES_VERBOSE
LOGi("Set timeout count to %i", counts);
#endif
}
/**
* Get incoming messages and perform certain actions.
*/
void MeshControl::process(uint8_t handle, void* p_data, uint16_t length) {
LOGi("Process incoming mesh message");
switch(handle) {
case EVENT_CHANNEL: {
// if (length != 1) {
// LOGi("wrong message, length: %d", length);
// break;
// }
mesh_message_t* msg = (mesh_message_t*) p_data;
LOGi("received event for:");
BLEutil::printArray(msg->targetAddress, BLE_GAP_ADDR_LEN);
LOGi("type: %s (%d), from ???", msg->evtMsg.type == EVT_POWER_ON ? "EVT_POWER_ON" : "EVT_POWER_OFF", msg->evtMsg.type);
// EventMeshPackage* msg = (EventMeshPackage*)p_data;
// LOGi("received %s (%d) event from ???", msg->evt == EVT_POWER_ON ? "EVT_POWER_ON" : "EVT_POWER_OFF", msg->evt);
break;
}
case PWM_CHANNEL: {
if (length != 1) {
LOGi("wrong message, length: %d", length);
break;
}
uint8_t* message = (uint8_t*)p_data;
uint32_t pwm_value;
uint8_t channel;
PWM::getInstance().getValue(channel, pwm_value);
if (*message == 1 && pwm_value == 0) {
LOGi("Turn lamp/device on");
PWM::getInstance().setValue(0, (uint8_t)-1);
} else if (*message == 0 && pwm_value != 0) {
LOGi("Turn lamp/device off");
PWM::getInstance().setValue(0, 0);
} else {
LOGi("skip pwm message");
}
break;
}
}
}
bool TrackedDeviceList::rem(const uint8_t* adrs_ptr) {
for (int i=0; i<getSize(); ++i) {
if (memcmp(adrs_ptr, _buffer->list[i].addr, BLE_GAP_ADDR_LEN) == 0) {
// Decrease size
_buffer->size--;
// Shift array
// TODO: Anne: order within the array shouldn't matter isn't it? so just copy the last item on slot i
for (int j=i; j<getSize(); ++j) {
memcpy(_buffer->list[j].addr, _buffer->list[j+1].addr, BLE_GAP_ADDR_LEN);
_buffer->list[j].rssiThreshold = _buffer->list[j+1].rssiThreshold;
_buffer->counters[j] = _buffer->counters[j+1];
// TODO: does this work too? might be faster..
//memcpy(&(_buffer->list[j]), &(_buffer->list[j+1]), sizeof(tracked_device_t));
}
LOGi("Removed [%02X %02X %02X %02X %02X %02X]", adrs_ptr[5],
adrs_ptr[4], adrs_ptr[3], adrs_ptr[2], adrs_ptr[1], adrs_ptr[0]);
return true;
}
}
return false; // Address is not in the list
}
int Compass::reportHeading() {
if (!_calibrated) return 0;
int heading, relative_heading;
// Heading is given in degrees away from the magnetic vector, increasing clockwise
heading = averageHeading();
// This gives us the relative heading with respect to the target angle
relative_heading = relativeHeading(heading, _targetHeading);
// char target_heading_[32];
// dtostrf(_targetHeading, 6, 3, target_heading_);
// char heading_[32];
// dtostrf(heading, 6, 3, heading_);
// char relative_heading_[32];
// dtostrf(relative_heading, 6, 3, relative_heading_);
//
// LOGi("_targetHeading: %s, heading: %s, relative_heading: %s", target_heading_, heading_, relative_heading_);
LOGi("targetHeading: %d, heading: %d, relative_heading: %d", _targetHeading, heading, relative_heading);
return 200;
}
void Tracker::startTracking() {
// if (!_stack) {
// LOGe(STR_ERR_FORGOT_TO_ASSIGN_STACK);
// return;
// }
if (!_tracking) {
//! Set to some value initially
_trackIsNearby = false;
//! Reset the counters
_trackedDeviceList->setTimeout(_timeoutCounts);
_tracking = true;
// if (!_stack->isScanning()) {
// LOGi(FMT_START, "tracking");
// _stack->startScanning();
// }
Timer::getInstance().start(_appTimerId, HZ_TO_TICKS(TRACKER_UPATE_FREQUENCY), this);
} else {
LOGi(FMT_ALREADY, "tracking");
}
}
void onCustom(message_t* msg) {
CustomCommands cmd = (CustomCommands)getType(msg);
switch(cmd) {
#ifdef MAZESOLVER
case INIT_MAZE: {
LOGi("mazSolver.init");
// Storage::setCurrentProgram(Prog_MazeSolver);
LOGi("Program MazeSolver selected");
// mazeSolver.init();
break;
}
case START_MAZE: {
LOGi("mazSolver.start");
// mazeSolver.start();
break;
}
case STOP_MAZE: {
LOGi("mazSolver.stop");
// mazeSolver.stop();
break;
}
case REPEAT_MAZE: {
LOGi("mazSolver.repeat");
// mazeSolver.repeat();
break;
}
case INIT_LINE_FOLLOWER: {
LOGi("linefollower.init");
// Storage::setCurrentProgram(Prog_LineFollower);
LOGi("Program LineFollower selected");
// mazeSolver.init();
break;
}
case START_LINE_FOLLOWER: {
LOGi("linefollower.start");
// mazeSolver.start();
break;
}
case STOP_LINE_FOLLOWER: {
LOGi("linefollower.stop");
// mazeSolver.stop();
break;
}
#endif
#ifdef LINE_FOLLOWER
case INIT_LINE_FOLLOWER: {
LOGi("linefollower.init");
// linefollower.init();
break;
}
case START_LINE_FOLLOWER: {
LOGi("linefollower.start");
// linefollower.start();
break;
}
case STOP_LINE_FOLLOWER: {
LOGi("linefollower.stop");
// linefollower.stop();
break;
}
#endif
#ifdef USE_COMPASS
case CALIBRATE_COMPSS: {
LOGi("compass.calibrate");
compass.calibrate();
break;
}
case INIT_HEADING: {
LOGi("compass.reset");
calibrateHeading();
break;
}
case TURN_DEG: {
LOGi("compass.turn");
turndegree_payload* payload = (turndegree_payload*) msg->payload;
turnDegrees(payload->angle);
break;
}
case SET_HEADING: {
LOGi("setAbsAngle");
turndegree_payload* payload = (turndegree_payload*) msg->payload;
setTargetHeading(payload->angle);
break;
}
#endif
#ifdef SUMO
case START_SUMO: {
LOGi("sumo.start");
// Storage::setCurrentProgram(Prog_Sumo);
LOGi("Program Sumo selected");
// sumo.start();
break;
}
case STOP_SUMO: {
LOGi("sumo.stop");
// sumo.stop();
break;
}
#endif
// case SET_PROGRAM: {
// program_payload* payload = (program_payload*)msg->payload;
// _currentProgram = (Program)payload->program;
// Storage::setCurrentProgram(_currentProgram);
// break;
// }
case SET_WHITE_LINES: {
whitelines_payload* payload = (whitelines_payload*)msg->payload;
_whiteLines = (bool)payload->whiteLines;
Storage::setWhiteLines(_whiteLines);
break;
}
}
}
void TrackedDevice::print() const {
LOGi("[%02X %02X %02X %02X %02X %02X]\trssi: %d", _buffer->addr[5],
_buffer->addr[4], _buffer->addr[3], _buffer->addr[2], _buffer->addr[1],
_buffer->addr[0], _buffer->rssiThreshold);
}
