Merge branch 'master' of https://github.com/ashlaban/dw1000-driver

apps/decawave-range/dw-range-anchor.c

+/**
+ * \file 	Decawace uwb demo application
+ * \author	Kim Albertsson
+ *
+ * \note If you get a HPDWARN in status reg you are probably taking to long 
+ * time to create a delayed send. Increase MESSAGE_DELAY_MS or optimise 
+ * critical path for creating packet.
+ */
+
+/*===========================================================================*/
+/*================================ Includes =================================*/
+
+#include <stdio.h> /* For printf() */
+#include <stdint.h>
+#include "string.h"
+#include <stddef.h>
+
+#include "udelay.h"
+
+#include "dw1000-base.h"
+#include "dwFrameTypes.h"
+#include "dw-range.h"
+
+#include "contiki.h"
+// #include "contiki-net.h"
+// #include "rest-engine.h"
+
+/*===========================================================================*/
+/*================================ Defines ==================================*/
+
+#define MESSAGE_DELAY_MS 10.0f
+#define DEVICE_TIME_UNITS (1.0/499.2e6/128.0)
+#define ANTENNA_DELAY ((515.0 / 2.0) * 1e-9 / (1.0/499.2e6/128.0))
+
+#define MASK_40_BIT 0x000000FFFFFFFFFFULL
+
+/*===========================================================================*/
+/*========================== Public Declarations ============================*/
+
+/**
+ * \brief Keeps track of all variables needed for a distance measurement.
+ */
+typedef struct
+{
+	uint64_t tsp; /**< Poll     sent     time */
+	uint64_t trp; /**< Poll     received time */
+	uint64_t tsr; /**< Response sent     time */
+	uint64_t trr; /**< Response received time */
+	uint64_t tsf; /**< Final    sent     time */
+	uint64_t trf; /**< Final    received time */
+	
+	int64_t  ttrt; /**< Round trip time as seen by tag.    */
+	int64_t  tart; /**< Round trip time as seen by anchor. */
+	int64_t  trt;  /**< Estimated round trip time.         */
+	int64_t  tof;  /**< Estimated Time of Flight. [~15.65ps per tick] */
+	double   s;    /**< Estimated distance                 */
+
+} distance_meas_t;
+
+typedef struct
+{
+	uint32_t num_meas;
+	uint32_t num_errors;
+
+	// Measurements
+	float accumulated_distance;
+	float max;
+	float min;
+	float stddev;
+
+	// Quality
+	float rx_power;
+	float fp_power;
+	float noise_level;
+	float fp_ampl;
+	
+	// Sensors
+	float temperature;
+	float voltage;
+} statistics_t;
+
+extern dw1000_base_driver dw1000;
+extern struct process * interrupt_handler_callback;
+
+static distance_meas_t dist_meas;
+static statistics_t    stats;
+
+// For coap service
+float lastMeasuredDistance = 0.f;
+float accumulated_distance = 0.f;
+
+// static void res_get_handler(void* request, void* response, uint8_t *buffer, uint16_t preferred_size, int32_t *offset);
+// RESOURCE(res_dw_range, "title=\"Distance: ?len=0..\";rt=\"Text\"", res_get_handler, NULL, NULL, NULL);
+
+/*---------------------------------------------------------------------------*/
+PROCESS(dw_main_process           , "Main process");
+PROCESS(dw_anchor_process         , "Anchor process (dw1000)");
+AUTOSTART_PROCESSES(&dw_main_process);
+/*---------------------------------------------------------------------------*/
+
+/*===========================================================================*/
+/*============================ Public Functions =============================*/
+
+/**
+ * \brief Used to convert from raw measurements to real world distances.
+ *
+ * \details
+ *
+ * \todo Present error bounds on this function.
+ * 
+ * @param  distance raw measurement
+ * @return          estimated distance
+ */
+static float distance_correction( float measurement )
+{
+	// return distance*0.8719 - 32.8600;
+	// return distance*0.9876 - 37.0932;
+	// return distance*0.9430 - 72.4511;
+	return measurement;
+}
+
+void print_statistics( statistics_t * stats )
+{
+	float avg = stats->accumulated_distance/stats->num_meas;
+	printf("========================================\n");
+	printf("Measurement no. %i\n", stats->num_meas);
+	printf("--- Data -------------------------------\n");
+	printf("Avg: %f\n", avg);
+	printf("Max: %f\n", stats->max);
+	printf("Min: %f\n", stats->min);
+	printf("Std dev: %f\n", stats->stddev);
+	printf("--- Quality ----------------------------\n");
+	stats->rx_power    = dw_get_rx_power();
+	stats->fp_power    = dw_get_fp_power();
+	stats->noise_level = dw_get_noise_level();
+	stats->fp_ampl     = dw_get_fp_ampl();
+	printf("rx_power   : %f\n", stats->rx_power);
+	printf("fp_power   : %f\n", stats->fp_power);
+	printf("noise_level: %f\n", stats->noise_level);
+	printf("fp_ampl    : %f\n", stats->fp_ampl);
+	printf("--- Sensors ----------------------------\n");
+	stats->temperature = dw_get_temperature(DW_ADC_SRC_LATEST);
+	stats->voltage     = dw_get_voltage(DW_ADC_SRC_LATEST);
+	printf("temperature: %f\n", stats->temperature);
+	printf("voltage    : %f\n", stats->voltage);
+	printf("--- Diagnostics ------------------------\n");
+	printf("num_packets_rx   : %i\n", packet_stats.num_packets_rx);
+	printf("num_packets_tx   : %i\n", packet_stats.num_packets_tx);
+	printf("num_packet_errors: %i\n", packet_stats.num_errors);
+	printf("========================================\n");
+}
+
+void reset_statistics( statistics_t * stats )
+{
+	stats->num_meas = 0;
+	stats->accumulated_distance = 0.f;
+	stats->max    = -1e9;
+	stats->min    =  1e9;
+	stats->stddev = 0.f;
+	stats->num_errors = 0;
+}
+
+
+/*---------------------------------------------------------------------------*/
+PROCESS_THREAD(dw_main_process, ev, data)
+{
+	PROCESS_BEGIN();
+
+	// Init coap
+	// rest_init_engine();
+	// rest_activate_resource(&res_dw_range, "test/distance");
+
+	// Init dw1000
+	process_start(&dw_interrupt_callback_proc, NULL);
+
+	dw_init();
+	dw_conf_print();
+	interrupt_handler_callback = &dw_anchor_process;
+	reset_statistics( &stats );
+	process_start(&dw_anchor_process, NULL);
+
+	// Start code proper
+	while(1)
+	{
+		PROCESS_WAIT_EVENT();
+	}
+ 
+  PROCESS_END();
+}
+/*---------------------------------------------------------------------------*/
+PROCESS_THREAD(dw_anchor_process, ev, data)
+{
+	PROCESS_BEGIN();
+
+	printf("Starting Anchor\n");
+	// For precise measurements this should be calibrated per device
+	dw_set_antenna_delay( ANTENNA_DELAY );
+
+	static ranging_state_t ranging_state = IDLE;
+	static uint64_t destAddr = 0ULL;
+
+	dw_frame_blink_t   frameBlink;
+	dw_frame_range_t   frameRangePoll;
+	dw_message_poll_t  msgPoll;
+	dw_frame_range_t   frameRangeFinal;
+	dw_message_final_t msgFinal;
+	while( 1 )
+	{
+		if (ranging_state == IDLE)
+		{
+			// This would be in the main switch as 
+			// case IDLE:
+			// if contiki multithreading would have allowed it.
+			dw1000_rx_conf_t rx_conf;
+			rx_conf.is_delayed = 0;
+			rx_conf.timeout = 0xFFFF;
+			dw_conf_rx( &rx_conf );	
+			dw_receive( DW_TRANCEIVE_ASYNC );
+			PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_CONTINUE);
+			ranging_state = *(ranging_state_t *)data;
+	
+			// printf("State: %i\n", ranging_state);
+			// printf("ranging_state: %i\n", ranging_state);
+		} 
+
+		uint32_t msgLen;
+		uint64_t dx_timestamp;
+		switch (ranging_state)
+		{
+			case MEASUREMENT_ADDED:
+				ranging_state = IDLE;
+				
+				if (stats.num_meas % 100  != 0) { break; }
+				print_statistics(&stats);
+				if (stats.num_meas != 1000) { break; }
+
+				// For easy calibration of distance_correction().
+				// Now you can take one sample and then reset device.
+				// Remove loop when calibration is done.
+				while(1){};
+
+				// Reset statistics.
+				reset_statistics( &stats );
+				printf("Reset.\n");
+
+				break;
+
+			case BLINK_RECEIVED:
+				
+				parseFrameBlink(dw1000.p_receive_buffer, dw1000.receive_buffer_len, &frameBlink);
+				destAddr = frameBlink.tag_id;
+
+				dx_timestamp = dw_get_rx_timestamp() + dw_ms_to_device_time( MESSAGE_DELAY_MS );
+
+				sendRangeInit( destAddr, (uint16_t)MESSAGE_DELAY_MS, dx_timestamp );
+				printf("Sent ranging init\n");
+				ranging_state = IDLE;
+				break;
+
+			case POLL_RECEIVED:
+				dist_meas.trp = dw_get_rx_timestamp();
+				dx_timestamp = dist_meas.trp + dw_ms_to_device_time( MESSAGE_DELAY_MS );
+				//printArray(dw1000.p_receive_buffer, dw1000.receive_buffer_len);
+
+				msgLen = dw1000.receive_buffer_len - DW_FRAME_RANGE_LEN - DW_FRAME_FCS_LEN;
+				parseFrameRange( dw1000.p_receive_buffer, dw1000.receive_buffer_len, &frameRangePoll );
+				parseMessagePoll( frameRangePoll.pPayload, msgLen, &msgPoll );
+
+				// TODO: Keep track of paired tags
+				if (destAddr == 0x0ULL)
+				{
+					destAddr = frameRangePoll.src_addr;
+				}
+
+				sendResponse( destAddr, &dist_meas.tsr, dx_timestamp );
+				// printArray( dw1000.p_receive_buffer, dw1000.receive_buffer_len );
+				ranging_state = IDLE;
+				break;
+
+			case FINAL_RECEIVED:
+				dist_meas.trf = dw_get_rx_timestamp();
+
+				msgLen = dw1000.receive_buffer_len - DW_FRAME_RANGE_LEN - DW_FRAME_FCS_LEN;
+				parseFrameRange( dw1000.p_receive_buffer, dw1000.receive_buffer_len, &frameRangeFinal );
+				parseMessageFinal( frameRangeFinal.pPayload, msgLen, &msgFinal );
+				
+				memcpy( &dist_meas.tsp, msgFinal.poll_msg_tx_timestamp , 5 );
+				memcpy( &dist_meas.trr, msgFinal.resp_msg_rx_timestamp , 5 );
+				memcpy( &dist_meas.tsf, msgFinal.final_msg_tx_timestamp, 5 );
+
+				uint64_t tsp = dist_meas.tsp;
+				uint64_t trp = dist_meas.trp;
+				uint64_t tsr = dist_meas.tsr;
+				uint64_t trr = dist_meas.trr;
+				uint64_t tsf = dist_meas.tsf;
+				uint64_t trf = dist_meas.trf;
+				dist_meas.ttrt  = ((trr - tsp) & MASK_40_BIT) - ((tsr - trp) & MASK_40_BIT);
+				dist_meas.tart  = ((trf - tsr) & MASK_40_BIT) - ((tsf - trr) & MASK_40_BIT);
+				dist_meas.trt   = ((dist_meas.tart & MASK_40_BIT) + (dist_meas.ttrt & MASK_40_BIT)) >> 1;
+				dist_meas.tof   = dist_meas.trt >> 1;
+				dist_meas.s     = 3e8 * 15.65e-12 * (double)dist_meas.tof;
+				dist_meas.s     = distance_correction(dist_meas.s);
+				if (dist_meas.s > 200.0f)
+				{
+					/* 
+						BUG: sometimes we get a really large value... 
+						This is possibly fixed now... (has to do with delayed 
+						send ignoring lowest 9 bits.)
+					*/
+					++stats.num_errors;
+					break;
+				}
+
+				stats.accumulated_distance += dist_meas.s;
+				stats.max = (dist_meas.s > stats.max) ? dist_meas.s : stats.max;
+				stats.min = (dist_meas.s < stats.min) ? dist_meas.s : stats.min;
+				++stats.num_meas;
+				ranging_state = MEASUREMENT_ADDED;
+				break;
+
+			case ERROR:
+			default   :
+				++packet_stats.num_errors; // TODO: bad encapsulation, change to software errors.
+				ranging_state = IDLE;
+				break;
+		}
+	}
+
+	PROCESS_END();
+}
+/*---------------------------------------------------------------------------*/
+// static void
+// res_get_handler(void* request, void* response, uint8_t *buffer, uint16_t preferred_size, int32_t *offset)
+// {
+// 	/**
+// 	 * \note This should be working... But it was a long time ago I tested it 
+// 	 * so it probably won't.
+// 	 */
+	
+// 	// Set up response payload.
+// 	static const uint32_t length = 21;
+// 	static char message[128];
+// 	sprintf( message, "Distance to tag: %4f", lastMeasuredDistance );
+// 	memcpy(buffer, message, length);
+
+// 	// Set up response packet
+// 	REST.set_header_content_type(response, REST.type.TEXT_PLAIN); /* text/plain is the default, hence this option could be omitted. */
+// 	REST.set_header_etag(response, (uint8_t *) &length, 1);
+// 	REST.set_response_payload(response, buffer, length);
+// }
\ No newline at end of file

apps/decawave-range/dw-range-tag.c

+/**
+* \file
+ * 	Decawace uwb demo application
+ * \author
+ *	Kim Albertsson
+ */
+
+// Tested with mulle board 145
+
+#include "contiki.h"
+
+#include <stdio.h> /* For printf() */
+#include <stdint.h>
+#include <stddef.h>
+
+#include "udelay.h"
+
+#include "dw1000-base.h"
+#include "dwFrameTypes.h"
+#include "dw-range.h"
+
+#define DEVICE_TIME_UNITS (1.0/499.2e6/128.0)
+#define ANTENNA_DELAY ((515.0 / 2.0) * 1e-9 / (1.0/499.2e6/128.0))
+
+typedef enum
+{
+	UNPAIRED = 0,
+	PAIRED
+} tag_state_t;
+
+extern struct process * interrupt_handler_callback;
+
+int error;
+
+/*---------------------------------------------------------------------------*/
+PROCESS(dw_main_process           , "Main process");
+PROCESS(dw_tag_process            , "Tag process (dw1000)");
+AUTOSTART_PROCESSES(&dw_main_process);
+/*---------------------------------------------------------------------------*/
+PROCESS_THREAD(dw_main_process, ev, data)
+{
+	PROCESS_BEGIN();
+
+	// Init dw1000
+	process_start(&dw_interrupt_callback_proc, NULL);
+
+	dw_init();
+	dw_conf_print();
+	interrupt_handler_callback = &dw_tag_process;
+	process_start(&dw_tag_process, NULL);
+
+	// Start code proper
+	while(1)
+	{
+		PROCESS_WAIT_EVENT();
+	}
+ 
+  PROCESS_END();
+}
+/*---------------------------------------------------------------------------*/
+PROCESS_THREAD(dw_tag_process, ev, data)
+{
+	PROCESS_BEGIN();
+
+	static uint64_t dest_addr = 0ULL;
+	static uint16_t responseDelay;
+
+	static tag_state_t     tag_state     = UNPAIRED;
+	static ranging_state_t ranging_state = IDLE;
+
+	printf("Starting Tag\n");
+	// Setting antenna delay
+	dw_set_antenna_delay( ANTENNA_DELAY );
+
+	// Enable rx timeout
+	dw_set_rx_timeout( 0xFFFF );
+	printf("Timeout: %f ms.\n", (float)dw_get_rx_timeout()/1000.0f);
+	dw_enable_rx_timeout();
+
+	// Configure rx
+	static dw1000_rx_conf_t rx_conf;
+	rx_conf.is_delayed = 0;
+	rx_conf.timeout = 0xFFFF;
+	dw_conf_rx( &rx_conf );
+	dw_receive( DW_TRANCEIVE_ASYNC );
+
+	static uint64_t tsp;
+	static uint64_t trr;
+	static uint64_t tsf;
+	static uint64_t dx_timestamp;
+	static uint32_t msgLen;
+	static dw_frame_range_t        frameRange;
+	static dw_message_range_init_t msgRangeInit;
+	static dw_message_response_t   msgResponse;
+	// static struct etimer et;
+	// etimer_set(&et, CLOCK_SECOND);
+	while (1)
+	{	
+		PROCESS_WAIT_EVENT();
+		// printf("Received Event. %i.\n", ev);
+		// printf("Receive  state. %i.\n", ranging_state);
+		//else
+		{
+			ranging_state = *(ranging_state_t *)data;
+			switch (ranging_state)
+			{
+				case RANGE_INIT_RECEIVED:
+					// printf("Range init received.\n");
+					msgLen = dw1000.receive_buffer_len - DW_FRAME_RANGE_LEN - DW_FRAME_FCS_LEN;
+					parseFrameRange(dw1000.p_receive_buffer, dw1000.receive_buffer_len, &frameRange);
+					parseMessageRangeInit(frameRange.pPayload, msgLen, &msgRangeInit);
+
+					dest_addr     =  frameRange.dest_addr;
+					responseDelay = msgRangeInit.response_delay_ms;
+
+					tag_state = PAIRED;
+					break;
+
+				case RESPONSE_RECEIVED:
+					//printf("Response Received.\n");
+					trr = dw_get_rx_timestamp();
+					dx_timestamp = trr + dw_ms_to_device_time( (float)responseDelay );
+
+					msgLen = dw1000.receive_buffer_len - DW_FRAME_RANGE_LEN - DW_FRAME_FCS_LEN;
+					parseFrameRange(dw1000.p_receive_buffer, dw1000.receive_buffer_len, &frameRange);
+					parseMessageResponse(frameRange.pPayload, msgLen, &msgResponse);
+					
+					// send final
+					sendFinal( dest_addr, tsp, trr, &tsf, dx_timestamp );
+					printf("Final sent.\n");
+					break;
+
+				case ERROR:
+				default:
+					//printf("Error.\n");
+					break;
+			}
+
+			ranging_state = IDLE;
+		}		
+
+		if ( ranging_state == IDLE /*ev == PROCESS_EVENT_TIMER*/ )
+		{
+			// send either poll or blink
+			if (tag_state == UNPAIRED && ranging_state == IDLE)
+			{
+				// send blink
+				//printf("Sending Blink.\n");
+				sendBlink();
+				// Listen for answer
+				ranging_state = RECEIVING;
+				//rx_conf.is_delayed = 1;
+				//rx_conf.dx_timestamp = dw_get_tx_timestamp() + dw_ms_to_device_time(4.8f);
+				dw_conf_rx( &rx_conf );
+				dw_receive( DW_TRANCEIVE_ASYNC );
+			}
+			else if (tag_state == PAIRED && ranging_state == IDLE)
+			{
+				// send poll
+				printf("Sending Poll.\n");
+				sendPoll( dest_addr, &tsp );
+
+				// Listen for answer
+				ranging_state = RECEIVING;
+				//rx_conf.is_delayed = 1;
+				//rx_conf.dx_timestamp = dw_get_tx_timestamp() + dw_ms_to_device_time(responseDelay-0.2);
+				dw_conf_rx( &rx_conf );
+				dw_receive( DW_TRANCEIVE_ASYNC );
+			}
+			// etimer_reset(&et);
+		}
+	}
+
+	PROCESS_END();
+}
+/*---------------------------------------------------------------------------*/
\ No newline at end of file