Commit fdafdf7b authored by Ian McInerney's avatar Ian McInerney

Localization computation block

parent a64f6900
......@@ -4,9 +4,15 @@
#include <stdbool.h>
#include "crtp.h"
// These are various codes used to configure the computation framework
enum comp_config_codes {
COMP_CONFIG_ENABLE = 0,
COMP_CONFIG_FUNC,
};
enum Computation_channels {
COMP_CHAN_QUERY = 0,
COMP_CHAN_COMPUTE,
COMP_CHAN_COMPUTE_DATA,
COMP_CHAN_CONFIG,
};
......
#ifndef COMPUATION_FUNC_H_
#define COMPUATION_FUNC_H_
#include "stabilizer_types.h"
typedef enum computationID {
COMPUTATION_NONE = 0,
COMPUTATION_LOCALIZATION,
......@@ -9,8 +11,10 @@ typedef enum computationID {
extern computationID computationType;
void computation_configReceived( CRTPPacket *pk, CRTPPacket *p );
void computation_packetReceived( CRTPPacket *pk, CRTPPacket *p );
void computation_packetReceived( CRTPPacket *pk );
void computation_processing();
void computation_algorithm_init();
void computation_update_state( state_t *state, sensorData_t *sensor );
#endif
\ No newline at end of file
......@@ -2,9 +2,24 @@
#define COMPUTATION_LOCALIZATION_H_
typedef struct computation_localization_config_packet {
uint8_t nodeNumber;
uint8_t numTotalNodes;
float stepSize;
uint8_t anchor;
float k1;
float k2;
float k3;
} computation_localization_config_packet;
typedef struct computation_localization_shared_node_data {
float estimatedX;
float estimatedY;
float estimatedZ;
float alpha;
float beta;
float gamma;
} computation_localization_node_data;
void computation_localization_newDistance(int anchor, float newRange);
#endif
\ No newline at end of file
......@@ -20,12 +20,6 @@ static CRTPPacket p;
static bool computationEnabled = false;
// These are various codes used to configure the computation framework
enum comp_config_codes {
COMP_CONFIG_ENABLE = 0,
COMP_CONFIG_FUNC,
};
/**
* Callback for the computation CRTP packet
*/
......@@ -38,12 +32,20 @@ static void Computation_crtpCB(CRTPPacket* pk) {
case COMP_CHAN_QUERY:
// The quadcopter is being queried as to the type of computation and if it exists
p.data[0] = computationType;
p.channel = COMP_CHAN_QUERY;
// Pass the return packet for sending
crtpSendPacket(&p);
break;
case COMP_CHAN_CONFIG:
switch (p.data[0]) {
p.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_CONFIG);
p.size = 1;
p.data[0] = 1;
switch (pk->data[0]) {
case COMP_CONFIG_ENABLE:
// Toggle the computation on or off
computationEnabled = p.data[1];
computationEnabled = pk->data[1];
if (computationEnabled) {
DEBUG_PRINT("Enabled Computation\n");
} else {
......@@ -55,22 +57,24 @@ static void Computation_crtpCB(CRTPPacket* pk) {
// Pass the config packet into the callback
computation_configReceived(pk, &p);
}
// Pass the return packet for sending
crtpSendPacket(&p);
break;
case COMP_CHAN_COMPUTE:
case COMP_CHAN_COMPUTE_DATA:
// Call the function to process the received data
computation_packetReceived(pk, &p);
computation_packetReceived(pk);
break;
}
// Pass the return packet for sending
crtpSendPacket(&p);
}
void Computation_Task() {
DEBUG_PRINT("Started Computation Task\n");
uint32_t lastWakeTime;
lastWakeTime = xTaskGetTickCount ();
lastWakeTime = xTaskGetTickCount();
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
......
#include "computation.h"
#include "computation_func.h"
#include "computation_localization.h"
#include "debug.h"
#include "locodeck.h"
#include "crtp.h"
#include "stabilizer_types.h"
#include "task.h"
// Because memcpy is in string.h for some reason...
#include <string.h>
#define MAX_NUM_NODES 10
computationID computationType = COMPUTATION_LOCALIZATION;
// This is the data local to this host
static float a;
static float mu;
static computation_localization_node_data localData;
static point_t localPosition;
static CRTPPacket dataPacket;
static float ranges[LOCODECK_NR_OF_ANCHORS];
static uint8_t anchorNumber;
static float twok1;
static float twok2;
static float twok3;
static float stepSize;
static uint8_t numNodes;
static uint8_t nodeNumber;
// This is the data from the neighbors
static computation_localization_node_data nodeData[MAX_NUM_NODES];
static uint8_t useNode[MAX_NUM_NODES];
static uint32_t lastNodeTime[MAX_NUM_NODES];
/*
* This function will configure the algorithm to be in its default state
*/
void computation_algorithm_init() {
// Zero out the stuff for this node
a = 0;
mu = 0;
localData.estimatedX = 0;
localData.estimatedY = 0;
localData.estimatedZ = 0;
localData.alpha = 0;
localData.beta = 0;
localData.gamma = 0;
localPosition.x = 0;
localPosition.y = 0;
localPosition.z = 0;
twok1 = 0;
twok2 = 0;
twok3 = 0;
stepSize = 0;
numNodes = 0;
// Zero out the stuff from the other nodes
for (uint8_t i = 0; i < MAX_NUM_NODES; i++) {
useNode[i] = 0;
lastNodeTime[i] = 0;
nodeData[i].estimatedX = 0;
nodeData[i].estimatedY = 0;
nodeData[i].estimatedZ = 0;
nodeData[i].alpha = 0;
nodeData[i].beta = 0;
nodeData[i].gamma = 0;
}
}
/**
* This is used to get the distance from the ranging deck
* This is used to get the distance from the ranging deck.
*/
void computation_localization_newDistance(int anchor, float newRange) {
DEBUG_PRINT("D: %f\n", newRange);
ranges[anchor] = newRange;
}
/*
* This function is called by stabilizer once a new state has been determined.
*/
void computation_update_state( state_t *state, sensorData_t *sensor) {
localPosition.x = state->position.x;
localPosition.y = state->position.y;
localPosition.z = state->position.z;
}
void computation_configReceived( CRTPPacket *pk, CRTPPacket *p ) {
// Save the received packet.
// Note that the config starts at index 1 because index 0 is the opcode for the config
computation_localization_config_packet config;
memcpy(&config, &(pk->data[1]), sizeof(computation_localization_config_packet) );
// Parse the config
stepSize = config.stepSize;
nodeNumber = config.nodeNumber;
numNodes = config.numTotalNodes;
anchorNumber = config.anchor;
twok1 = 2*config.k1;
twok2 = 2*config.k2;
twok3 = 2*config.k3;
DEBUG_PRINT("COMP: Step %2.2f\n", stepSize);
DEBUG_PRINT("COMP: Node %d\n", nodeNumber);
DEBUG_PRINT("COMP: NumNodes %d\n", numNodes);
DEBUG_PRINT("COMP: Anchor %d\n", anchorNumber);
DEBUG_PRINT("COMP: twok1 %2.2f\n", twok1);
DEBUG_PRINT("COMP: twok2 %2.2f\n", twok2);
DEBUG_PRINT("COMP: twok3 %2.2f\n", twok3);
}
void computation_packetReceived( CRTPPacket *pk, CRTPPacket *p ) {
void computation_packetReceived( CRTPPacket *pk ) {
// The node the packet came from
uint8_t i = pk->data[0];
// Copy the data into the storage array slot for the node
if (i < MAX_NUM_NODES) {
memcpy(&(nodeData[i]), &(pk->data[1]), sizeof(computation_localization_node_data));
if (useNode[i] == 0) {
DEBUG_PRINT("Node %d activated\n", i);
}
useNode[i] = 1;
lastNodeTime[i] = xTaskGetTickCount(); // Update the time received
}
}
/*
* This function does the actual computation
*/
void computation_processing() {
// The number of nodes used in this computation
uint8_t numComputingNodes = 0;
// Go through and make sure all the data is valid
uint32_t currentTick = xTaskGetTickCount();
for ( int i = 0; i < MAX_NUM_NODES; i++ ) {
float timeDelta = currentTick - lastNodeTime[i];
if ( (timeDelta > M2T(1000)) && (useNode[i] != 0) ) {
useNode[i] = 0;
DEBUG_PRINT("Node %d has gone stale\n", i);
}
numComputingNodes += useNode[i];
}
// These are local to the node
double dmu = (localData.estimatedX - localPosition.x) * (localData.estimatedX - localPosition.x)
+ (localData.estimatedY - localPosition.y) * (localData.estimatedY - localPosition.y)
+ (localData.estimatedZ - localPosition.z) * (localData.estimatedZ - localPosition.z)
- (a - ranges[anchorNumber]) * (a - ranges[anchorNumber]);
}
\ No newline at end of file
double da = 2 * (2*dmu + mu)*(a - ranges[anchorNumber]) - 2*a;
// These are based upon data from the other nodes
double dx = -2 * (2*dmu + mu)*(localData.estimatedX - localPosition.x)
+ numComputingNodes*localData.alpha
+ twok1 * numComputingNodes*localData.estimatedX;
double dy = -2 * (2*dmu + mu)*(localData.estimatedY - localPosition.y)
+ numComputingNodes*localData.beta
+ twok2 * numComputingNodes*localData.estimatedY;
double dz = -2 * (2*dmu + mu)*(localData.estimatedZ - localPosition.z)
+ numComputingNodes*localData.gamma
+ twok3 * numComputingNodes*localData.estimatedZ;
double dalpha = -1 * numComputingNodes*localData.estimatedX;
double dbeta = -1 * numComputingNodes*localData.estimatedY;
double dgamma = -1 * numComputingNodes*localData.estimatedZ;
// Go through and compute the updates
for (int i = 0; i < MAX_NUM_NODES; i++ ) {
// Make sure the node should be used
if (useNode[i] == 0) {
continue;
}
dx -= ( nodeData[i].alpha + twok1*nodeData[i].estimatedX );
dy -= ( nodeData[i].beta + twok1*nodeData[i].estimatedY );
dz -= ( nodeData[i].gamma + twok1*nodeData[i].estimatedZ );
dalpha += nodeData[i].estimatedX;
dbeta += nodeData[i].estimatedY;
dgamma += nodeData[i].estimatedZ;
}
// Update the local information (simple Euler's method)
a += (stepSize * da);
mu += (stepSize * dmu);
// Update the information to share (simple Euler's method)
localData.estimatedX += (stepSize * dx);
localData.estimatedY += (stepSize * dy);
localData.estimatedZ += (stepSize * dz);
localData.alpha += (stepSize * dalpha);
localData.beta += (stepSize * dbeta);
localData.gamma += (stepSize * dgamma);
// Package the information into a packet and then send it
dataPacket.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_COMPUTE_DATA);
dataPacket.data[0] = nodeNumber;
dataPacket.size = sizeof(computation_localization_node_data) + 1;
memcpy(&(dataPacket.data[1]), &localData, sizeof(computation_localization_node_data));
crtpSendPacket(&dataPacket);
}
......@@ -43,6 +43,7 @@
#include "controller.h"
#include "power_distribution.h"
#include "controller_update.h"
#include "computation_func.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
......@@ -127,6 +128,7 @@ static void stabilizerTask(void* param)
#else
sensorsAcquire(&sensorData, tick);
stateEstimator(&state, &sensorData, tick);
computation_update_state(&state, &sensorData);
commanderXYZ_GetPosition(&state, tick);
#endif
commanderXYZ_GetSetpoint(&setpoint, &state, &resetControllers);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment