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 ...@@ -27,10 +27,11 @@ PLATFORM ?= CF2
SENSORS ?= stock SENSORS ?= stock
ESTIMATOR ?= complementary ESTIMATOR ?= complementary
COMPUTATION ?= localization COMPUTATION ?= localization
#CONTROLLER ?= pid CONTROLLER ?= pid
CONTROLLER ?= lqi #CONTROLLER ?= lqi
POWER_DISTRIBUTION ?= stock POWER_DISTRIBUTION ?= stock
LPS_TDMA_ENABLE = 1
ifeq ($(PLATFORM), CF1) ifeq ($(PLATFORM), CF1)
OPENOCD_TARGET ?= target/stm32f1x_stlink.cfg OPENOCD_TARGET ?= target/stm32f1x_stlink.cfg
...@@ -163,7 +164,7 @@ PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o ...@@ -163,7 +164,7 @@ PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o
PROJ_OBJ += sensors_$(SENSORS).o power_distribution_$(POWER_DISTRIBUTION).o PROJ_OBJ += sensors_$(SENSORS).o power_distribution_$(POWER_DISTRIBUTION).o
PROJ_OBJ += controller_update.o PROJ_OBJ += controller_update.o
PROJ_OBJ += computation_CRTP.o computation_$(COMPUTATION).o PROJ_OBJ += computation.o computation_$(COMPUTATION).o
# Deck Core # Deck Core
PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o
...@@ -414,3 +415,5 @@ include tools/make/targets.mk ...@@ -414,3 +415,5 @@ include tools/make/targets.mk
#include dependencies #include dependencies
-include $(DEPS) -include $(DEPS)
...@@ -82,6 +82,7 @@ ...@@ -82,6 +82,7 @@
#define PARAM_TASK_PRI 1 #define PARAM_TASK_PRI 1
#define PROXIMITY_TASK_PRI 0 #define PROXIMITY_TASK_PRI 0
#define PM_TASK_PRI 0 #define PM_TASK_PRI 0
#define COMPUTATION_TASK_PRI 1
#ifdef PLATFORM_CF2 #ifdef PLATFORM_CF2
#define SYSLINK_TASK_PRI 5 #define SYSLINK_TASK_PRI 5
...@@ -120,6 +121,7 @@ ...@@ -120,6 +121,7 @@
#define PROXIMITY_TASK_NAME "PROXIMITY" #define PROXIMITY_TASK_NAME "PROXIMITY"
#define EXTRX_TASK_NAME "EXTRX" #define EXTRX_TASK_NAME "EXTRX"
#define UART_RX_TASK_NAME "UART" #define UART_RX_TASK_NAME "UART"
#define COMPUTATION_TASK_NAME "COMPUTATION"
//Task stack sizes //Task stack sizes
#define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE) #define SYSTEM_TASK_STACKSIZE (2* configMINIMAL_STACK_SIZE)
...@@ -140,6 +142,7 @@ ...@@ -140,6 +142,7 @@
#define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define PROXIMITY_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE #define EXTRX_TASK_STACKSIZE configMINIMAL_STACK_SIZE
#define UART_RX_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 //The radio channel. From 0 to 125
#define RADIO_CHANNEL 80 #define RADIO_CHANNEL 80
......
...@@ -41,8 +41,18 @@ ...@@ -41,8 +41,18 @@
#include "arm_math.h" #include "arm_math.h"
#endif #endif
#ifdef COMPUTATION_TYPE_localization
#include "computation_localization.h"
#include "arm_math.h"
#endif
#define RX_TIMEOUT 1000 #define RX_TIMEOUT 1000
#define TDMA_NSLOTS_BITS 2
#define TDMA_SLOT 1
// TDMA handling // TDMA handling
bool tdmaSynchronized; bool tdmaSynchronized;
dwTime_t frameStart; dwTime_t frameStart;
...@@ -74,6 +84,17 @@ int anchors[N_ANCHORS] = {1,2,3,4,5,6}; ...@@ -74,6 +84,17 @@ int anchors[N_ANCHORS] = {1,2,3,4,5,6};
#define TAG_ADDRESS 8+TDMA_SLOT #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 // Outlier rejection
#ifdef ESTIMATOR_TYPE_kalman #ifdef ESTIMATOR_TYPE_kalman
#define RANGING_HISTORY_LENGTH 32 #define RANGING_HISTORY_LENGTH 32
...@@ -209,6 +230,22 @@ static uint32_t rxcallback(dwDevice_t *dev) { ...@@ -209,6 +230,22 @@ static uint32_t rxcallback(dwDevice_t *dev) {
options->distance[current_anchor] = SPEED_OF_LIGHT * tprop; options->distance[current_anchor] = SPEED_OF_LIGHT * tprop;
options->pressures[current_anchor] = report->asl; 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 #ifdef ESTIMATOR_TYPE_kalman
// Outliers rejection // Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH; rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
......
#ifndef COMPUTATION_CRTP_H #ifndef COMPUTATION_H_
#define COMPUTATION_CRTP_H #define COMPUTATION_H_
#include <stdbool.h> #include <stdbool.h>
#include "crtp.h" #include "crtp.h"
...@@ -7,11 +7,12 @@ ...@@ -7,11 +7,12 @@
enum Computation_channels { enum Computation_channels {
COMP_CHAN_QUERY = 0, COMP_CHAN_QUERY = 0,
COMP_CHAN_COMPUTE, 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 #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_ #ifndef COMPUTATION_LOCALIZATION_H_
#define 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 #endif
\ No newline at end of file
...@@ -31,6 +31,10 @@ ...@@ -31,6 +31,10 @@
#include <math.h> #include <math.h>
#define USE_PSEUDO_NONLINEAR_CORRECTION
#define USE_CONTROLLER_THRUST_COMPENSATION
enum STATE_CONTROLLER_TYPE { enum STATE_CONTROLLER_TYPE {
CONTROLLER_BYPASS = 0, CONTROLLER_BYPASS = 0,
CONTROLLER_PID = 1, CONTROLLER_PID = 1,
...@@ -47,11 +51,18 @@ extern const int stateControllerType; ...@@ -47,11 +51,18 @@ extern const int stateControllerType;
* @param batteryVolt The battery voltage * @param batteryVolt The battery voltage
* @return Scale factor [0, 2] * @return Scale factor [0, 2]
*/ */
#ifdef USE_CONTROLLER_THRUST_COMPENSATION
inline float controller_thrustAdjustment(float batteryVolt) { inline float controller_thrustAdjustment(float batteryVolt) {
float scale = 1 + (3.7 - batteryVolt)/3.7; float scale = 1 + (3.7 - batteryVolt)/3.7;
return( scale ); 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 * Pseudo-nonlinear correction to correct the X & Y for a non zero yaw
...@@ -62,10 +73,17 @@ inline float controller_thrustAdjustment(float batteryVolt) { ...@@ -62,10 +73,17 @@ inline float controller_thrustAdjustment(float batteryVolt) {
* @param y The earth-frame Y location * @param y The earth-frame Y location
* @param yaw The yaw angle in radians * @param yaw The yaw angle in radians
*/ */
inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) { #ifdef USE_PSEUDO_NONLINEAR_CORRECTION
*correctedX = (x*cosf(yaw)) - (y*sinf(yaw)); inline void controller_yawCorrection(float *correctedX, float *correctedY, float x, float y, float yaw) {
*correctedY = (x*sinf(yaw)) + (y*cosf(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); void stateControllerInit(void);
bool stateControllerTest(void); bool stateControllerTest(void);
......
...@@ -2,8 +2,15 @@ ...@@ -2,8 +2,15 @@
#include <stdbool.h> #include <stdbool.h>
#include "crtp.h" #include "crtp.h"
#include "debug.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 // Is the stuff initialized
static bool isInit = 0; static bool isInit = 0;
...@@ -11,12 +18,16 @@ static bool isInit = 0; ...@@ -11,12 +18,16 @@ static bool isInit = 0;
// Packet structure used to return stuff to the client // Packet structure used to return stuff to the client
static CRTPPacket p; 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) { static void Computation_crtpCB(CRTPPacket* pk) {
p.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_QUERY); p.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_QUERY);
...@@ -26,17 +37,28 @@ static void Computation_crtpCB(CRTPPacket* pk) { ...@@ -26,17 +37,28 @@ static void Computation_crtpCB(CRTPPacket* pk) {
default: default:
case COMP_CHAN_QUERY: case COMP_CHAN_QUERY:
// The quadcopter is being queried as to the type of computation and if it exists // The quadcopter is being queried as to the type of computation and if it exists
if (computationFunc == NULL) { p.data[0] = computationType;
p.data[0] = 1;
} else {
p.data[0] = 2;
}
break; break;
case COMP_CHAN_COMPUTE: case COMP_CHAN_CONFIG:
// Call the computation function switch (p.data[0]) {
if (computationFunc != NULL) { case COMP_CONFIG_ENABLE:
computationFunc(pk, &p); // 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; break;
} }
...@@ -44,23 +66,34 @@ static void Computation_crtpCB(CRTPPacket* pk) { ...@@ -44,23 +66,34 @@ static void Computation_crtpCB(CRTPPacket* pk) {
crtpSendPacket(&p); crtpSendPacket(&p);
} }
void Computation_CRTP_registerComputationCallback(void (*func)(CRTPPacket *pk, CRTPPacket *p) ) { void Computation_Task() {
DEBUG_PRINT("Registered Computation\n"); DEBUG_PRINT("Started Computation Task\n");
computationFunc = func;
} uint32_t lastWakeTime;
lastWakeTime = xTaskGetTickCount ();
void Computation_CRTP_Init() { while(1) {
if(isInit) { vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
return; if (computationEnabled) {
// Perform the computations
computation_processing();
}
} }
}
void Computation_Init() {
// Call the CRTP initialization stuff for the callback
crtpInit(); crtpInit();
crtpRegisterPortCB(CRTP_PORT_COMP, Computation_crtpCB); 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; isInit = true;
} }
bool distributedComputation_CRTP_Test() { bool Computation_Test() {
crtpTest(); crtpTest();
return isInit; return isInit;
} }
#include "computation_CRTP.h" #include "computation.h"
#include "computation_func.h"
#include "debug.h" #include "debug.h"
#include "locodeck.h" #include "locodeck.h"
#include "crtp.h" #include "crtp.h"
computationID computationType = COMPUTATION_LOCALIZATION;
static float ranges[LOCODECK_NR_OF_ANCHORS]; static float ranges[LOCODECK_NR_OF_ANCHORS];
/** /**
* This is used to get the distance from the ranging deck * 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); DEBUG_PRINT("D: %f\n", newRange);
ranges[anchor] = newRange; ranges[anchor] = newRange;
} }
void computation_localization( CRTPPacket *pk, CRTPPacket *p) { void computation_configReceived( CRTPPacket *pk, CRTPPacket *p ) {
} }
void ComputationInit() { void computation_packetReceived( CRTPPacket *pk, CRTPPacket *p ) {
// Call the CRTP initialization stuff for the callback
Computation_CRTP_Init(); }
Computation_CRTP_registerComputationCallback( computation_localization );
// void computation_processing() {
} }
\ No newline at end of file
...@@ -50,7 +50,6 @@ const int stateControllerType = CONTROLLER_LQI; ...@@ -50,7 +50,6 @@ const int stateControllerType = CONTROLLER_LQI;
#define DEG_TO_RAD_F(x) ( x * ((float)M_PI / 180.0)) #define DEG_TO_RAD_F(x) ( x * ((float)M_PI / 180.0))
#define USE_PSEUDO_NONLINEAR_CORRECTION
/*float K[4][16] = { /*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, 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 }, { 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] = { ...@@ -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 } { 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] = { 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, 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 }, { 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 }, { -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 } { 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]; static float integrators[4];
...@@ -206,7 +221,7 @@ void stateController(control_t *control, const sensorData_t *sensors, ...@@ -206,7 +221,7 @@ void stateController(control_t *control, const sensorData_t *sensors,
// If the correction should be done, do it // If the correction should be done, do it
#ifdef USE_PSEUDO_NONLINEAR_CORRECTION #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], controller_yawCorrection(&error[state_u], &error[state_v],
xVelError, yVelError, yawRad); xVelError, yVelError, yawRad);
...@@ -234,14 +249,14 @@ void stateController(control_t *control, const sensorData_t *sensors, ...@@ -234,14 +249,14 @@ void stateController(control_t *control, const sensorData_t *sensors,
error[state_z_integ] = -integrators[z_integ]; error[state_z_integ] = -integrators[z_integ];
// Compute the error for the angular velocity // 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_p] = -1.0 * DEG_TO_RAD_F(sensors->gyro.x);
error[state_q] = -1.0 * DEG_TO_RAD_F(sensors->gyro.y);// * (float)M_PI / 180; error[state_q] = -1.0 * DEG_TO_RAD_F(sensors->gyro.y);
error[state_r] = -1.0 * DEG_TO_RAD_F(sensors->gyro.z);// * (float)M_PI / 180; error[state_r] = -1.0 * DEG_TO_RAD_F(sensors->gyro.z);
// Compute the angle error // Compute the angle error
error[state_phi] = -1.0 * DEG_TO_RAD_F(state->attitude.roll);// * (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);// * (float)M_PI / 180; error[state_theta] = -1.0 * DEG_TO_RAD_F(state->attitude.pitch);
error[state_psi] = DEG_TO_RAD_F(setpoint->attitude.yaw - state->cameraYaw);// * (float)M_PI / 180; error[state_psi] = DEG_TO_RAD_F(setpoint->attitude.yaw - state->cameraYaw);
// Integrate the yaw error and compute the integral error // Integrate the yaw error and compute the integral error
integrators[psi_integ] += (error[state_psi] * CONTROLLER_DT); integrators[psi_integ] += (error[state_psi] * CONTROLLER_DT);
......
...@@ -60,6 +60,8 @@ ...@@ -60,6 +60,8 @@
#include "buzzer.h" #include "buzzer.h"
#include "sound.h" #include "sound.h"
#include "computation.h"
#ifdef PLATFORM_CF1 #ifdef PLATFORM_CF1
#include "uart.h" #include "uart.h"
#endif #endif
...@@ -185,6 +187,8 @@ void systemTask(void *arg) ...@@ -185,6 +187,8 @@ void systemTask(void *arg)
soundInit(); soundInit();
memInit(); memInit();
Computation_Init();
#ifdef PROXIMITY_ENABLED #ifdef PROXIMITY_ENABLED
proximityInit(); proximityInit();
#endif #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