diff --git a/examples/mean_embed/.gitignore b/examples/mean_embed/.gitignore deleted file mode 100644 index a743f37a76..0000000000 --- a/examples/mean_embed/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -bin/* -cf2.* diff --git a/examples/mean_embed/Kbuild b/examples/mean_embed/Kbuild deleted file mode 100644 index 9d804337a0..0000000000 --- a/examples/mean_embed/Kbuild +++ /dev/null @@ -1 +0,0 @@ -obj-y += src/ diff --git a/examples/mean_embed/Makefile b/examples/mean_embed/Makefile deleted file mode 100644 index d0a76e2ac3..0000000000 --- a/examples/mean_embed/Makefile +++ /dev/null @@ -1,23 +0,0 @@ -# The firmware uses the Kbuild build system. There are 'Kbuild' files in this -# example that outlays what needs to be built. (check src/Kbuild). -# -# The firmware is configured using options in Kconfig files, the -# values of these end up in the .config file in the firmware directory. -# -# By setting the OOT_CONFIG (it is '$(PWD)/oot-config' by default) environment -# variable you can provide a custom configuration. It is important that you -# enable the app-layer. See app-config in this directory for example. - -# -# We want to execute the main Makefile for the firmware project, -# it will handle the build for us. -# -CRAZYFLIE_BASE := ../.. - -# -# We override the default OOT_CONFIG here, we could also name our config -# to oot-config and that would be the default. -# -OOT_CONFIG := $(PWD)/app-config - -include $(CRAZYFLIE_BASE)/tools/make/oot.mk diff --git a/examples/mean_embed/README.md b/examples/mean_embed/README.md deleted file mode 100644 index d33b7d887b..0000000000 --- a/examples/mean_embed/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Hello world App for Crazyflie 2.X - -This folder contains the app layer application for the Crazyflie to print a hello world debug message, which can be read in the console tab of the [cfclient](https://github.com/bitcraze/crazyflie-clients-python). - -See App layer API guide and build instructions [here](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/app_layer/) \ No newline at end of file diff --git a/examples/mean_embed/app-config b/examples/mean_embed/app-config deleted file mode 100644 index c86fe9169e..0000000000 --- a/examples/mean_embed/app-config +++ /dev/null @@ -1,3 +0,0 @@ -CONFIG_APP_ENABLE=y -CONFIG_APP_PRIORITY=1 -CONFIG_APP_STACKSIZE=350 diff --git a/examples/mean_embed/include/coll_avoid_main.h b/examples/mean_embed/include/coll_avoid_main.h deleted file mode 100644 index 1d11e5bbe5..0000000000 --- a/examples/mean_embed/include/coll_avoid_main.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef __COLL_AVOID_MAIN_H__ -#define __COLL_AVOID_MAIN_H__ - -#include -void getThrusts(int16_t*); -uint8_t isHighLevel(); -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/mlp.h b/examples/mean_embed/include/mlp.h deleted file mode 100644 index c74c9e6ec6..0000000000 --- a/examples/mean_embed/include/mlp.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __MLP_H__ -#define __MLP_H__ - -#define PHI_A_H 24 -#define OUTPUT0 32 -#define PHI_A_V 4 - -typedef struct _INPUTOUTPUT -{ - float out0[OUTPUT0]; - float phi_a[PHI_A_V]; -}inpOutVec; - -typedef struct MLPINPUT -{ - int len; - float inputVec[PHI_A_H]; - -}mlpInput; - -void calcMlpOutput(mlpInput* inp , float* out); - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/neighbor_encoder.h b/examples/mean_embed/include/neighbor_encoder.h deleted file mode 100644 index 244e42549b..0000000000 --- a/examples/mean_embed/include/neighbor_encoder.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef __NEIGHBOR_ENCOODER_H__ -#define __NEIGHBOR_ENCOODER_H__ - -#include "../include/neighbor_observation.h" -// structure constants -#define NEIGHBOR_NETWORK_OUT 8 -#define PSI_ETA_H 6 -#define PSI_ETA_V 8 - -#define K_NEIGHBORS 6 -#define NEIGHB_ENCODER_W {{PSI_ETA_V, PSI_ETA_H}, {PSI_ETA_V,PSI_ETA_V}}; -#define NEIGHB_ENCODER_B {PSI_ETA_V, PSI_ETA_V}; - - - -void calcNeighborEncoderOutput(neighb_obs* inp, float* outp); - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/neighbor_observation.h b/examples/mean_embed/include/neighbor_observation.h deleted file mode 100644 index 9241114cc8..0000000000 --- a/examples/mean_embed/include/neighbor_observation.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef __NEIGHBOR_OBSERVATION_H__ -#define __NEIGHBOR_OBSERVATION_H__ - -#include "../include/vec.h" -#include -#define NEIGHBOROBSSIZE 6 -#define KNEARESTAMOUNT 6 -#define MAXNEIGhBVELX 3 -#define MAXNEIGhBVELY 3 -#define MAXNEIGhBVELZ 3 - -#define MAXNEIGHBPOSX 10 -#define MAXNEIGHBPOSY 10 -#define MAXNEIGHBPOSZ 10 - -#define MINNEIGHBVELX -MAXNEIGHBVELX -#define MINNEIGHBVELY -MAXNEIGHBVELY -#define MINNEIGHBVELZ -MAXNEIGHBVELZ - -#define MINNEIGHBPOSX -MAXNEIGHBPOSX -#define MINNEIGHBPOSY -MAXNEIGHBPOSY -#define MINNEIGHBPOSZ -MAXNEIGHBPOSZ - - -typedef struct _NEIGHB_OBS -{ - union - { - struct - { - Vector3 relPos; - Vector3 relVel; - }; - Vector3 neighb_obs_arr[NEIGHBOROBSSIZE]; - }; - - int obsLen; - -}neighb_obs; - -typedef struct _PacketData -{ - uint8_t id; - Vector3 pos; - Vector3 vel; -} PacketData; // size: ? bytes + 12 bytes + 12 bytes // max: 60 bytes - - -void updateNeighbObservation(neighb_obs* kNearest); - - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/nn.h b/examples/mean_embed/include/nn.h deleted file mode 100644 index c17da344fe..0000000000 --- a/examples/mean_embed/include/nn.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef __NN_H__ -#define __NN_H__ - -#define N_DRONES 8 - -typedef struct _NN_INPUT_OUTPUT_ -{ - float outputSelf[16]; - float outputNeighbor[8]; - float output[4]; -}nnInputOutput; - - -void feedForwardNN(float*); -int getMyId(); - - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/p2p_comm.h b/examples/mean_embed/include/p2p_comm.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/examples/mean_embed/include/self_encoder.h b/examples/mean_embed/include/self_encoder.h deleted file mode 100644 index dec76e7acc..0000000000 --- a/examples/mean_embed/include/self_encoder.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef __SELF_ENCOODER_H__ -#define __SELF_ENCOODER_H__ - - -// structure constant -#define SELF_NETWORK_OUT 16 -#define PSI_S_H 18 -#define PSI_S_V 16 - -void calcSelfEncoderOutput(float*, float*); -float* getSelfOutput(); - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/self_observation.h b/examples/mean_embed/include/self_observation.h deleted file mode 100644 index 36f2ddd7d3..0000000000 --- a/examples/mean_embed/include/self_observation.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef __SELF_OBSERVATION_H__ -#define __SELF_OBSERVATION_H__ -#include "../include/vec.h" - -#define SELFOBSERVATIONSIZE 18 -#define N_DRONES 8 - -#define MAXVELX 3 -#define MAXVELY 3 -#define MAXVELZ 3 - -#define MAXPOSX 10 -#define MAXPOSY 10 -#define MAXPOSZ 10 - -#define MAXR1 1 -#define MAXR2 1 -#define MAXR3 1 -#define MAXR4 1 -#define MAXR5 1 -#define MAXR6 1 -#define MAXR7 1 -#define MAXR8 1 -#define MAXR9 1 - -#define MAXW1 40 -#define MAXW2 40 -#define MAXW3 40 - -#define MINVELX -MAXVELX -#define MINVELY -MAXVELY -#define MINVELZ -MAXVELZ - -#define MINPOSX -MAXPOSX -#define MINPOSY -MAXPOSY -#define MINPOSZ -MAXPOSZ - -#define MINR1 -MAXR1 -#define MINR2 -MAXR2 -#define MINR3 -MAXR3 -#define MINR4 -MAXR4 -#define MINR5 -MAXR5 -#define MINR6 -MAXR6 -#define MINR7 -MAXR7 -#define MINR8 -MAXR8 -#define MINR9 -MAXR9 - -#define MINW1 -MAXW1 -#define MINW2 -MAXW2 -#define MINW3 -MAXW3 - - -typedef struct _SELF_OBS -{ - - float px; - float py; - float pz; - - float vx; - float vy; - float vz; - - float r1; - float r2; - float r3; - float r4; - float r5; - float r6; - float r7; - float r8; - float r9; - - float wx; - float wy; - float wz; - -}self_obs; - - -void updateSelfObservation(self_obs*); -Vector3 getPosition(void); -Vector3 getVeloc(void); -void clipPosition(Vector3*); -void clipVelocity(Vector3*); - -#endif \ No newline at end of file diff --git a/examples/mean_embed/include/vec.h b/examples/mean_embed/include/vec.h deleted file mode 100644 index 166fe462e7..0000000000 --- a/examples/mean_embed/include/vec.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef __VEC_H__ -#define __VEC_H__ - -typedef struct _Vec3 -{ - float x; - float y; - float z; -}Vector3; - -typedef struct _Vec6 -{ - float a; - float b; - float c; - float d; - float e; - float f; -}Vector6; - -float calcNormf(Vector3); -float calcMeanMatrixf(float**, float*, int); -Vector3 addVecf(Vector3, Vector3); -Vector3 substractVec(Vector3, Vector3); -Vector3 negateVec(Vector3 vec); -void scaleVec(float*, float, int); -float dotProdf(float*, float*); -float clipVal(float, float, float); -float calcDistf(Vector3, Vector3); -float normf(Vector3); - - -#endif \ No newline at end of file diff --git a/examples/mean_embed/src/Kbuild b/examples/mean_embed/src/Kbuild deleted file mode 100644 index 4143b4e75c..0000000000 --- a/examples/mean_embed/src/Kbuild +++ /dev/null @@ -1,8 +0,0 @@ -obj-y += coll_avoid_main.o -obj-y += neighbor_encoder.o -obj-y += mlp.o -obj-y += neighbor_observation.o -obj-y += nn.o -obj-y += self_encoder.o -obj-y += self_observation.o -obj-y += vec.o diff --git a/examples/mean_embed/src/coll_avoid_main.c b/examples/mean_embed/src/coll_avoid_main.c deleted file mode 100644 index a9d4bd52e4..0000000000 --- a/examples/mean_embed/src/coll_avoid_main.c +++ /dev/null @@ -1,193 +0,0 @@ -/** - * ,---------, ____ _ __ - * | ,-^-, | / __ )(_) /_______________ _____ ___ - * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * | / ,--ยด | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2019 Bitcraze AB - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, in version 3. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - * - * hello_world.c - App layer application of a simple hello world debug print every - * 2 seconds. - */ - - -#include -#include -#include - -#include "app.h" - -#include "FreeRTOS.h" -#include "task.h" - -#include "debug.h" -#include "../include/nn.h" -#include "../include/neighbor_observation.h" -#include "../include/self_observation.h" -#include "radiolink.h" -#include "param.h" -#include "log.h" -#include "configblock.h" -#include "stabilizer_types.h" -#include "commander.h" -#include "../include/coll_avoid_main.h" - - - -static uint8_t state = 0; -static float thrusts[4]; -static int16_t thrustsToMotor[4]; -static uint64_t timer = 0; -static PacketData ownPacket; -static uint8_t isHighLevelController = 1; -static uint8_t isMeanEmbed = 0; - -void communicate(); - -void communicate() -{ - if (timer == ownPacket.id) - { - ownPacket.pos = getPosition(); - ownPacket.vel = getVeloc(); - P2PPacket packet; - packet.port = 0; - packet.size = sizeof(PacketData); - memcpy(packet.data, &ownPacket, sizeof(PacketData)); - radiolinkSendP2PPacketBroadcast(&packet); - } -} - -void getThrusts(int16_t* thrst) -{ - thrst[0] = thrustsToMotor[0]; - thrst[1] = thrustsToMotor[1]; - thrst[2] = thrustsToMotor[2]; - thrst[3] = thrustsToMotor[3]; -} - -uint8_t isHighLevel() -{ - return isHighLevelController; -} - -void appMain() { // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - - uint64_t address = configblockGetRadioAddress(); - uint8_t my_id =(uint8_t)((address) & 0x00000000ff); - ownPacket.id = my_id; - static setpoint_t setpoint; - static Vector3 lastPosition; - - while(1) { - vTaskDelay(M2T(10)); - lastPosition = getPosition(); - //communicate(); - timer++; - - if(timer >= N_DRONES) - { - timer = 0; - } - - switch (state) - { - case 0: // do nothing - break; - case 1: // start - if(isHighLevel()) - { - setpoint.mode.x = modeAbs; - setpoint.mode.y = modeAbs; - setpoint.mode.z = modeAbs; - - setpoint.position.x = lastPosition.x; - setpoint.position.y = lastPosition.y; - setpoint.position.z = .9f; - - - commanderSetSetpoint(&setpoint, 3); - } - else{ - if(!isMeanEmbed) - { - consolePrintf("entered app main mean embed \n"); - isMeanEmbed = 1; - } - feedForwardNN(thrusts); - for(int k = 0; k < 4; k++) - { - thrustsToMotor[k] = (int16_t)(INT16_MAX*thrusts[k]); - } - } - break; - case 2: // land - - if(isHighLevel()) - { - setpoint.mode.x = modeAbs; - setpoint.mode.y = modeAbs; - setpoint.mode.z = modeAbs; - - setpoint.position.x = lastPosition.x; - setpoint.position.y = lastPosition.y; - setpoint.position.z = .05f; - - if(getPosition().z < 0.15f) - { - setpoint.mode.x = modeDisable; - setpoint.mode.y = modeDisable; - setpoint.mode.z = modeDisable; - - state = 0; - } - commanderSetSetpoint(&setpoint, 3); - } - else{ - for(int k = 0; k < 4; k++) - { - thrustsToMotor[k] = (int16_t)(INT16_MAX*0); - } - } - - break; - default: - break; - } - } -} - -PARAM_GROUP_START(ctrlnn) -PARAM_ADD(PARAM_FLOAT, thrst1, &thrusts[0]) -PARAM_ADD(PARAM_FLOAT, thrst2, &thrusts[1]) -PARAM_ADD(PARAM_FLOAT, thrst3, &thrusts[2]) -PARAM_ADD(PARAM_FLOAT, thrst4, &thrusts[3]) -PARAM_ADD(PARAM_UINT8, stt, &state) -PARAM_ADD(PARAM_UINT8, id, &(ownPacket.id)) -PARAM_ADD(PARAM_UINT8, controller, &isHighLevelController) -//PARAM_ADD(PARAM_INT32, int, &dbgint) -PARAM_GROUP_STOP(ctrlnn) - - -LOG_GROUP_START(logNN) -LOG_ADD(LOG_FLOAT, thrst1, &thrusts[0]) -LOG_ADD(LOG_FLOAT, thrst2, &thrusts[2]) -LOG_ADD(LOG_FLOAT, thrst3, &thrusts[3]) -LOG_ADD(LOG_FLOAT, thrst4, &thrusts[4]) -LOG_GROUP_STOP(logNN) \ No newline at end of file diff --git a/examples/mean_embed/src/mlp.c b/examples/mean_embed/src/mlp.c deleted file mode 100644 index af0eb4008b..0000000000 --- a/examples/mean_embed/src/mlp.c +++ /dev/null @@ -1,190 +0,0 @@ -#include "../include/mlp.h" - -static const float phi_a_w0[OUTPUT0][PHI_A_H] = - {{-0.0712, -0.1064, 0.0829, -0.0613, -0.1034, 0.1828, -0.1816, -0.1680, - 0.2552, -0.0522, 0.1578, 0.2222, 0.2968, 0.0967, -0.1428, -0.1108, - -0.4192, 0.2888, -0.2358, -0.0817, -0.0972, -0.1711, 0.0574, -0.2275}, - { 0.2312, -0.1308, 0.2763, -0.3347, -0.1600, -0.3166, 0.1986, 0.1450, - 0.0752, -0.3626, -0.1812, 0.0386, 0.0553, 0.1420, 0.1220, 0.2385, - 0.1138, -0.0805, -0.0999, 0.0641, 0.0791, -0.0325, -0.1671, 0.3535}, - { 0.3482, 0.0388, 0.1835, -0.0676, -0.2657, -0.0429, 0.0184, -0.2028, - 0.1248, -0.2311, 0.3759, 0.2991, 0.2532, 0.1218, -0.2372, -0.2879, - -0.2200, 0.2978, -0.0530, 0.1220, -0.0325, -0.1115, -0.0511, 0.0037}, - {-0.1948, -0.2005, -0.1613, -0.1941, 0.1435, -0.2193, 0.0984, -0.3325, - -0.0653, 0.0868, 0.1395, -0.3700, -0.0590, 0.2330, -0.1407, 0.2554, - 0.1562, 0.2448, -0.0649, -0.2819, 0.0528, 0.0479, -0.1232, -0.2559}, - {-0.3058, 0.0844, -0.2721, 0.2093, 0.1553, -0.1531, -0.3528, 0.0954, - 0.1921, 0.0520, -0.3780, 0.0343, -0.3395, 0.0299, -0.3348, 0.2086, - 0.1775, 0.2092, -0.2513, -0.3729, 0.1111, 0.0518, -0.2350, -0.1031}, - {-0.3280, 0.0304, -0.1232, -0.3138, 0.0150, -0.1558, -0.1337, 0.2085, - -0.0798, 0.0611, -0.1580, -0.0429, -0.0222, -0.2424, 0.1159, -0.1876, - -0.2442, 0.2854, -0.2007, -0.2001, 0.2215, 0.1065, 0.1947, -0.2235}, - {-0.3183, -0.0354, -0.1625, -0.2739, 0.2576, 0.0871, -0.0959, -0.1109, - -0.0473, -0.0672, 0.0345, 0.2020, -0.2763, 0.0608, -0.3146, 0.2149, - 0.1724, 0.1308, 0.1112, 0.0560, -0.0192, 0.1338, -0.2540, -0.1687}, - {-0.2828, -0.2917, 0.0211, -0.2895, 0.1079, 0.0627, -0.1996, -0.1104, - -0.2181, -0.2593, 0.0508, 0.0641, -0.1492, 0.1129, -0.1922, 0.1868, - -0.0504, 0.2750, -0.2739, -0.0546, 0.2708, 0.1060, 0.0249, -0.0984}, - { 0.1434, -0.2773, -0.0919, -0.1473, 0.1378, -0.2877, 0.0709, -0.1354, - -0.0957, 0.1281, -0.2401, -0.1320, 0.0311, -0.2665, 0.1775, -0.1598, - -0.1272, -0.3746, -0.3200, -0.1464, 0.1128, -0.1672, 0.2830, 0.3378}, - {-0.0960, -0.1668, 0.0216, -0.0725, -0.3077, -0.3070, -0.2707, 0.2228, - 0.3534, -0.1011, 0.1424, -0.1142, -0.2399, 0.2151, -0.1116, 0.2720, - 0.0810, -0.1186, -0.1141, 0.0393, 0.2152, -0.0196, -0.3592, -0.0420}, - {-0.2989, 0.0974, -0.0752, 0.2167, 0.2403, 0.0860, 0.3203, -0.1122, - 0.2362, 0.0248, 0.2084, 0.1807, -0.2074, 0.3658, -0.2251, 0.1363, - 0.2378, -0.4014, -0.0138, 0.1672, 0.0964, 0.3507, 0.2466, 0.2364}, - { 0.0509, 0.2209, 0.2165, -0.1340, -0.1855, -0.2989, -0.1612, 0.0684, - -0.3344, -0.0668, -0.2063, 0.2429, 0.0654, 0.0823, 0.1671, -0.1607, - 0.0412, 0.1792, 0.4476, -0.4720, 0.0696, -0.3830, -0.0732, 0.1058}, - { 0.2792, 0.2259, 0.0741, -0.1851, 0.1547, 0.0414, -0.1482, -0.0530, - -0.1750, 0.2782, -0.2334, -0.0488, 0.2091, 0.1727, -0.3492, 0.1027, - -0.1024, 0.1988, 0.3037, -0.0861, 0.2612, 0.3353, 0.5178, 0.1874}, - {-0.2915, 0.1257, -0.0879, -0.2291, -0.2712, 0.0894, 0.0048, 0.0468, - -0.0638, 0.1943, 0.2690, -0.1886, 0.0456, -0.1509, -0.0105, -0.2355, - 0.1048, -0.2126, -0.1006, -0.2675, -0.1555, -0.2203, 0.2665, -0.2718}, - { 0.1799, 0.0644, -0.1061, -0.1791, 0.1467, 0.3779, 0.1611, 0.0735, - -0.4474, -0.2905, -0.0619, -0.2220, 0.2301, -0.0976, 0.0798, 0.2212, - 0.0831, 0.3045, 0.2622, -0.0194, -0.2424, -0.0424, 0.5804, 0.1006}, - { 0.0828, -0.0055, -0.1092, 0.3157, -0.0458, 0.2426, -0.2037, -0.2936, - 0.2033, 0.2721, 0.0187, -0.3140, -0.1800, -0.3168, 0.0729, 0.0910, - -0.1596, -0.1677, 0.1133, 0.0136, 0.0326, 0.0662, -0.1121, -0.2236}, - {-0.1296, -0.0080, -0.2178, 0.2457, 0.0725, 0.3180, -0.1449, -0.3226, - 0.1605, 0.2144, 0.2629, 0.1828, 0.2240, 0.0986, 0.1336, -0.0244, - -0.2578, 0.1273, -0.6254, -0.1470, 0.1993, -0.1277, 0.2026, -0.1687}, - {-0.2050, 0.2352, -0.3164, -0.2756, -0.3283, -0.1105, 0.0261, -0.0186, - -0.1072, 0.0438, 0.0390, -0.2885, 0.2076, 0.2931, -0.2257, 0.1590, - -0.0656, 0.0989, 0.1377, -0.2657, -0.0067, -0.2681, -0.2660, -0.1745}, - { 0.2315, -0.1074, 0.0958, -0.3056, -0.1739, 0.0148, 0.0237, -0.2700, - -0.1113, 0.0654, -0.3063, -0.0856, 0.0495, 0.2827, 0.0913, -0.1682, - -0.0032, -0.0109, 0.3569, -0.0075, -0.0014, 0.2144, 0.4364, 0.1786}, - { 0.3324, -0.1227, 0.0230, 0.0726, 0.0831, 0.0845, -0.0187, -0.3424, - 0.5108, 0.1941, -0.1455, 0.0465, 0.1754, 0.0468, -0.2300, -0.2807, - 0.2723, 0.1669, 0.1417, 0.2825, -0.2002, -0.0971, 0.1336, -0.0616}, - { 0.2706, 0.0883, -0.1059, -0.1534, -0.1794, 0.1086, 0.2069, 0.0779, - -0.1473, 0.0046, 0.1069, -0.0019, 0.0320, -0.4350, 0.0361, 0.3017, - 0.2567, -0.0609, 0.4020, -0.3362, 0.1702, -0.2900, -0.0054, 0.1079}, - { 0.2471, -0.1640, 0.3448, 0.0487, -0.0395, -0.2338, -0.1029, 0.0334, - 0.1258, -0.1725, -0.2195, 0.1711, -0.1671, -0.1898, 0.2942, 0.3279, - -0.0009, -0.6206, 0.0455, 0.1578, -0.0771, 0.1008, -0.1259, -0.0761}, - {-0.0953, 0.1526, 0.4998, 0.1582, 0.3574, -0.1884, -0.0804, -0.1895, - 0.3223, -0.0182, 0.1742, 0.1203, -0.2689, 0.2590, 0.2118, 0.1036, - 0.1040, -0.4601, -0.2876, 0.0326, 0.3799, 0.2296, 0.1505, 0.0106}, - {-0.2505, -0.3978, -0.2441, 0.2073, 0.1664, 0.0420, 0.2092, -0.0649, - -0.1656, -0.2019, 0.0421, 0.0787, -0.4329, 0.1727, 0.0292, 0.0533, - -0.4603, 0.2112, 0.2096, -0.4242, 0.1540, -0.2893, 0.2056, 0.0827}, - { 0.2250, 0.0183, -0.0879, 0.1550, -0.2158, -0.2641, -0.1416, -0.0999, - 0.0886, -0.2420, 0.1847, -0.0487, 0.2623, -0.2057, -0.2274, -0.1562, - -0.0188, -0.0452, -0.1617, 0.1200, -0.1980, 0.3523, 0.1085, 0.2039}, - {-0.2841, -0.0308, -0.3209, -0.0574, -0.0445, -0.1532, -0.1391, 0.0121, - -0.0189, 0.2393, 0.2106, -0.2015, -0.0446, -0.2435, -0.3088, -0.2461, - -0.1615, -0.0368, 0.0939, -0.2100, -0.3404, 0.3396, -0.1521, -0.2210}, - { 0.1976, 0.0829, -0.2833, 0.0031, -0.5334, -0.0234, -0.3062, 0.1773, - 0.2994, -0.1035, 0.0785, 0.1398, -0.0689, 0.0786, -0.1912, 0.0058, - 0.1308, -0.2160, -0.1852, -0.0266, -0.0044, -0.2206, -0.1520, -0.4545}, - {-0.2509, -0.0386, 0.0104, -0.0276, 0.1403, -0.2632, -0.1365, 0.0635, - -0.3449, -0.3634, -0.1965, 0.0138, 0.0870, 0.2508, -0.1682, 0.0956, - 0.1881, 0.0100, 0.0503, -0.1770, 0.0773, -0.1674, 0.1090, 0.1298}, - { 0.0342, -0.2648, -0.0472, 0.1023, 0.0362, 0.1506, 0.2156, 0.1036, - 0.0771, -0.2933, 0.2129, 0.2955, 0.2300, 0.3041, -0.1129, -0.2748, - 0.0255, 0.0577, 0.0565, -0.0093, 0.3067, -0.0508, -0.0673, 0.1449}, - { 0.1143, -0.3537, -0.1388, 0.1410, -0.0894, -0.2456, 0.2090, -0.2197, - -0.3265, 0.1082, 0.0740, -0.1193, -0.2859, 0.2902, 0.1274, -0.1502, - -0.0144, -0.1391, 0.4043, 0.1775, 0.0081, -0.2247, 0.0119, 0.0472}, - { 0.2873, 0.0246, -0.1145, -0.1128, -0.1429, -0.3801, 0.0354, 0.3041, - 0.2344, 0.0782, -0.1879, 0.2963, 0.2024, -0.0992, 0.3164, -0.2065, - 0.0393, -0.2267, -0.4688, -0.1743, -0.2090, -0.0185, -0.0593, -0.1863}, - { 0.2873, 0.3362, -0.0536, 0.2923, 0.3511, 0.1803, 0.1132, -0.1624, - 0.3294, -0.1993, -0.1402, -0.2640, -0.2047, 0.0009, -0.3328, -0.1201, - -0.0204, 0.3771, -0.1442, 0.0668, 0.0300, -0.2818, 0.2694, 0.2016}}; - - - -static const float phi_a_w1[PHI_A_V][OUTPUT0] = - {{ 0.3329, -0.1276, 0.0918, 0.2119, 0.3917, -0.3107, -0.2685, -0.1820, - -0.0174, -0.0038, -0.1845, 0.2183, 0.0061, 0.3297, -0.0361, 0.5149, - -0.1369, -0.3979, -0.1449, 0.3987, 0.4457, -0.1420, -0.1927, 0.0442, - -0.0847, -0.0561, 0.1585, -0.3072, -0.3367, -0.1182, -0.2269, 0.3008}, - { 0.3090, -0.0101, -0.3483, 0.0950, -0.1629, 0.0155, -0.0219, -0.3649, - -0.1861, 0.0499, 0.3040, -0.0118, -0.3855, 0.2326, -0.4366, 0.4052, - -0.0648, -0.2014, -0.4505, 0.3231, 0.1331, 0.1894, 0.2207, -0.1932, - -0.2741, -0.3451, 0.1244, 0.0371, -0.2939, -0.3118, 0.1921, 0.1167}, - { 0.2376, -0.1729, 0.4389, -0.0996, -0.1011, -0.0809, 0.1965, 0.3883, - 0.0138, 0.3260, -0.4577, -0.3590, -0.2023, -0.0061, -0.4515, 0.0480, - 0.2738, -0.0590, -0.1110, 0.1829, -0.2011, 0.2585, -0.0424, 0.2681, - 0.3659, 0.3033, 0.2925, 0.0921, -0.0036, -0.2495, 0.3795, -0.0715}, - { 0.4243, -0.3684, 0.3214, -0.0147, -0.3961, -0.3262, -0.1087, -0.2791, - 0.3640, 0.3414, 0.0576, 0.0807, 0.2526, -0.3599, -0.0896, -0.3003, - 0.1847, 0.4085, -0.1963, 0.1871, -0.2922, -0.4090, 0.0260, 0.1939, - 0.1729, 0.0977, -0.0352, -0.0624, 0.1996, -0.2464, -0.3536, 0.2300}}; - - - - -static const float phi_a_b0[OUTPUT0] = { 0.0354, 0.0430, -0.0494, -0.0428, -0.1345, -0.0336, -0.0551, -0.0281, - -0.0599, -0.0498, 0.1913, -0.0012, 0.1343, -0.0535, 0.1148, 0.0752, - -0.0608, -0.0023, -0.0280, 0.0357, 0.0447, -0.0258, 0.0825, -0.2215, - -0.0424, -0.0056, -0.0749, -0.0908, 0.0577, -0.0546, -0.0545, 0.1290}; - - -static const float phi_a_b1[PHI_A_V] = {-0.0033, 0.0984, -0.1976, 0.1254}; - - -static inpOutVec* networkVecs; - -static void feedForwardMLP(float*); -static void feedForwardPhiA0(float*, int); -static void feedForwardPhiA1(float*, int); - -void calcMlpOutput(mlpInput* input, float* out) -{ - - feedForwardMLP(input->inputVec); - for(int k = 0; k < 4; k++) - { - out[k] = networkVecs->phi_a[k]; - } -} - -static void feedForwardMLP(float* inp) -{ - - for(int k = 0; k < 32; k++) - { - feedForwardPhiA0(inp, k); - } - - for(int k = 0; k < 4; k++) - { - feedForwardPhiA1(networkVecs->out0, k); - } - -} - -static void feedForwardPhiA0(float* inp, int raw) -{ - float tmp = 0; - - for(int k = 0; k < 24; k++) - { - tmp += inp[k]*phi_a_w0[raw][k]; - } - - networkVecs->out0[raw] = (tmp + phi_a_b0[raw] >= 0) ? tmp + phi_a_b0[raw] : 0; - -} - -static void feedForwardPhiA1(float* inp, int raw) -{ - float tmp = 0; - for(int k = 0; k < 24; k++) - { - tmp += inp[k]*phi_a_w1[raw][k]; - } - networkVecs->phi_a[raw] = tmp + phi_a_b1[raw]; -} - - - - - diff --git a/examples/mean_embed/src/neighbor_encoder.c b/examples/mean_embed/src/neighbor_encoder.c deleted file mode 100644 index 61166bb279..0000000000 --- a/examples/mean_embed/src/neighbor_encoder.c +++ /dev/null @@ -1,186 +0,0 @@ -#include "../include/neighbor_encoder.h" -#include "../include/vec.h" -#include "../include/neighbor_observation.h" - - -// static variables -//////////////////////////////////////// WEIGHTS ///////////////////////////////////////////////// -typedef struct ENCODER_VECTORS -{ - float inp[6]; - float out0[8]; - float out1[8]; -}encoderVectors; - -static const float psi_eta_w0[PSI_ETA_V][PSI_ETA_H] = {{ 0.5417, -0.6794, -0.6421, 0.0920, -0.2375, -0.1385}, - { 0.5207, -0.7076, -0.4285, -0.1278, 0.3393, -0.1915}, - { 0.4607, -0.6019, -0.3728, -0.3768, 0.4533, 0.2590}, - { 0.5416, -0.5736, 0.0822, -0.4494, 0.4473, 0.0059}, - {-0.0306, -0.1329, 0.1382, 0.1680, -0.0375, -0.3448}, - {-0.4118, 0.3257, 0.2667, -0.4095, -0.0998, -0.2194}, - { 0.8233, -0.1962, 0.1271, -0.1577, -0.2611, 0.2663}, - { 0.3432, -0.3766, -0.7890, 0.0856, 0.2210, 0.2198}}; - - - -static const float psi_eta_w1[PSI_ETA_V][PSI_ETA_V] = {{-3.2516e-02, -3.8135e-01, 2.4380e-01, -3.0701e-01, -1.7432e-01, 3.5791e-01, 2.9315e-01, -1.3260e-01}, - { 1.2666e-03, 2.2348e-01, 3.3893e-01, -6.0730e-01, 3.4299e-01, -6.8567e-01, -1.1741e+00, -3.3565e-01}, - {-8.4525e-01, -6.0548e-01, -5.9543e-01, 4.0336e-02, -2.1229e-01, -2.7084e-01, -2.0249e-01, 2.6816e-01}, - { 3.0907e-01, 1.4779e-01, -5.6850e-01, 3.8580e-01, -1.3173e-01, 5.4307e-01, -2.3703e-01, -5.3131e-01}, - {-5.9161e-01, 3.2005e-02, 4.0227e-01, 4.1339e-01, -2.2896e-02, -1.7285e-02, -2.4415e-01, -4.6611e-01}, - {-7.8942e-02, 3.7274e-01, 4.5881e-01, -2.9711e-01, 5.6955e-01, -2.3767e-01, -1.8596e-01, -8.4011e-01}, - {-7.7198e-01, -5.1864e-01, 5.4855e-01, -1.8149e-01, 4.5347e-01, -7.9741e-02, -7.5352e-02, -1.6073e-01}, - {-8.1760e-01, -2.0768e-01, 3.6510e-01, 1.9212e-01, -7.1986e-04, 4.5544e-02, -1.7139e-01, -4.4614e-01}}; - - - - - - - -static const float psi_eta_b0[PSI_ETA_V] = { 0.0905, 0.1520, -0.1166, 0.0220, 0.2115, 0.0347, 0.1080, 0.1474}; - - - - - - - - -static const float psi_eta_b1[PSI_ETA_V] = { 0.2088, 0.2443, 0.2780, 0.1496, -0.0144, 0.1181, 0.1876, 0.2691}; - - - - - - -static encoderVectors encoderVecs; - - -static const int KNeighbors = (int)K_NEIGHBORS; - -// static functions -static void calcMean(float*); -static int getAmountKNeighbors(); -static int getAmountOutputs(); -static void feedForwardNeighborEncoder(neighb_obs* inp, float* outp); -static void feedForwardPsiEta(neighb_obs obsK); -static void feedForwardPsiEta0(float* inp, int raw); -static void feedForwardPsiEta1(float* inp, int raw); -static void addVectors(float* arr1, float* arr2, int len); - - -//feed forward functions for psi_eta, adapted for the FPGA -//feed forward variables for psi_eta, adapted for the FPGA - -void calcNeighborEncoderOutput(neighb_obs* inp, float* outp) -{ - feedForwardNeighborEncoder(inp, outp); -} - -static void feedForwardNeighborEncoder(neighb_obs* inp, float* outpEncoder) -{ - int sz = getAmountKNeighbors(); - - for(int k = 0; k < sz; k++) - { - feedForwardPsiEta(inp[k]); - addVectors(encoderVecs.out1,outpEncoder,8); - } - - calcMean(outpEncoder); -} - -static void feedForwardPsiEta(neighb_obs obsK) -{ - int sz = (int)PSI_ETA_V; - - encoderVecs.inp[0] = obsK.relPos.x; - encoderVecs.inp[1] = obsK.relPos.y; - encoderVecs.inp[2] = obsK.relPos.z; - encoderVecs.inp[3] = obsK.relVel.x; - encoderVecs.inp[4] = obsK.relVel.y; - encoderVecs.inp[5] = obsK.relVel.z; - - for(int k = 0; k < sz; k++) - { - feedForwardPsiEta0(encoderVecs.inp, k); - } - - for(int k = 0; k < sz; k++) - { - feedForwardPsiEta1(encoderVecs.out0, k); - } - -} - -static void feedForwardPsiEta0(float* inp, int raw) -{ - int sz = (int)PSI_ETA_H; - float temp = 0.0F; - - for(int k = 0; k < sz; k++) - { - temp += psi_eta_w0[raw][k]*inp[k]; - } - - - temp += psi_eta_b0[raw]; - - encoderVecs.out0[raw] = temp >= 0 ? temp : 0; -} - -static void feedForwardPsiEta1(float* inp, int raw) -{ - int sz = (int)PSI_ETA_V; - float temp = 0.0F; - - for(int k = 0; k < sz; k++) - { - temp += psi_eta_w1[raw][k]*inp[k]; - } - - temp += psi_eta_b1[raw]; - - encoderVecs.out1[raw] = temp >= 0 ? temp : 0; -} - - -static void calcMean(float* networkOut) -{ - float scalar; - - if(getAmountKNeighbors() == 0.0) - { - scalar = 1.0; - } - else - { - scalar = getAmountKNeighbors(); - } - - scaleVec(networkOut, (1.0f)/scalar, getAmountOutputs()); -} - -static int getAmountKNeighbors() -{ - return (int)KNeighbors; -} - -static int getAmountOutputs() -{ - return (int)NEIGHBOR_NETWORK_OUT; -} - -static void addVectors(float* arr1, float* arr2, int len) -{ - - /*perform addition of encoder outputs*/ - int vecLen = len; - - for(int k = 0; k < vecLen; k++) - { - arr2[k] = arr1[k] + arr2[k]; - } -} - diff --git a/examples/mean_embed/src/neighbor_observation.c b/examples/mean_embed/src/neighbor_observation.c deleted file mode 100644 index b2c8e2351b..0000000000 --- a/examples/mean_embed/src/neighbor_observation.c +++ /dev/null @@ -1,131 +0,0 @@ -#include "../include/neighbor_observation.h" -#include "../include/vec.h" -#include "radiolink.h" -#include "../include/self_observation.h" -#include - - - - -static void checkAmountAdded(); - -static PacketData neighborInfo[N_DRONES]; - -static bool isInit = false; -static int collectedK = 0; -static int farthestNeighbor = 0; -static int lastAdded = 0; -static int collectedList[6] = {-1,-1,-1,-1,-1,-1}; - -void p2pcallbackHandler(P2PPacket *p) -{ - PacketData received; - memcpy(&received, p->data, sizeof(PacketData)); - lastAdded = received.id; - neighborInfo[received.id] = received; -} - -void updateNeighbObservation(neighb_obs* kNearestArr) -{ - - if(!isInit) - { - p2pRegisterCB(p2pcallbackHandler); - isInit = true; - } - - Vector3 ownPos = getPosition(); - Vector3 ownVel = getVeloc(); - Vector3 newPos = neighborInfo[lastAdded].pos; - Vector3 newVel = neighborInfo[lastAdded].vel; - - checkAmountAdded(); - if(collectedK == 0) - { - return; - } - else if(collectedK == 1) - { - kNearestArr[0].relPos = substractVec(ownPos, newPos); - kNearestArr[0].relVel = substractVec(ownVel, newVel); - - clipPosition(&(kNearestArr[0].relPos)); - clipVelocity(&(kNearestArr[0].relVel)); - - farthestNeighbor = 0; - - - } - else if((collectedK < 6) && (collectedK > 1)) - { - kNearestArr[collectedK].relPos = substractVec(ownPos, newPos); - kNearestArr[collectedK].relVel = substractVec(ownVel, newVel); - - clipPosition(&(kNearestArr[collectedK].relPos)); - clipVelocity(&(kNearestArr[collectedK].relVel)); - - Vector3 newRelPos = kNearestArr[collectedK].relPos; - - for(int k = 0; k < collectedK; k++) - { - if(calcNormf(newRelPos) < calcNormf(kNearestArr[k].relPos) ) - { - farthestNeighbor = k; - newRelPos = kNearestArr[k].relPos; - } - } - - } - else - { - if(calcDistf(ownPos,newPos) < calcNormf(kNearestArr[farthestNeighbor].relPos) ) - { - - kNearestArr[farthestNeighbor].relPos = substractVec(ownPos, newPos); - kNearestArr[farthestNeighbor].relVel = substractVec(ownVel, newVel); - clipPosition(&(kNearestArr[farthestNeighbor].relPos)); - clipVelocity(&(kNearestArr[farthestNeighbor].relVel)); - - Vector3 checkPos = kNearestArr[0].relPos; - farthestNeighbor = 0; - for(int k = 1; k < 6; k++) - { - if(calcNormf(checkPos) < calcNormf(kNearestArr[k].relPos) ) - { - farthestNeighbor = k; - checkPos = kNearestArr[k].relPos; - } - } - } - - } -} - -static void checkAmountAdded() -{ - - int amountAdded = 0; - int lastIsInList = 0; - for(int k = 0; k < 6; k++) - { - if(collectedList[k] != -1) - { - amountAdded++; - if(collectedList[k] == lastAdded) - { - lastIsInList = 1; - } - } - else - { - if(!lastIsInList) - { - collectedList[k] = lastAdded; - amountAdded++; - } - break; - } - } - - collectedK = amountAdded; -} \ No newline at end of file diff --git a/examples/mean_embed/src/nn.c b/examples/mean_embed/src/nn.c deleted file mode 100644 index e7f9740af3..0000000000 --- a/examples/mean_embed/src/nn.c +++ /dev/null @@ -1,109 +0,0 @@ -#include "../include/nn.h" -#include "../include/mlp.h" -#include "../include/neighbor_encoder.h" -#include "../include/self_encoder.h" -#include "../include/self_observation.h" -#include "../include/neighbor_observation.h" -#include "../include/vec.h" -#include "log.h" -#include "param.h" - -#include - - - - static nnInputOutput* nnIO; - static self_obs* selfObservation; - - static float selfObservationArr[3]; - static neighb_obs kNearestObservations[6]; - static mlpInput* mlpInp; - static uint8_t isInitialized = 0; - -static void nnInit(); - -static void nnInit() -{ - // for(int k = 0; k < 18; k++) - // { - // selfObservation->self_obs_arr[k] = 0.0; - // } - - // selfObservation->obsLen = 18; - mlpInp->len = 24; - - for(int k = 0; k < 6; k++) - { - kNearestObservations[k].obsLen = 6; - } -} - - -void feedForwardNN(float* thrusts) -{//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - if(!isInitialized) - { - nnInit(); - isInitialized = 1; - } - - updateSelfObservation(selfObservation); - - float inparr[18]; - - selfObservationArr[0] = selfObservation->px; - selfObservationArr[1] = selfObservation->py; - selfObservationArr[2] = selfObservation->pz; - - inparr[0] = selfObservation->px; - inparr[1] = selfObservation->py; - inparr[2] = selfObservation->pz; - inparr[15] = selfObservation->wx; - inparr[16] = selfObservation->wy; - inparr[17] = selfObservation->wz; - - inparr[3] = selfObservation->vx; - inparr[4] = selfObservation->vy; - inparr[5] = selfObservation->vz; - - inparr[6] = selfObservation->r1; - inparr[7] = selfObservation->r2; - inparr[8] = selfObservation->r3; - inparr[9] = selfObservation->r4; - inparr[10] = selfObservation->r5; - inparr[11] = selfObservation->r6; - inparr[12] = selfObservation->r7; - inparr[13] = selfObservation->r8; - inparr[14] = selfObservation->r9; - - calcSelfEncoderOutput(inparr, nnIO->outputSelf); - updateNeighbObservation(kNearestObservations); - calcNeighborEncoderOutput(kNearestObservations, nnIO->outputNeighbor); - - for(int k = 0; k < 16; k++) - { - mlpInp->inputVec[k] = nnIO->outputSelf[k]; - } - - for(int k = 0; k < 8; k++) - { - mlpInp->inputVec[k+16] = nnIO->outputNeighbor[k]; - } - - calcMlpOutput(mlpInp, nnIO->output); - - for(int k = 0; k < 4; k++) - { - nnIO->output[k] = 0.5f*(clipVal(nnIO->output[k], -1.0f, 1.0f)+1.0f); - thrusts[k] = nnIO->output[k]; - } - - - -} - -LOG_GROUP_START(selfObsLog) -LOG_ADD(LOG_FLOAT, observedPx, &(selfObservationArr[0])) -LOG_ADD(LOG_FLOAT, observedPy, &(selfObservationArr[1])) -LOG_ADD(LOG_FLOAT, observedPz, &(selfObservationArr[2])) -LOG_GROUP_STOP(selfObsLog) diff --git a/examples/mean_embed/src/p2p_comm.c b/examples/mean_embed/src/p2p_comm.c deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/examples/mean_embed/src/self_encoder.c b/examples/mean_embed/src/self_encoder.c deleted file mode 100644 index 7d254d01b8..0000000000 --- a/examples/mean_embed/src/self_encoder.c +++ /dev/null @@ -1,125 +0,0 @@ -#include "../include/self_encoder.h" -#include "../include/self_observation.h" - -static const float psi_s_w0[PSI_S_V][PSI_S_H] = - {{-0.0662, 0.3452, -0.3113, 0.1355, -0.1227, 0.1908, 0.0846, 0.2394, 0.6991, -0.4368, -0.0878, 0.7022, 0.3299, 0.3813, -0.1453, 0.1490, -0.3326, 0.0430}, - {-0.4683, -0.1766, 0.1659, -0.2728, 0.2527, 0.0585, -0.0269, 0.1027, 0.0485, -0.0892, -0.4915, -0.5159, 0.1689, 0.1008, -0.3050, 0.1804, 0.0541, -0.1983}, - { 0.2333, -0.2491, 0.1221, -0.0032, -0.1882, 0.0468, 0.1509, 0.4143, 0.0279, -0.5042, -0.2667, -0.4767, -0.1078, -0.6814, 0.0076, -0.2941, 0.0747, -0.0172}, - { 0.2144, -0.1406, 0.2780, 0.4513, 0.1943, -0.1525, 0.2180, 0.1255, -0.2388, -0.1287, 0.3059, 0.0472, 0.0552, 0.3991, 0.0157, -0.0153, -0.1694, -0.0279}, - {-0.0246, -0.2286, 0.0516, -0.0038, 0.0427, -0.0857, -0.3547, -0.2948, 0.3701, -0.3303, -0.0064, -0.6275, -0.2725, -0.0279, -0.0233, 0.0866, -0.0221, 0.2275}, - {-0.3314, -0.2148, 0.1179, 0.1803, -0.0418, -0.3289, 0.1683, 0.2426, 0.3655, -0.2856, 0.0922, 0.2120, 0.3931, 0.3006, 0.0901, -0.0054, -0.2338, -0.0853}, - {-0.1393, -0.2347, 0.0131, -0.2020, -0.3871, 0.3052, -0.4132, 0.0293, 0.0044, -0.0433, -0.4579, -0.0112, 0.1834, 0.1635, -0.2135, -0.2554, -0.2808, 0.3194}, - { 0.6233, 0.0428, -0.2972, 0.3854, 0.1641, -0.1369, -0.0290, 0.1973, 0.6751, -0.3663, -0.4210, 0.2197, 0.0473, 0.1738, 0.5842, 0.3289, -0.0873, 0.3057}, - {-0.0086, 0.0979, -0.0659, 0.2710, -0.1330, -0.2632, 0.2494, 0.0199, 0.1356, 0.1464, -0.2420, -0.0307, 0.1209, -0.2244, -0.1368, 0.0175, -0.3494, 0.1585}, - { 0.2642, -0.0339, -0.5516, -0.2765, -0.0681, -0.4706, -0.3218, 0.2690, -0.2179, -0.0846, -0.1271, 0.1376, -0.3665, -0.3318, 0.1813, -0.2635, -0.1976, 0.1428}, - {-0.2205, 0.2181, 0.0107, -0.0930, 0.3627, -0.0281, 0.0598, 0.0595, -0.4494, -0.1360, -0.1005, 0.5787, 0.0489, 0.0827, 0.3912, 0.1166, 0.2322, 0.0515}, - { 0.2817, -0.5139, -0.0217, 0.2580, -0.1518, 0.2917, -0.5457, 0.3771, 0.5212, -0.3686, -0.2467, -0.3812, -0.1057, -0.5984, 0.0037, -0.3689, -0.2234, -0.0347}, - {-0.1918, -0.1030, -0.6120, 0.0584, -0.1612, -0.1658, -0.3394, -0.1159, -0.0414, -0.1935, 0.0199, -0.6832, -0.1298, -0.0702, 0.2351, -0.1735, 0.0778, 0.1048}, - { 0.1406, -0.0630, 0.0402, -0.1023, -0.1967, -0.1072, 0.3123, 0.4231, 0.4700, 0.0201, -0.4529, 0.3023, -0.0613, -0.0145, 0.0573, -0.3353, -0.1141, 0.2823}, - { 0.2242, -0.1356, -0.2215, -0.2183, -0.1892, -0.2922, 0.0647, -0.3096, 0.1227, 0.0945, -0.1477, -0.5202, 0.6107, -0.0830, -0.2246, -0.0332, -0.3552, -0.3099}, - {-0.2605, 0.1200, -0.2905, -0.2414, -0.1508, 0.0824, -0.4199, 0.4743, -0.7421, -0.3876, 0.3022, 0.1035, -0.5520, -0.2008, 0.0393, -0.0935, 0.3028, 0.1908}}; - - - - -static const float psi_s_w1[PSI_S_V][PSI_S_V] = - {{ 1.9581e-01, 5.3327e-02, 3.6715e-01, -1.1817e-01, 2.8837e-01,9.8057e-02, 7.4590e-02, 5.7743e-01, 8.0308e-02, 4.2634e-01,4.3457e-01, 4.7408e-01, 3.8704e-01, 3.0703e-01, -4.0900e-01,3.0138e-01 }, - {-1.6066e-01, 4.9240e-01, -4.2024e-01, -1.4680e-01, -5.1533e-01,7.5000e-02, 1.7595e-01, -4.2977e-02, -2.4217e-01, 2.5096e-01,-2.5555e-03, -4.1754e-01, -3.2590e-02, -2.4556e-01, 1.5678e-01,3.7005e-01 }, - {-1.3250e-01, 1.4054e-01, -2.6032e-01, 2.5570e-01, -1.1610e-01,1.9535e-01, 8.3425e-02, -5.0316e-02, 2.1941e-01, -3.9512e-01,-4.1106e-01, 1.8835e-01, 3.1899e-04, -2.2109e-01, -1.0922e-01,-1.1184e-01 }, - { 2.3445e-01, 1.5436e-01, 1.1519e-01, 8.4605e-02, -1.7936e-01,2.7795e-01, -2.3522e-01, 2.7890e-01, -2.6846e-01, 8.3189e-02,7.8286e-03, 2.9490e-01, 4.5604e-01, 9.2201e-03, 3.1857e-01,-2.7782e-01 }, - { 6.1801e-02, 3.8141e-01, 1.5012e-01, 9.6598e-02, -3.9146e-01,-3.8188e-01, -2.8867e-01, -5.0792e-02, -1.0945e-02, -6.9323e-02,4.2257e-01, 1.2646e-01, -3.2491e-02, -2.3882e-01, 8.4930e-02,-1.8734e-01 }, - { 3.4958e-01, -1.5469e-01, -4.8494e-01, -2.2917e-01, -1.2637e-01,4.1461e-01, 1.9887e-01, -1.8798e-01, 2.8988e-01, -6.9427e-02,4.2089e-01, -2.1884e-01, 2.9137e-01, -4.8625e-02, -2.1367e-01,3.1088e-01 }, - { 2.8374e-02, -2.4576e-01, -4.0544e-02, -4.8397e-01, 3.3958e-01,-3.6039e-02, 3.1874e-01, -1.5216e-01, -3.6724e-01, -2.9763e-01,3.7866e-01, 1.4657e-01, -2.1168e-01, -3.5354e-01, -2.0552e-01,-1.5328e-01 }, - { 9.3689e-02, -3.1999e-01, 2.7953e-01, 2.8219e-01, 2.4587e-01,3.4304e-01, -1.8565e-01, -3.4398e-01, -2.3130e-01, -4.5904e-01,1.6217e-01, 1.0514e-01, -3.1147e-01, -1.1060e-01, 2.1064e-01,9.8844e-02 }, - { 1.1079e-01, -1.0003e-01, -1.7026e-01, -2.7861e-01, -2.8503e-01,3.0852e-01, -1.6946e-01, 2.7983e-01, -2.0090e-01, 3.4971e-01,-3.1150e-01, 2.3411e-02, 2.1674e-01, -2.3583e-01, 8.4774e-02,-1.6775e-02 }, - { 3.5231e-01, 3.9967e-01, 2.6734e-01, -2.6958e-02, 4.7094e-02,2.4331e-01, 2.7943e-01, -2.5235e-01, -1.4126e-01, 3.0374e-01,-4.9282e-02, -3.0586e-01, -1.0886e-01, 3.1441e-01, -4.5516e-01,-1.9011e-01 }, - { 1.3537e-02, -3.8242e-02, 3.6228e-01, 4.2222e-02, 3.1370e-01,-1.0915e-01, -1.6676e-01, -3.4093e-01, 1.3025e-01, 3.1174e-01,6.5209e-02, 9.0264e-03, 3.8307e-01, 1.1739e-01, 1.8726e-01,-1.8879e-01 }, - { 3.1748e-01, 1.6982e-01, 4.5055e-01, 1.1478e-01, 1.8107e-01,1.8439e-01, 3.6778e-01, -1.5626e-01, 3.3904e-01, -7.0071e-02,-3.7640e-01, 1.5829e-01, 4.3535e-01, -2.1402e-02, 1.1310e-01,-1.9487e-01 }, - {-4.0641e-01, 7.7237e-02, 1.6306e-01, -2.1728e-01, 2.6732e-01,-9.2251e-02, 2.1546e-01, -1.8759e-01, -2.7305e-01, 1.5863e-01,-1.5372e-01, -2.1420e-01, 1.1866e-01, -1.2151e-02, -2.9213e-01,2.3631e-01 }, - {-3.4916e-01, 6.7106e-03, -1.2481e-01, -3.2595e-01, 1.5743e-01,-1.8672e-02, 8.6956e-02, -2.7569e-01, 5.5803e-02, -1.8629e-01,-5.7327e-02, 3.5916e-01, 3.7832e-01, -1.7097e-01, 4.0584e-01,-2.3531e-01 }, - { 3.7445e-01, -3.7032e-01, 3.1318e-02, -2.9446e-01, 4.3544e-01,7.3776e-02, 3.9327e-01, 2.6036e-01, -1.8300e-01, -3.3134e-01,3.6208e-02, 4.4270e-01, 1.0135e-02, 1.5132e-01, 3.5430e-01,-3.1990e-01 }, - { 3.1519e-01, -2.1691e-01, -5.9664e-01, 1.9881e-01, -2.2782e-01,-3.4393e-02, -2.2044e-01, 4.5168e-01, 3.9545e-01, -2.7917e-01,1.3510e-01, 5.4833e-02, -5.1843e-01, 7.2694e-02, -2.3412e-01,-1.6568e-01 }}; - - -static const float psi_s_b0[PSI_S_V] = {0.2276, 0.0345, 0.2541, -0.2226, 0.1145, 0.0618, 0.1488, 0.2155, -0.1619, 0.1102, 0.2277, 0.3863, 0.2162, 0.0595, 0.1464, 0.1720}; -static const float psi_s_b1[PSI_S_V] = { 0.1128, 0.0906, 0.0120, 0.0400, 0.1909, 0.1306, -0.0103, 0.0698, 0.1583, -0.1146, 0.0655, 0.0491, 0.0940, 0.2372, 0.0860, 0.0875}; - -static int getSelfEncoderOutputSize(); -static int getSelfEncoderOutput0Size(); -static int getEncoderInputSize(); - -static void feedForwardSelfEncoder(float* inp, float* dst); -static void feedForwardPsiS(float* inp, float* dst); -static void feedForwardPsiS0(float* inp, float* dst,int k); -static void feedForwardPsiS1(float* inp, float* dst,int k); - - -void calcSelfEncoderOutput(float* inp, float* dst) -{ - feedForwardSelfEncoder(inp, dst); -} - -static void feedForwardSelfEncoder(float* inp, float* dst) -{ - feedForwardPsiS(inp, dst); -} - - -static void feedForwardPsiS(float* inp, float* dst) -{//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY - - float out0[getSelfEncoderOutput0Size()]; - for(int k = 0; k < getSelfEncoderOutputSize(); k++) - { - feedForwardPsiS0(inp, out0, k); - } - - for(int k = 0; k < getSelfEncoderOutputSize(); k++) - { - feedForwardPsiS1(out0, dst, k); - } -} - - -static void feedForwardPsiS0(float* inp, float* out, int raw) -{//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY - int sz = getEncoderInputSize(); - float temp = 0.0; - - for(int k = 0; k < sz; k++) - { - temp += psi_s_w0[raw][k]*inp[k]; - } - - out[raw] = (temp + psi_s_b0[raw]) >= 0 ? temp + psi_s_b0[raw] : 0 ; -} - -static void feedForwardPsiS1(float* inp, float* out, int raw) -{//YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY - int sz = getSelfEncoderOutputSize(); - float temp = 0.0; - - for(int k = 0; k < sz; k++) - { - temp += psi_s_w1[raw][k]*inp[k]; - } - - out[raw] = (temp + psi_s_b1[raw]) >= 0 ? temp + psi_s_b1[raw] : 0; -} - - -static int getEncoderInputSize() -{ - return (int)PSI_S_H; -} - -static int getSelfEncoderOutputSize() -{ - return (int)PSI_S_V; -} - -static int getSelfEncoderOutput0Size() -{ - return (int)PSI_S_V; -} - diff --git a/examples/mean_embed/src/self_observation.c b/examples/mean_embed/src/self_observation.c deleted file mode 100644 index 1e70d9bc75..0000000000 --- a/examples/mean_embed/src/self_observation.c +++ /dev/null @@ -1,233 +0,0 @@ -#include "../include/self_observation.h" -#include "../include/vec.h" -#include "estimator_kalman.h" -#include "stabilizer_types.h" -#include "stabilizer.h" -#include "../include/vec.h" -#include "param.h" -/* min max values - EnvInfo(obs_space=Dict('obs': Box([-10. -10. -10. -3. -3. -3. -1. -1. -1. -1. -1. -1. -1. -1. - -1. -40. -40. -40. -10. -10. -10. -6. -6. -6. -10. -10. -10. -6. - -6. -6. -10. -10. -10. -6. -6. -6. -10. -10. -10. -6. -6. -6. - -10. -10. -10. -6. -6. -6. -10. -10. -10. -6. -6. -6.], [10. 10. 10. 3. 3. 3. 1. 1. 1. 1. 1. 1. 1. 1. 1. 40. 40. 40. - 10. 10. 10. 6. 6. 6. 10. 10. 10. 6. 6. 6. 10. 10. 10. 6. 6. 6. - 10. 10. 10. 6. 6. 6. 10. 10. 10. 6. 6. 6. 10. 10. 10. 6. 6. 6.] - -*/ -//static const int selfObservationSize = (int)SELFOBSERVATIONSIZE; - -// constants variables -struct selfObservationLimit -{ - float maxVelx; - float maxVely; - float maxVelz; - - float maxPosx; - float maxPosy; - float maxPosz; - - float maxR1; - float maxR2; - float maxR3; - float maxR4; - float maxR5; - float maxR6; - float maxR7; - float maxR8; - float maxR9; - - float maxW1; - float maxW2; - float maxW3; - - - float minVelx; - float minVely; - float minVelz; - - float minPosx; - float minPosy; - float minPosz; - - float minR1; - float minR2; - float minR3; - float minR4; - float minR5; - float minR6; - float minR7; - float minR8; - float minR9; - - float minW1; - float minW2; - float minW3; -}obsLimit -= -{ (float) MAXVELX, - (float) MAXVELY, - (float) MAXVELZ, - - (float) MAXPOSX, - (float) MAXPOSY, - (float) MAXPOSZ, - - 1.0f, - 1.0f, - 1.0f, - 1.0f, - 1.0f, - 1.0f, - 1.0f, - 1.0f, - 1.0f, - - (float)MAXW1, - (float)MAXW2, - (float)MAXW3, - - (float)MINVELX, - (float)MINVELY, - (float)MINVELZ, - - (float)MINPOSX, - (float)MINPOSY, - (float)MINPOSZ, - - -1.0f, - -1.0f, - -1.0f, - -1.0f, - -1.0f, - -1.0f, - -1.0f, - -1.0f, - -1.0f, - - (float)MINW1, - (float)MINW2, - (float)MINW3 - }; - -//static self_obs selfObservation; -static Vector3 targetPos = {.x = 0.f, .y = 0.f , .z= .4f}; -static void clipOrientation(float*); -static void clipAngularVel(Vector3*); - - -void updateSelfObservation(self_obs* slfObs) -{// YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYy - // get position - Vector3 ownPos = getPosition(); - clipPosition(&ownPos); - - Vector3 ownVel = getVelocity(); - clipVelocity(&ownVel); - - Vector3 ownAngVel = getAngularVelocity(); - clipAngularVel(&ownAngVel); - - float rotationMat[9]; - estimatorKalmanGetEstimatedRot(rotationMat); - clipOrientation(rotationMat); - - slfObs->px = targetPos.x - ownPos.x; - slfObs->py = targetPos.y - ownPos.y; - slfObs->pz = targetPos.z - ownPos.z; - - slfObs->vx = ownVel.x; - slfObs->vy = ownVel.y; - slfObs->vz = ownVel.z; - - slfObs->wx = ownAngVel.x; - slfObs->wy = ownAngVel.y; - slfObs->wz = ownAngVel.z; - - slfObs->r1 = rotationMat[0]; - slfObs->r2 = rotationMat[1]; - slfObs->r3 = rotationMat[2]; - slfObs->r4 = rotationMat[3]; - slfObs->r5 = rotationMat[4]; - slfObs->r6 = rotationMat[5]; - slfObs->r7 = rotationMat[6]; - slfObs->r8 = rotationMat[7]; - slfObs->r9 = rotationMat[8]; - -} - -void clipPosition(Vector3* pos) -{ - pos->x = clipVal(pos->x, obsLimit.minPosx, obsLimit.maxPosx); - pos->y = clipVal(pos->y, obsLimit.minPosy, obsLimit.maxPosy); - pos->x = clipVal(pos->x, obsLimit.minPosx, obsLimit.maxPosx); -} - -void clipVelocity(Vector3* vel) -{ - vel->x = clipVal(vel->x, obsLimit.minVelx, obsLimit.maxVelx); - vel->y = clipVal(vel->y, obsLimit.minVely, obsLimit.maxVely); - vel->x = clipVal(vel->x, obsLimit.minVelx, obsLimit.maxVelx); -} - -static void clipOrientation(float* orientArr) -{ - orientArr[0] = clipVal(orientArr[0], obsLimit.minR1, obsLimit.maxR1); - orientArr[1] = clipVal(orientArr[1], obsLimit.minR2, obsLimit.maxR2); - orientArr[2] = clipVal(orientArr[2], obsLimit.minR3, obsLimit.maxR3); - orientArr[3] = clipVal(orientArr[3], obsLimit.minR4, obsLimit.maxR4); - orientArr[4] = clipVal(orientArr[4], obsLimit.minR5, obsLimit.maxR5); - orientArr[5] = clipVal(orientArr[5], obsLimit.minR6, obsLimit.maxR6); - orientArr[6] = clipVal(orientArr[6], obsLimit.minR7, obsLimit.maxR7); - orientArr[7] = clipVal(orientArr[7], obsLimit.minR8, obsLimit.maxR8); - orientArr[8] = clipVal(orientArr[8], obsLimit.minR9, obsLimit.maxR9); -} - - -static void clipAngularVel(Vector3* angVel) -{ - angVel->x = clipVal(angVel->x, obsLimit.minW1, obsLimit.maxW1); - angVel->y = clipVal(angVel->y, obsLimit.minW2, obsLimit.maxW2); - angVel->z = clipVal(angVel->z, obsLimit.minW3, obsLimit.maxW3); -} - - -Vector3 getPosition(void) -{ - point_t pt; - estimatorKalmanGetEstimatedPos(&pt); - return (Vector3){.x = pt.x, .y=pt.y, .z=pt.z}; -} - -Vector3 getVeloc(void) -{ - return getVelocity(); -} - - - -PARAM_GROUP_START(selfobs) -PARAM_ADD(PARAM_FLOAT, xt, &targetPos.x) -PARAM_ADD(PARAM_FLOAT, yt, &targetPos.y) -PARAM_ADD(PARAM_FLOAT, zt, &targetPos.z) -PARAM_GROUP_STOP(selfobs) - - -/* -LOG_GROUP_START(selfObsLog) -LOG_ADD(LOG_FLOAT, thrst1, &thrusts[0]) -LOG_ADD(LOG_FLOAT, thrst2, &thrusts[2]) -LOG_ADD(LOG_FLOAT, thrst3, &thrusts[3]) -LOG_ADD(LOG_FLOAT, thrst4, &thrusts[4]) -LOG_GROUP_STOP(selfObsLog)*/ - - - - - - - - - - - diff --git a/examples/mean_embed/src/vec.c b/examples/mean_embed/src/vec.c deleted file mode 100644 index 001be2f51d..0000000000 --- a/examples/mean_embed/src/vec.c +++ /dev/null @@ -1,57 +0,0 @@ -#include "../include/vec.h" -#include - -Vector3 addVecf(Vector3 vec1, Vector3 vec2) -{ - Vector3 result = { - .x=vec1.x+vec2.x, - .y=vec1.y+vec2.y, - .z=vec1.z+vec2.z - }; - - return result; -} - -Vector3 negateVec(Vector3 vec) -{ - return (Vector3){.x=-vec.x, .y=-vec.y, .z=-vec.z}; -} - -Vector3 substractVec(Vector3 from, Vector3 to) -{ - return addVecf(from, negateVec(to)); -} - -void scaleVec(float* dst, float scl, int len) -{ - for(int k = 0; k < len; k++) - { - dst[k] = dst[k]*scl; - } -} - -float clipVal(float val, float min, float max) -{// checked - if(val < min) - { - return min; - } - else if (val > max) - { - return max; - } - else{ - return val; - } - -} - -float calcNormf(Vector3 vec) -{ - return sqrtf(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z); -} - -float calcDistf(Vector3 vec1, Vector3 vec2) -{ - return calcNormf(substractVec(vec1, vec2)); -} diff --git a/examples/mean_embed_2/include/mlp.h b/examples/mean_embed_2/include/mlp.h index c74c9e6ec6..f60a6e4dea 100644 --- a/examples/mean_embed_2/include/mlp.h +++ b/examples/mean_embed_2/include/mlp.h @@ -5,19 +5,21 @@ #define OUTPUT0 32 #define PHI_A_V 4 +/* typedef struct _INPUTOUTPUT { float out0[OUTPUT0]; float phi_a[PHI_A_V]; }inpOutVec; - +*/ +/* typedef struct MLPINPUT { int len; float inputVec[PHI_A_H]; -}mlpInput; +}mlpInput;*/ -void calcMlpOutput(mlpInput* inp , float* out); +void calcMlpOutput(float* inp , float* out); #endif \ No newline at end of file diff --git a/examples/mean_embed_2/include/self_observation.h b/examples/mean_embed_2/include/self_observation.h index 36f2ddd7d3..1b88f3d10a 100644 --- a/examples/mean_embed_2/include/self_observation.h +++ b/examples/mean_embed_2/include/self_observation.h @@ -78,7 +78,7 @@ typedef struct _SELF_OBS }self_obs; -void updateSelfObservation(self_obs*); +void updateSelfObservation(float*); Vector3 getPosition(void); Vector3 getVeloc(void); void clipPosition(Vector3*); diff --git a/examples/mean_embed_2/src/coll_avoid_main.c b/examples/mean_embed_2/src/coll_avoid_main.c index a9d4bd52e4..f044ebe1d9 100644 --- a/examples/mean_embed_2/src/coll_avoid_main.c +++ b/examples/mean_embed_2/src/coll_avoid_main.c @@ -55,7 +55,7 @@ static float thrusts[4]; static int16_t thrustsToMotor[4]; static uint64_t timer = 0; static PacketData ownPacket; -static uint8_t isHighLevelController = 1; +static uint8_t isHighLevelController = 0; static uint8_t isMeanEmbed = 0; void communicate(); @@ -87,7 +87,7 @@ uint8_t isHighLevel() return isHighLevelController; } -void appMain() { // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx +void appMain() { uint64_t address = configblockGetRadioAddress(); uint8_t my_id =(uint8_t)((address) & 0x00000000ff); @@ -127,13 +127,12 @@ void appMain() { // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX else{ if(!isMeanEmbed) { - consolePrintf("entered app main mean embed \n"); + //consolePrintf("entered app main mean embed \n"); isMeanEmbed = 1; } - feedForwardNN(thrusts); for(int k = 0; k < 4; k++) { - thrustsToMotor[k] = (int16_t)(INT16_MAX*thrusts[k]); + thrustsToMotor[k] = (int16_t)(INT16_MAX*(thrusts[k]*2.0f)); } } break; @@ -160,9 +159,15 @@ void appMain() { // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX commanderSetSetpoint(&setpoint, 3); } else{ - for(int k = 0; k < 4; k++) + + + if(getPosition().z < 0.15f) { - thrustsToMotor[k] = (int16_t)(INT16_MAX*0); + for(int k = 0; k < 4; k++) + { + thrustsToMotor[k] = (int16_t)(INT16_MAX*(0.0f)); + } + state = 0; } } @@ -170,6 +175,8 @@ void appMain() { // XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX default: break; } + feedForwardNN(thrusts); + } } @@ -189,5 +196,5 @@ LOG_GROUP_START(logNN) LOG_ADD(LOG_FLOAT, thrst1, &thrusts[0]) LOG_ADD(LOG_FLOAT, thrst2, &thrusts[2]) LOG_ADD(LOG_FLOAT, thrst3, &thrusts[3]) -LOG_ADD(LOG_FLOAT, thrst4, &thrusts[4]) +LOG_ADD(LOG_FLOAT, thrst4, &thrusts[1]) LOG_GROUP_STOP(logNN) \ No newline at end of file diff --git a/examples/mean_embed_2/src/mlp.c b/examples/mean_embed_2/src/mlp.c index af0eb4008b..5eebbfef26 100644 --- a/examples/mean_embed_2/src/mlp.c +++ b/examples/mean_embed_2/src/mlp.c @@ -130,19 +130,20 @@ static const float phi_a_b0[OUTPUT0] = { 0.0354, 0.0430, -0.0494, -0.0428, -0.1 static const float phi_a_b1[PHI_A_V] = {-0.0033, 0.0984, -0.1976, 0.1254}; -static inpOutVec* networkVecs; +static float out0[OUTPUT0]; +static float phi_a[PHI_A_V]; static void feedForwardMLP(float*); static void feedForwardPhiA0(float*, int); static void feedForwardPhiA1(float*, int); -void calcMlpOutput(mlpInput* input, float* out) +void calcMlpOutput(float* input, float* out) { - feedForwardMLP(input->inputVec); + feedForwardMLP(input); for(int k = 0; k < 4; k++) { - out[k] = networkVecs->phi_a[k]; + out[k] = phi_a[k]; } } @@ -156,7 +157,7 @@ static void feedForwardMLP(float* inp) for(int k = 0; k < 4; k++) { - feedForwardPhiA1(networkVecs->out0, k); + feedForwardPhiA1(out0, k); } } @@ -170,7 +171,7 @@ static void feedForwardPhiA0(float* inp, int raw) tmp += inp[k]*phi_a_w0[raw][k]; } - networkVecs->out0[raw] = (tmp + phi_a_b0[raw] >= 0) ? tmp + phi_a_b0[raw] : 0; + out0[raw] = (tmp + phi_a_b0[raw] >= 0) ? tmp + phi_a_b0[raw] : 0; } @@ -181,7 +182,7 @@ static void feedForwardPhiA1(float* inp, int raw) { tmp += inp[k]*phi_a_w1[raw][k]; } - networkVecs->phi_a[raw] = tmp + phi_a_b1[raw]; + phi_a[raw] = tmp + phi_a_b1[raw]; } diff --git a/examples/mean_embed_2/src/nn.c b/examples/mean_embed_2/src/nn.c index e7f9740af3..354c68f4e9 100644 --- a/examples/mean_embed_2/src/nn.c +++ b/examples/mean_embed_2/src/nn.c @@ -11,91 +11,39 @@ #include +static float selfObservation[18]; +static float thrusts_out[4]; +static float outputSelf[16]; +static float outputNeighbor[8]; - static nnInputOutput* nnIO; - static self_obs* selfObservation; - static float selfObservationArr[3]; - static neighb_obs kNearestObservations[6]; - static mlpInput* mlpInp; - static uint8_t isInitialized = 0; - -static void nnInit(); - -static void nnInit() -{ - // for(int k = 0; k < 18; k++) - // { - // selfObservation->self_obs_arr[k] = 0.0; - // } - - // selfObservation->obsLen = 18; - mlpInp->len = 24; - - for(int k = 0; k < 6; k++) - { - kNearestObservations[k].obsLen = 6; - } -} +static float mlpInput[24]; +static neighb_obs kNearestObservations[6]; void feedForwardNN(float* thrusts) -{//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - if(!isInitialized) - { - nnInit(); - isInitialized = 1; - } - +{ updateSelfObservation(selfObservation); - - float inparr[18]; - - selfObservationArr[0] = selfObservation->px; - selfObservationArr[1] = selfObservation->py; - selfObservationArr[2] = selfObservation->pz; - - inparr[0] = selfObservation->px; - inparr[1] = selfObservation->py; - inparr[2] = selfObservation->pz; - inparr[15] = selfObservation->wx; - inparr[16] = selfObservation->wy; - inparr[17] = selfObservation->wz; - - inparr[3] = selfObservation->vx; - inparr[4] = selfObservation->vy; - inparr[5] = selfObservation->vz; - - inparr[6] = selfObservation->r1; - inparr[7] = selfObservation->r2; - inparr[8] = selfObservation->r3; - inparr[9] = selfObservation->r4; - inparr[10] = selfObservation->r5; - inparr[11] = selfObservation->r6; - inparr[12] = selfObservation->r7; - inparr[13] = selfObservation->r8; - inparr[14] = selfObservation->r9; - - calcSelfEncoderOutput(inparr, nnIO->outputSelf); + calcSelfEncoderOutput(selfObservation, outputSelf); updateNeighbObservation(kNearestObservations); - calcNeighborEncoderOutput(kNearestObservations, nnIO->outputNeighbor); + calcNeighborEncoderOutput(kNearestObservations, outputNeighbor); for(int k = 0; k < 16; k++) { - mlpInp->inputVec[k] = nnIO->outputSelf[k]; + mlpInput[k] = outputSelf[k]; } for(int k = 0; k < 8; k++) { - mlpInp->inputVec[k+16] = nnIO->outputNeighbor[k]; + mlpInput[k+16] = outputNeighbor[k]; } - calcMlpOutput(mlpInp, nnIO->output); + calcMlpOutput(mlpInput, thrusts); for(int k = 0; k < 4; k++) { - nnIO->output[k] = 0.5f*(clipVal(nnIO->output[k], -1.0f, 1.0f)+1.0f); - thrusts[k] = nnIO->output[k]; + thrusts[k] = 0.5f*(clipVal(thrusts[k], -1.0f, 1.0f)+1.0f); + thrusts_out[k] = thrusts[k]; } @@ -103,7 +51,69 @@ void feedForwardNN(float* thrusts) } LOG_GROUP_START(selfObsLog) -LOG_ADD(LOG_FLOAT, observedPx, &(selfObservationArr[0])) -LOG_ADD(LOG_FLOAT, observedPy, &(selfObservationArr[1])) -LOG_ADD(LOG_FLOAT, observedPz, &(selfObservationArr[2])) +LOG_ADD(LOG_FLOAT, observedPx, &(selfObservation[0])) +LOG_ADD(LOG_FLOAT, observedPy, &(selfObservation[1])) +LOG_ADD(LOG_FLOAT, observedPz, &(selfObservation[2])) +LOG_ADD(LOG_FLOAT, observedVx, &(selfObservation[3])) +LOG_ADD(LOG_FLOAT, observedVy, &(selfObservation[4])) +LOG_ADD(LOG_FLOAT, observedVz, &(selfObservation[5])) +LOG_ADD(LOG_FLOAT, observedR1, &(selfObservation[6])) +LOG_ADD(LOG_FLOAT, observedR2, &(selfObservation[7])) +LOG_ADD(LOG_FLOAT, observedR3, &(selfObservation[8])) +LOG_ADD(LOG_FLOAT, observedR4, &(selfObservation[9])) +LOG_ADD(LOG_FLOAT, observedR5, &(selfObservation[10])) +LOG_ADD(LOG_FLOAT, observedR6, &(selfObservation[11])) +LOG_ADD(LOG_FLOAT, observedR7, &(selfObservation[12])) +LOG_ADD(LOG_FLOAT, observedR8, &(selfObservation[13])) +LOG_ADD(LOG_FLOAT, observedR9, &(selfObservation[14])) +LOG_ADD(LOG_FLOAT, observedWx, &(selfObservation[15])) +LOG_ADD(LOG_FLOAT, observedWy, &(selfObservation[16])) +LOG_ADD(LOG_FLOAT, observedWz, &(selfObservation[17])) + +LOG_ADD(LOG_FLOAT, t0, &(thrusts_out[0])) +LOG_ADD(LOG_FLOAT, t1, &(thrusts_out[1])) +LOG_ADD(LOG_FLOAT, t2, &(thrusts_out[2])) +LOG_ADD(LOG_FLOAT, t3, &(thrusts_out[3])) + +LOG_ADD(LOG_FLOAT, os1, &(outputSelf[0])) +LOG_ADD(LOG_FLOAT, os2, &(outputSelf[1])) +LOG_ADD(LOG_FLOAT, os3, &(outputSelf[2])) +LOG_ADD(LOG_FLOAT, os4, &(outputSelf[3])) +LOG_ADD(LOG_FLOAT, os5, &(outputSelf[4])) +LOG_ADD(LOG_FLOAT, os6, &(outputSelf[5])) +LOG_ADD(LOG_FLOAT, os7, &(outputSelf[6])) +LOG_ADD(LOG_FLOAT, os8, &(outputSelf[7])) +LOG_ADD(LOG_FLOAT, os9, &(outputSelf[8])) +LOG_ADD(LOG_FLOAT, os10, &(outputSelf[9])) +LOG_ADD(LOG_FLOAT, os11, &(outputSelf[10])) +LOG_ADD(LOG_FLOAT, os12, &(outputSelf[11])) +LOG_ADD(LOG_FLOAT, os13, &(outputSelf[12])) +LOG_ADD(LOG_FLOAT, os14, &(outputSelf[13])) +LOG_ADD(LOG_FLOAT, os15, &(outputSelf[14])) +LOG_ADD(LOG_FLOAT, os16, &(outputSelf[15])) LOG_GROUP_STOP(selfObsLog) + + +LOG_GROUP_START(nebOb) +LOG_ADD(LOG_FLOAT, on1, &(outputNeighbor[0])) +LOG_ADD(LOG_FLOAT, on2, &(outputNeighbor[1])) +LOG_ADD(LOG_FLOAT, on3, &(outputNeighbor[2])) +LOG_ADD(LOG_FLOAT, on4, &(outputNeighbor[3])) +LOG_ADD(LOG_FLOAT, on5, &(outputNeighbor[4])) +LOG_ADD(LOG_FLOAT, on6, &(outputNeighbor[5])) +LOG_ADD(LOG_FLOAT, on7, &(outputNeighbor[6])) +LOG_ADD(LOG_FLOAT, on8, &(outputNeighbor[7])) +LOG_GROUP_STOP(nebOb) + + +LOG_GROUP_START(mlpNet) +LOG_ADD(LOG_FLOAT, mlp1, &(mlpInput[0])) +LOG_ADD(LOG_FLOAT, mlp2, &(mlpInput[1])) +LOG_ADD(LOG_FLOAT, mlp3, &(mlpInput[2])) +LOG_ADD(LOG_FLOAT, mlp4, &(mlpInput[3])) +LOG_ADD(LOG_FLOAT, mlp5, &(mlpInput[4])) +LOG_ADD(LOG_FLOAT, mlp6, &(mlpInput[5])) +LOG_ADD(LOG_FLOAT, mlp7, &(mlpInput[6])) +LOG_ADD(LOG_FLOAT, mlp8, &(mlpInput[7])) +LOG_GROUP_STOP(mlpNet) + diff --git a/examples/mean_embed_2/src/self_observation.c b/examples/mean_embed_2/src/self_observation.c index 1e70d9bc75..91f61118c5 100644 --- a/examples/mean_embed_2/src/self_observation.c +++ b/examples/mean_embed_2/src/self_observation.c @@ -116,7 +116,7 @@ static void clipOrientation(float*); static void clipAngularVel(Vector3*); -void updateSelfObservation(self_obs* slfObs) +void updateSelfObservation(float* slfObs) {// YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYy // get position Vector3 ownPos = getPosition(); @@ -132,27 +132,27 @@ void updateSelfObservation(self_obs* slfObs) estimatorKalmanGetEstimatedRot(rotationMat); clipOrientation(rotationMat); - slfObs->px = targetPos.x - ownPos.x; - slfObs->py = targetPos.y - ownPos.y; - slfObs->pz = targetPos.z - ownPos.z; + slfObs[0] = targetPos.x - ownPos.x; + slfObs[1] = targetPos.y - ownPos.y; + slfObs[2] = targetPos.z - ownPos.z; - slfObs->vx = ownVel.x; - slfObs->vy = ownVel.y; - slfObs->vz = ownVel.z; + slfObs[3] = ownVel.x; + slfObs[4] = ownVel.y; + slfObs[5] = ownVel.z; - slfObs->wx = ownAngVel.x; - slfObs->wy = ownAngVel.y; - slfObs->wz = ownAngVel.z; + slfObs[15] = ownAngVel.x; + slfObs[16] = ownAngVel.y; + slfObs[17] = ownAngVel.z; - slfObs->r1 = rotationMat[0]; - slfObs->r2 = rotationMat[1]; - slfObs->r3 = rotationMat[2]; - slfObs->r4 = rotationMat[3]; - slfObs->r5 = rotationMat[4]; - slfObs->r6 = rotationMat[5]; - slfObs->r7 = rotationMat[6]; - slfObs->r8 = rotationMat[7]; - slfObs->r9 = rotationMat[8]; + slfObs[6] = rotationMat[0]; + slfObs[7] = rotationMat[1]; + slfObs[8] = rotationMat[2]; + slfObs[9] = rotationMat[3]; + slfObs[10] = rotationMat[4]; + slfObs[11] = rotationMat[5]; + slfObs[12] = rotationMat[6]; + slfObs[13] = rotationMat[7]; + slfObs[14] = rotationMat[8]; } @@ -160,14 +160,14 @@ void clipPosition(Vector3* pos) { pos->x = clipVal(pos->x, obsLimit.minPosx, obsLimit.maxPosx); pos->y = clipVal(pos->y, obsLimit.minPosy, obsLimit.maxPosy); - pos->x = clipVal(pos->x, obsLimit.minPosx, obsLimit.maxPosx); + pos->z = clipVal(pos->x, obsLimit.minPosz, obsLimit.maxPosz); } void clipVelocity(Vector3* vel) { vel->x = clipVal(vel->x, obsLimit.minVelx, obsLimit.maxVelx); vel->y = clipVal(vel->y, obsLimit.minVely, obsLimit.maxVely); - vel->x = clipVal(vel->x, obsLimit.minVelx, obsLimit.maxVelx); + vel->z = clipVal(vel->z, obsLimit.minVelz, obsLimit.maxVelz); } static void clipOrientation(float* orientArr) diff --git a/src/modules/interface/stabilizer.h b/src/modules/interface/stabilizer.h index e590a50b40..0d07eab93e 100644 --- a/src/modules/interface/stabilizer.h +++ b/src/modules/interface/stabilizer.h @@ -30,7 +30,7 @@ #include #include "estimator.h" -#include "/home/saz/GitHub/masterthesis_crazyflie/examples/mean_embed/include/vec.h" +#include "/home/saz/GitHub/masterthesis_crazyflie/examples/mean_embed_2/include/vec.h" /** * Initialize the stabilizer subsystem and launch the stabilizer loop task.