示例#1
0
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;
}
示例#2
0
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;
}
示例#3
0
// 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;
}
示例#4
0
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);
}
示例#5
0
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;
}
示例#6
0
// 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); }
   }
}
示例#7
0
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);
         }
      }
   }
}
示例#8
0
static void test_get_heading(void)
{
    int heading;
    heading = get_heading();
    TEST_ASSERT(heading == 180);
}
示例#9
0
template <typename State> auto get_heading(const Particle<State> &p) {
    return get_heading(p.state);
}
示例#10
0
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);
	}
}