Commit e6c41056 authored by Ian McInerney's avatar Ian McInerney

Refactored computation to be more like stabilier

parent 62186dbc
......@@ -27,10 +27,11 @@ PLATFORM ?= CF2
SENSORS ?= stock
ESTIMATOR ?= complementary
COMPUTATION ?= localization
#CONTROLLER ?= pid
CONTROLLER ?= lqi
CONTROLLER ?= pid
#CONTROLLER ?= lqi
POWER_DISTRIBUTION ?= stock
LPS_TDMA_ENABLE = 1
ifeq ($(PLATFORM), CF1)
OPENOCD_TARGET ?= target/stm32f1x_stlink.cfg
......@@ -163,7 +164,7 @@ PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o
PROJ_OBJ += sensors_$(SENSORS).o power_distribution_$(POWER_DISTRIBUTION).o
PROJ_OBJ += controller_update.o
PROJ_OBJ += computation_CRTP.o computation_$(COMPUTATION).o
PROJ_OBJ += computation.o computation_$(COMPUTATION).o
# Deck Core
PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o
......@@ -414,3 +415,5 @@ include tools/make/targets.mk
#include dependencies
-include $(DEPS)
......@@ -82,6 +82,7 @@
#define PARAM_TASK_PRI 1
#define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0
#define COMPUTATION_TASK_PRI 1
#ifdef PLATFORM_CF2
#define SYSLINK_TASK_PRI 5
......@@ -120,6 +121,7 @@
#define PROXIMITY_TASK_NAME "PROXIMITY"
#define EXTRX_TASK_NAME "EXTRX"
#define UART_RX_TASK_NAME "UART"
#define COMPUTATION_TASK_NAME "COMPUTATION"
//Task stack sizes
#define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
......@@ -140,6 +142,7 @@
#define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART_RX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define COMPUTATION_TASK_STACKSIZE configMINIMAL_STACK_SIZE
//The radio channel. From 0 to 125
#define RADIO_CHANNEL 80
......
......@@ -41,8 +41,18 @@
#include "arm_math.h"
#endif
#ifdef COMPUTATION_TYPE_localization
#include "computation_localization.h"
#include "arm_math.h"
#endif
#define RX_TIMEOUT 1000
#define TDMA_NSLOTS_BITS 2
#define TDMA_SLOT 1
// TDMA handling
bool tdmaSynchronized;
dwTime_t frameStart;
......@@ -74,6 +84,17 @@ int anchors[N_ANCHORS] = {1,2,3,4,5,6};
#define TAG_ADDRESS 8+TDMA_SLOT
// Outlier rejection
#ifdef COMPUTATION_TYPE_localization
#define RANGING_HISTORY_LENGTH 32
#define OUTLIER_TH 4
static struct {
float32_t history[RANGING_HISTORY_LENGTH];
size_t ptr;
} rangingStats[LOCODECK_NR_OF_ANCHORS];
#endif
// Outlier rejection
#ifdef ESTIMATOR_TYPE_kalman
#define RANGING_HISTORY_LENGTH 32
......@@ -209,6 +230,22 @@ static uint32_t rxcallback(dwDevice_t *dev) {
options->distance[current_anchor] = SPEED_OF_LIGHT * tprop;
options->pressures[current_anchor] = report->asl;
#ifdef COMPUTATION_TYPE_localization
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
float32_t mean;
float32_t stddev;
arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
float32_t diff = fabsf(mean - options->distance[current_anchor]);
rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];
if ( diff < (OUTLIER_TH*stddev) ){
computation_localization_newDistance(current_anchor, options->distance[current_anchor]);
}
#endif
#ifdef ESTIMATOR_TYPE_kalman
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
......
#ifndef COMPUTATION_CRTP_H
#define COMPUTATION_CRTP_H
#ifndef COMPUTATION_H_
#define COMPUTATION_H_
#include <stdbool.h>
#include "crtp.h"
......@@ -7,11 +7,12 @@
enum Computation_channels {
COMP_CHAN_QUERY = 0,
COMP_CHAN_COMPUTE,
COMP_CHAN_CONFIG,
};
void Computation_CRTP_Init();
bool Computation_CRTP_Test();
void Computation_CRTP_registerComputationCallback(void (*func)(CRTPPacket *pk, CRTPPacket *p) );
void Computation_Init();
bool Computation_Test();
#endif
\ No newline at end of file
#ifndef COMPUATION_FUNC_H_
#define COMPUATION_FUNC_H_
typedef enum computationID {
COMPUTATION_NONE = 0,
COMPUTATION_LOCALIZATION,
} computationID;
extern computationID computationType;
void computation_configReceived( CRTPPacket *pk, CRTPPacket *p );
void computation_packetReceived( CRTPPacket *pk, CRTPPacket *p );
void computation_processing();
#endif
\ No newline at end of file
#ifndef COMPUTATION_LOCALIZATION_H_
#define COMPUTATION_LOCALIZATION_H_
void computation_newDistance(int anchor, float newRange);
typedef struct computation_localization_config_packet {
uint8_t numTotalNodes;
} computation_localization_config_packet;
void computation_localization_newDistance(int anchor, float newRange);
#endif
\ No newline at end of file
......@@ -31,6 +31,10 @@
#include <math.h>
#define USE_PSEUDO_NONLINEAR_CORRECTION
#define USE_CONTROLLER_THRUST_COMPENSATION
enum STATE_CONTROLLER_TYPE {
CONTROLLER_BYPASS = 0,
CONTROLLER_PID = 1,
......@@ -47,11 +51,18 @@ extern const int stateControllerType;
* @param batteryVolt The battery voltage
* @return Scale factor [0, 2]
*/
#ifdef USE_CONTROLLER_THRUST_COMPENSATION
inline float controller_thrustAdjustment(float batteryVolt) {
float scale = 1 + (3.7 - batteryVolt)/3.7;
return( scale );
}
#else
inline float controller_thrustAdjustment(float batteryVolt) {
return( 1 );
}
#endif
/**
* Pseudo-nonlinear correction to correct the X & Y for a non zero yaw
......@@ -62,10 +73,17 @@ inline float controller_thrustAdjustment(float batteryVolt) {
* @param y The earth-frame Y location
* @param yaw The yaw angle in radians
*/
inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) {
*correctedX = (x*cosf(yaw)) - (y*sinf(yaw));
*correctedY = (x*sinf(yaw)) + (y*cosf(yaw));
}
#ifdef USE_PSEUDO_NONLINEAR_CORRECTION
inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) {
*correctedX = (x*cosf(yaw)) - (y*sinf(yaw));
*correctedY = (x*sinf(yaw)) + (y*cosf(yaw));
}
#else
inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) {
*correctedX = x;
*correctedY = y;
}
#endif
void stateControllerInit(void);
bool stateControllerTest(void);
......
......@@ -2,8 +2,15 @@
#include <stdbool.h>
#include "crtp.h"
#include "debug.h"
#include "config.h"
#include "computation_CRTP.h"
#include "FreeRTOS.h"
#include "task.h"
#include "stabilizer_types.h"
#include "computation.h"
#include "computation_func.h"
// Is the stuff initialized
static bool isInit = 0;
......@@ -11,12 +18,16 @@ static bool isInit = 0;
// Packet structure used to return stuff to the client
static CRTPPacket p;
static void (*computationFunc)(CRTPPacket *pk, CRTPPacket *p);
static bool computationEnabled = false;
#define NULL 0
// These are various codes used to configure the computation framework
enum comp_config_codes {
COMP_CONFIG_ENABLE = 0,
COMP_CONFIG_FUNC,
};
/**
* Callback for the controller update CRTP packet
* Callback for the computation CRTP packet
*/
static void Computation_crtpCB(CRTPPacket* pk) {
p.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_QUERY);
......@@ -26,17 +37,28 @@ static void Computation_crtpCB(CRTPPacket* pk) {
default:
case COMP_CHAN_QUERY:
// The quadcopter is being queried as to the type of computation and if it exists
if (computationFunc == NULL) {
p.data[0] = 1;
} else {
p.data[0] = 2;
}
p.data[0] = computationType;
break;
case COMP_CHAN_COMPUTE:
// Call the computation function
if (computationFunc != NULL) {
computationFunc(pk, &p);
case COMP_CHAN_CONFIG:
switch (p.data[0]) {
case COMP_CONFIG_ENABLE:
// Toggle the computation on or off
computationEnabled = p.data[1];
if (computationEnabled) {
DEBUG_PRINT("Enabled Computation\n");
} else {
DEBUG_PRINT("Disabled Computation\n");
}
break;
case COMP_CONFIG_FUNC:
default:
// Pass the config packet into the callback
computation_configReceived(pk, &p);
}
case COMP_CHAN_COMPUTE:
// Call the function to process the received data
computation_packetReceived(pk, &p);
break;
}
......@@ -44,23 +66,34 @@ static void Computation_crtpCB(CRTPPacket* pk) {
crtpSendPacket(&p);
}
void Computation_CRTP_registerComputationCallback(void (*func)(CRTPPacket *pk, CRTPPacket *p) ) {
DEBUG_PRINT("Registered Computation\n");
computationFunc = func;
}
void Computation_Task() {
DEBUG_PRINT("Started Computation Task\n");
uint32_t lastWakeTime;
lastWakeTime = xTaskGetTickCount ();
void Computation_CRTP_Init() {
if(isInit) {
return;
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
if (computationEnabled) {
// Perform the computations
computation_processing();
}
}
}
void Computation_Init() {
// Call the CRTP initialization stuff for the callback
crtpInit();
crtpRegisterPortCB(CRTP_PORT_COMP, Computation_crtpCB);
// Create the computation task
xTaskCreate(Computation_Task, COMPUTATION_TASK_NAME,
COMPUTATION_TASK_STACKSIZE, NULL, COMPUTATION_TASK_PRI, NULL);
isInit = true;
}
bool distributedComputation_CRTP_Test() {
bool Computation_Test() {
crtpTest();
return isInit;
}
#include "computation_CRTP.h"
#include "computation.h"
#include "computation_func.h"
#include "debug.h"
#include "locodeck.h"
#include "crtp.h"
computationID computationType = COMPUTATION_LOCALIZATION;
static float ranges[LOCODECK_NR_OF_ANCHORS];
/**
* This is used to get the distance from the ranging deck
*/
void computation_newDistance(int anchor, float newRange) {
void computation_localization_newDistance(int anchor, float newRange) {
DEBUG_PRINT("D: %f\n", newRange);
ranges[anchor] = newRange;
}
void computation_localization( CRTPPacket *pk, CRTPPacket *p) {
void computation_configReceived( CRTPPacket *pk, CRTPPacket *p ) {
}
void ComputationInit() {
// Call the CRTP initialization stuff for the callback
Computation_CRTP_Init();
Computation_CRTP_registerComputationCallback( computation_localization );
//
void computation_packetReceived( CRTPPacket *pk, CRTPPacket *p ) {
}
void computation_processing() {
}
\ No newline at end of file
......@@ -50,7 +50,6 @@ const int stateControllerType = CONTROLLER_LQI;
#define DEG_TO_RAD_F(x) ( x * ((float)M_PI / 180.0))
#define USE_PSEUDO_NONLINEAR_CORRECTION
/*float K[4][16] = {
{ 0.000, 0.000, -17865.955, 0.000, 0.000, 0.000, 0.000, 0.000, -14113.031, 0.000, 0.000, 0.000, 0.000, 0.000, 4000.000, 0.000 },
{ 0.000, 3335.500, 0.000, 2363.287, 0.000, 0.000, 0.000, 3640.412, 0.000, 7357.024, 0.000, 0.000, 0.000, -600.000, 0.000, 0.000 },
......@@ -90,11 +89,27 @@ float K[4][16] = {
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1962.159, 0.000, 0.000, 0.000, 0.000, 0.000, 5192.510, 0.000, 0.000, 0.000, -500.000 }
}; */
/* This has a decent response, but oscillation on the Z axis still Zdot = 400, Z = 75
float K[4][16] = {
{ 0.000, 0.000, -40625.316, 0.000, 0.000, 0.000, 0.000, 0.000, -7553.973, 0.000, 0.000, 0.000, 0.000, 0.000, 10.000, 0.000 },
{ 0.000, 11370.158, 0.000, 2115.858, 0.000, 0.000, 0.000, 6999.308, 0.000, 21050.179, 0.000, 0.000, 0.000, -1000.000, 0.000, 0.000 },
{ -11373.279, 0.000, 0.000, 0.000, 2124.936, 0.000, -6999.754, 0.000, 0.000, 0.000, 21099.874, 0.000, 1000.000, 0.000, 0.000, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 1928.195, 0.000, 0.000, 0.000, 0.000, 0.000, 5001.928, 0.000, 0.000, 0.000, -5.000 }
};*/
/* Decent reponse, but angles still had some oscillation
float K[4][16] = {
{ 0.000, 0.000, -8171.783, 0.000, 0.000, 0.000, 0.000, 0.000, -7608.177, 0.000, 0.000, 0.000, 0.000, 0.000, 100.000, 0.000 },
{ 0.000, 3147.229, 0.000, 1339.962, 0.000, 0.000, 0.000, 3541.816, 0.000, 8817.296, 0.000, 0.000, 0.000, -1000.000, 0.000, 0.000 },
{ -3149.968, 0.000, 0.000, 0.000, 1346.024, 0.000, -3542.589, 0.000, 0.000, 0.000, 8840.965, 0.000, 1000.000, 0.000, 0.000, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 5654.404, 0.000, 0.000, 0.000, 0.000, 0.000, 10056.385, 0.000, 0.000, 0.000, -100.000 }
};*/
float K[4][16] = {
{ 0.000, 0.000, -8171.783, 0.000, 0.000, 0.000, 0.000, 0.000, -7608.177, 0.000, 0.000, 0.000, 0.000, 0.000, 100.000, 0.000 },
{ 0.000, 3194.482, 0.000, 1390.027, 0.000, 0.000, 0.000, 3555.132, 0.000, 9043.023, 0.000, 0.000, 0.000, -1000.000, 0.000, 0.000 },
{ -3197.213, 0.000, 0.000, 0.000, 1396.188, 0.000, -3555.900, 0.000, 0.000, 0.000, 9066.888, 0.000, 1000.000, 0.000, 0.000, 0.000 },
{ 0.000, 0.000, 0.000, 0.000, 0.000, 5952.081, 0.000, 0.000, 0.000, 0.000, 0.000, 15039.628, 0.000, 0.000, 0.000, -100.000 }
};
static float integrators[4];
......@@ -206,7 +221,7 @@ void stateController(control_t *control, const sensorData_t *sensors,
// If the correction should be done, do it
#ifdef USE_PSEUDO_NONLINEAR_CORRECTION
float yawRad = DEG_TO_RAD_F(state->cameraYaw);// * (float)M_PI / 180;
float yawRad = DEG_TO_RAD_F(state->cameraYaw);
controller_yawCorrection(&error[state_u], &error[state_v],
xVelError, yVelError, yawRad);
......@@ -234,14 +249,14 @@ void stateController(control_t *control, const sensorData_t *sensors,
error[state_z_integ] = -integrators[z_integ];
// Compute the error for the angular velocity
error[state_p] = -1.0 * DEG_TO_RAD_F(sensors->gyro.x);// * (float)M_PI / 180;
error[state_q] = -1.0 * DEG_TO_RAD_F(sensors->gyro.y);// * (float)M_PI / 180;
error[state_r] = -1.0 * DEG_TO_RAD_F(sensors->gyro.z);// * (float)M_PI / 180;
error[state_p] = -1.0 * DEG_TO_RAD_F(sensors->gyro.x);
error[state_q] = -1.0 * DEG_TO_RAD_F(sensors->gyro.y);
error[state_r] = -1.0 * DEG_TO_RAD_F(sensors->gyro.z);
// Compute the angle error
error[state_phi] = -1.0 * DEG_TO_RAD_F(state->attitude.roll);// * (float)M_PI / 180;
error[state_theta] = -1.0 * DEG_TO_RAD_F(state->attitude.pitch);// * (float)M_PI / 180;
error[state_psi] = DEG_TO_RAD_F(setpoint->attitude.yaw - state->cameraYaw);// * (float)M_PI / 180;
error[state_phi] = -1.0 * DEG_TO_RAD_F(state->attitude.roll);
error[state_theta] = -1.0 * DEG_TO_RAD_F(state->attitude.pitch);
error[state_psi] = DEG_TO_RAD_F(setpoint->attitude.yaw - state->cameraYaw);
// Integrate the yaw error and compute the integral error
integrators[psi_integ] += (error[state_psi] * CONTROLLER_DT);
......
......@@ -60,6 +60,8 @@
#include "buzzer.h"
#include "sound.h"
#include "computation.h"
#ifdef PLATFORM_CF1
#include "uart.h"
#endif
......@@ -185,6 +187,8 @@ void systemTask(void *arg)
soundInit();
memInit();
Computation_Init();
#ifdef PROXIMITY_ENABLED
proximityInit();
#endif
......
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