Commit 20682fe2 authored by Ian McInerney's avatar Ian McInerney

Initial computation work

parent 298b6242
......@@ -26,8 +26,9 @@ PLATFORM ?= CF2
##### Sets the name of the stabilizer module to use.
SENSORS ?= stock
ESTIMATOR ?= complementary
CONTROLLER ?= pid
#CONTROLLER ?= lqr
COMPUTATION ?= localization
#CONTROLLER ?= pid
CONTROLLER ?= lqr
POWER_DISTRIBUTION ?= stock
......@@ -111,7 +112,7 @@ FREERTOS_OBJ = list.o tasks.o queue.o timers.o $(MEMMANG_OBJ)
# Crazyflie sources
VPATH += src/init src/hal/src src/modules/src src/utils/src src/drivers/src
VPATH += src/modules/src/controllers src/modules/src/estimators
VPATH += src/modules/src/controllers src/modules/src/estimators src/modules/src/computation
VPATH_CF1 += src/platform/cf1
VPATH_CF2 += src/platform/cf2
......@@ -162,6 +163,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
# Deck Core
PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o
......@@ -220,6 +222,7 @@ GDB = $(CROSS_COMPILE)gdb
INCLUDES = -I$(FREERTOS)/include -I$(PORT) -Isrc
INCLUDES += -Isrc/config -Isrc/hal/interface -Isrc/modules/interface
INCLUDES += -Isrc/modules/interface/controllers -Isrc/modules/interface/estimators
INCLUDES += -Isrc/modules/interface/computation
INCLUDES += -Isrc/utils/interface -Isrc/drivers/interface -Isrc/platform
INCLUDES += -Ivendor/CMSIS/CMSIS/Include
......@@ -267,7 +270,7 @@ ifeq ($(USE_ESKYLINK), 1)
CFLAGS += -DUSE_ESKYLINK
endif
CFLAGS += -DBOARD_REV_$(REV) -DSENSORS_TYPE_$(SENSORS) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION)
CFLAGS += -DBOARD_REV_$(REV) -DSENSORS_TYPE_$(SENSORS) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION) -DCOMPUTATION_TYPE_$(COMPUTATION)
CFLAGS += $(PROCESSOR) $(INCLUDES) $(STFLAGS)
ifeq ($(PLATFORM), CF1)
......
......@@ -52,6 +52,10 @@
#include "estimator_kalman.h"
#endif
#ifdef COMPUTATION_TYPE_localization
#include "computation_localization.h"
#endif
#include "arm_math.h"
#include "libdw1000.h"
......@@ -390,6 +394,13 @@ static void dwm1000Task(void *param)
rangingState |= (1<<current_anchor);
failedRanging[current_anchor] = 0;
DEBUG_PRINT("D: ");
DEBUG_PRINT("%f\n", distance[current_anchor]);
#ifdef COMPUTATION_TYPE_localization
computation_newDistance(current_anchor, distance[current_anchor]);
#endif
#ifdef ESTIMATOR_TYPE_kalman
// Ouliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
......@@ -632,6 +643,7 @@ static void dwm1000Init(DeckInfo *info)
NVIC_Init(&NVIC_InitStructure);
isInit = true;
DEBUG_PRINT("DW1000 Initialized\n");
}
static bool dwm1000Test()
......
#ifndef COMPUTATION_CRTP_H
#define COMPUTATION_CRTP_H
#include <stdbool.h>
#include "crtp.h"
enum Computation_channels {
COMP_CHAN_QUERY = 0,
COMP_CHAN_COMPUTE,
};
void Computation_CRTP_Init();
bool Computation_CRTP_Test();
void Computation_CRTP_registerComputationCallback(void (*func)(CRTPPacket *pk, CRTPPacket *p) );
#endif
\ No newline at end of file
#ifndef COMPUTATION_LOCALIZATION_H_
#define COMPUTATION_LOCALIZATION_H_
void computation_newDistance(int anchor, float newRange);
#endif
\ No newline at end of file
......@@ -45,6 +45,7 @@ typedef enum {
CRTP_PORT_XYZ_SET = 0x06,
CRTP_PORT_XYZ_POS = 0x07,
CRTP_PORT_CONTROLLER_UPDATE = 0x08,
CRTP_PORT_COMP = 0x09,
CRTP_PORT_PLATFORM = 0x0D,
CRTP_PORT_LINK = 0x0F,
} CRTPPort;
......
#include <stdint.h>
#include <stdbool.h>
#include "crtp.h"
#include "debug.h"
#include "computation_CRTP.h"
// Is the stuff initialized
static bool isInit = 0;
// Packet structure used to return stuff to the client
static CRTPPacket p;
static void (*computationFunc)(CRTPPacket *pk, CRTPPacket *p);
#define NULL 0
/**
* Callback for the controller update CRTP packet
*/
static void Computation_crtpCB(CRTPPacket* pk) {
p.header = CRTP_HEADER(CRTP_PORT_COMP, COMP_CHAN_QUERY);
p.size = 1;
p.data[0] = 0;
switch( pk->channel ) {
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;
}
break;
case COMP_CHAN_COMPUTE:
// Call the computation function
if (computationFunc != NULL) {
computationFunc(pk, &p);
}
break;
}
// Pass the return packet for sending
crtpSendPacket(&p);
}
void Computation_CRTP_registerComputationCallback(void (*func)(CRTPPacket *pk, CRTPPacket *p) ) {
DEBUG_PRINT("Registered Computation\n");
computationFunc = func;
}
void Computation_CRTP_Init() {
if(isInit) {
return;
}
crtpInit();
crtpRegisterPortCB(CRTP_PORT_COMP, Computation_crtpCB);
isInit = true;
}
bool distributedComputation_CRTP_Test() {
crtpTest();
return isInit;
}
#include "computation_CRTP.h"
#include "debug.h"
#include "crtp.h"
#define N_ANCHORS 6
static float ranges[N_ANCHORS];
/**
* This is used to get the distance from the ranging deck
*/
void computation_newDistance(int anchor, float newRange) {
ranges[anchor] = newRange;
DEBUG_PRINT("D: %f", newRange);
}
void computation_localization( CRTPPacket *pk, CRTPPacket *p) {
}
void ComputationInit() {
// Call the CRTP initialization stuff for the callback
Computation_CRTP_Init();
Computation_CRTP_registerComputationCallback( computation_localization );
//
}
\ No newline at end of file
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