void use_rnb_data(uint8_t power)
{
uint8_t brightness_matrix[6][6];
pack_measurements_into_matrix(brightness_matrix);;
//print_brightness_matrix(brightness_matrix);
uint8_t emitter_total[6];
uint8_t sensor_total[6];
fill_S_and_T(brightness_matrix, sensor_total, emitter_total);
float bearing = get_bearing(sensor_total);
float heading = get_heading(emitter_total, bearing);
float initial_range = get_initial_range_guess(bearing, heading, sensor_total, emitter_total, brightness_matrix, power);
float range = range_estimate(brightness_matrix, initial_range, bearing, heading, power);
//TODO: Actually incorporate the ID #.
//rnb temp = {.range = range, .bearing = bearing, .heading = heading, .id_number = 0};
//printf("Bearing: %f | Heading: %f | Initial Range Guess: %f | Range: %f\r\n", rad_to_deg(bearing), rad_to_deg(heading), initial_range, range);
last_good_rnb.range = range;
last_good_rnb.bearing = bearing;
last_good_rnb.heading = heading;
last_good_rnb.brightness_matrix_ptr = brightness_matrix;
last_good_rnb.id_number = last_command_source_id;
rnb_updated=1;
}
static int8_t proc_heading(node_id_t requester,
uint8_t *req_buf, uint8_t req_len,
uint8_t *reply_buf, uint8_t reply_size,
uint8_t *reply_len)
{
int16_t heading;
int8_t rc;
LOG("heading req from "); LOGP("%u\r\n", requester);
if (reply_size < RPC_HEADING_REPLY_LEN) {
LOG("WARN: reply buf too small: ");
LOGP("%u/%u\r\n", reply_size, RPC_HEADING_REPLY_LEN);
return NRK_ERROR;
}
if (!have_compass) {
LOG("WARN: rpc failed: no compass\r\n");
return NRK_ERROR;
}
rc = get_heading(&heading);
if (rc != NRK_OK) {
LOG("WARN: failed to get heading\r\n");
return rc;
}
reply_buf[RPC_HEADING_REPLY_HEADING_OFFSET] = heading & 0xFF;
reply_buf[RPC_HEADING_REPLY_HEADING_OFFSET + 1] = heading >> 8;
*reply_len = RPC_HEADING_REPLY_HEADING_LEN;
memcpy(reply_buf + RPC_HEADING_REPLY_MAG_OFFSET, mag_uT, sizeof(mag_uT));
*reply_len += sizeof(mag_uT);
return NRK_OK;
}
// Whole function to get angles from accelerometer + compass
vectorMe accelcompass_angle_acquisition(vectorMe a, vectorMe m)
{
// Vector p should be defined as pointing forward, parallel to the ground, with coordinates {X, Y, Z}.
vectorMe p = {0, -1, 0};
vectorMe angles;
float heading;
//compass_read_data(&a, &m);
heading = get_heading(&a, &m, &p);
accel_g(&a);
angles = accel_angle(a);
angles.z= heading;
return angles;
}
static void periodic_heading_process(bool enabled,
nrk_time_t *next_event, nrk_sig_mask_t *wait_mask)
{
int16_t heading;
int8_t rc;
nrk_time_t now;
if (!enabled)
return;
rc = get_heading(&heading);
OUT("heading: ");
show_heading(rc == NRK_OK ? &heading : NULL);
nrk_time_get(&now);
nrk_time_add(next_event, now, heading_period);
}
int8_t cmd_head(uint8_t argc, char **argv)
{
int16_t heading;
int8_t rc = NRK_OK, rc_node = NRK_OK;
#if ENABLE_COMPASS_RPC
node_id_t node;
uint8_t i;
#endif
if (!(argc == 1 || argc >= 2)) {
OUT("usage: head [<node>...]\r\n");
return NRK_ERROR;
}
if (have_compass) {
rc_node = get_heading(&heading);
if (rc_node != NRK_OK)
rc = rc_node;
}
OUT("node: heading [x y z] (uT)\r\n");
if (have_compass) {
OUTP("%d: ", this_node_id);
show_heading(rc == NRK_OK ? &heading : NULL);
}
if (argc >= 2) {
#if ENABLE_COMPASS_RPC
for (i = 1; i < argc; ++i) {
node = atoi(argv[i]);
rc_node = rpc_heading(node, &heading);
if (rc_node != NRK_OK)
rc = rc_node;
OUTP("%d: ", node);
show_heading(rc == NRK_OK ? &heading : NULL);
}
#else
OUT("ERROR: COMPASS_RPC not linked in\r\n");
return NRK_ERROR;
#endif
}
return rc;
}
// Object Observer Interface
void
dmz::QtPluginCanvasObject::create_object (
const UUID &Identity,
const Handle ObjectHandle,
const ObjectType &Type,
const ObjectLocalityEnum Locality) {
Config data;
ObjectType currentType (Type);
if (_find_config_from_type (data, currentType)) {
String name (currentType.get_name ());
name << "." << ObjectHandle;
ObjectStruct *os (new ObjectStruct (ObjectHandle));
os->item->setData (QtCanvasObjectNameIndex, name.get_buffer ());
ObjectModule *objMod (get_object_module ());
if (objMod) {
Vector pos;
Matrix ori;
objMod->lookup_position (ObjectHandle, _defaultAttributeHandle, pos);
objMod->lookup_orientation (ObjectHandle, _defaultAttributeHandle, ori);
os->posX = pos.get_x ();
os->posY = pos.get_z ();
os->heading = get_heading (ori);
os->update ();
}
_objectTable.store (os->ObjHandle, os);
if (_canvasModule) { _canvasModule->add_item (os->ObjHandle, os->item); }
}
}
void
dmz::QtPluginCanvasObject::update_object_orientation (
const UUID &Identity,
const Handle ObjectHandle,
const Handle AttributeHandle,
const Matrix &Value,
const Matrix *PreviousValue) {
if (AttributeHandle == _defaultAttributeHandle) {
ObjectStruct *os (_objectTable.lookup (ObjectHandle));
if (os) {
os->heading = get_heading (Value);
if (!_updateTable.lookup (ObjectHandle)) {
_updateTable.store (ObjectHandle, os);
}
}
}
}
static void test_get_heading(void)
{
int heading;
heading = get_heading();
TEST_ASSERT(heading == 180);
}
template <typename State> auto get_heading(const Particle<State> &p) {
return get_heading(p.state);
}
int main()
{
DDRC = 0; // all inputs
PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL, respectively
TWSR = 0; // clear bit-rate prescale bits
TWBR = 17; // produces an SCL frequency of 400 kHz with a 20 MHz CPU clock speed
clear();
//enable accelerometer
i2c_start();
i2c_write_byte(0x30); // write acc
i2c_write_byte(0x20); // CTRL_REG1_A
i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
i2c_stop();
//enable magnetometer
i2c_start();
i2c_write_byte(0x3C); // write mag
i2c_write_byte(0x02); // MR_REG_M
i2c_write_byte(0x00); // continuous conversion mode
i2c_stop();
vector a, m;
char ribbon_segment[8];
unsigned char button;
enum calibration_mode mode = CAL_NONE;
vector cal_m_max = {0, 0, 0};
vector cal_m_min = {0, 0, 0};
while(1)
{
// see if a button was pressed to enable calibration mode
// each button displays max and min measurements for one axis:
// top = X, middle = Y, bottom = Z
// if any button is pressed a second time, return to normal mode
button = get_single_debounced_button_press(ANY_BUTTON);
if (button & TOP_BUTTON)
{
if (mode != CAL_X)
mode = CAL_X;
else
mode = CAL_NONE;
}
else if (button & MIDDLE_BUTTON)
{
if (mode != CAL_Y)
mode = CAL_Y;
else
mode = CAL_NONE;
}
else if (button & BOTTOM_BUTTON)
{
if (mode != CAL_Z)
mode = CAL_Z;
else
mode = CAL_NONE;
}
if (mode == CAL_NONE) // normal mode - display heading and compass ribbon
{
vector a_avg = {0,0,0}, m_avg = {0,0,0};
// take 5 acceleration and magnetic readings and average them
for(int i = 0; i < 5; i++)
{
read_data(&a, &m);
a_avg.x += a.x;
a_avg.y += a.y;
a_avg.z += a.z;
m_avg.x += m.x;
m_avg.y += m.y;
m_avg.z += m.z;
}
a_avg.x /= 5;
a_avg.y /= 5;
a_avg.z /= 5;
m_avg.x /= 5;
m_avg.y /= 5;
m_avg.z /= 5;
int heading = get_heading(&a_avg, &m_avg, &p);
// select the portion of the ribbon to display
strlcpy(ribbon_segment, &ribbon[(heading + 5) / 10], 8);
clear();
print_long(heading);
lcd_goto_xy(4, 0);
print_character('v'); // center indicator
lcd_goto_xy(1, 1);
print(ribbon_segment); // ribbon segment
}
else // calibration mode - record and display max/min measurements
{
read_data_raw(&a, &m);
if (m.x < cal_m_min.x) cal_m_min.x = m.x;
if (m.x > cal_m_max.x) cal_m_max.x = m.x;
if (m.y < cal_m_min.y) cal_m_min.y = m.y;
if (m.y > cal_m_max.y) cal_m_max.y = m.y;
if (m.z < cal_m_min.z) cal_m_min.z = m.z;
if (m.z > cal_m_max.z) cal_m_max.z = m.z;
clear();
switch (mode)
{
case CAL_X:
print("Xmax:");
print_long(cal_m_max.x);
lcd_goto_xy(0, 1);
print("min:");
print_long(cal_m_min.x);
break;
case CAL_Y:
print("Ymax:");
print_long(cal_m_max.y);
lcd_goto_xy(0, 1);
print("min:");
print_long(cal_m_min.y);
break;
default:
print("Zmax:");
print_long(cal_m_max.z);
lcd_goto_xy(0, 1);
print("min:");
print_long(cal_m_min.z);
break;
}
}
delay_ms(100);
}
}
