Bitcraze Forums

Code: Select all

#define DEBUG_MODULE "mlModelDeck"
#include "deck.h"
#include "system.h"
#include "commander.h"
#include "range.h"  // get the 6axis distance measurements
#include "log.h"

#include "FreeRTOS.h"
#include "task.h"

#include "debug.h"
#include "sysload.h"
#include "sensors.h"

#define TRUE 1
#define FALSE 0

// These are my variable which will be used in this deck
static float inModelIMU[12] = {0};
static float inModelIMUOld[6] = {0};
static float outModelIMU[6] = {0};
static float errorModelIMU[6] = {0};


static void mlModelDeckTask()
{
	// static setpoint_t setpoint;
	systemWaitStart();
    DEBUG_PRINT("FINISHED\n");

    static sensorData_t sensorData;
    uint32_t tick;
    vTaskDelay(M2T(200));

    while(1){
        // tick = xTaskGetTickCount();

        // This is where I will have following commented out code

        // // Error model input preparation and evaluate
        // // Saving old inputs
        // if (((tick+5) % 20) < 5){
        //   for (int ii=0;ii<6;ii++){
        //     inModelIMUOld[ii] = inModelIMU[ii];
        //   }
        //   inModelIMU[0] = sensorData.gyro.x/150;
        //   inModelIMU[1] = sensorData.gyro.y/150;
        //   inModelIMU[2] = sensorData.gyro.z/150;
        //   inModelIMU[3] = sensorData.acc.x/150;
        //   inModelIMU[4] = sensorData.acc.y/150;
        //   inModelIMU[5] = sensorData.acc.z/150;
        
        //   for (int ii=0;ii<6;ii++){
        //     // Preparing whole input for model
        //     inModelIMU[6+ii] = inModelIMUOld[ii];
        //     outModelIMU[ii] = 0;
        //   }
        //   // DEBUG_PRINT("tick: %d\n",tick);
        //   // DEBUG_PRINT("in_value: %f, %f, %f, %f, %f, %f\n", sensorData.gyro.x, sensorData.gyro.y, sensorData.gyro.z, sensorData.acc.x, sensorData.acc.y, sensorData.acc.z);
        //   // DEBUG_PRINT("in_value: %f, %f, %f, %f, %f, %f\n", inModelIMU[0], inModelIMU[1], inModelIMU[2], inModelIMU[3], inModelIMU[4], inModelIMU[5]);
        //   // get_inference_IMU(&inModelIMU,12/2,&outModelIMU);
        //   // vTaskDelay(M2T(100));
        //   DEBUG_PRINT("ou_value: %f, %f, %f, %f, %f, %f\n", outModelIMU[0], outModelIMU[1], outModelIMU[2], outModelIMU[3], outModelIMU[4], outModelIMU[5]);

        //   errorModelIMU[0] = outModelIMU[0] - sensorData.gyro.x/150;
        //   errorModelIMU[1] = outModelIMU[1] - sensorData.gyro.y/150;
        //   errorModelIMU[2] = outModelIMU[2] - sensorData.gyro.z/150;
        //   errorModelIMU[3] = outModelIMU[3] - sensorData.acc.x/150;
        //   errorModelIMU[4] = outModelIMU[4] - sensorData.acc.y/150;
        //   errorModelIMU[5] = outModelIMU[5] - sensorData.acc.z/150;
        // }
        vTaskDelay(M2T(100));
    }
}

static void init() {
	xTaskCreate(mlModelDeckTask, "mlModelDeckTask",
		1000 /* Stack size in terms of WORDS (usually 4 bytes) */,
		NULL, /*priority*/3, NULL);
}

static bool test() {
	return true;
}

const DeckDriver mlModelDeck = {
	.vid = 0,
	.pid = 0,
	.name = "mlModelDeck",

	.usedGpio = 0,  // FIXME: set the used pins

	.init = init,
	.test = test,
};

DECK_DRIVER(mlModelDeck);

LOG_GROUP_START(stabilizer)
LOG_ADD(LOG_FLOAT, errorModelIMU0, &errorModelIMU[0])
LOG_ADD(LOG_FLOAT, errorModelIMU1, &errorModelIMU[1])
LOG_ADD(LOG_FLOAT, errorModelIMU2, &errorModelIMU[2])
LOG_ADD(LOG_FLOAT, errorModelIMU3, &errorModelIMU[3])
LOG_ADD(LOG_FLOAT, errorModelIMU4, &errorModelIMU[4])
LOG_ADD(LOG_FLOAT, errorModelIMU5, &errorModelIMU[5])
LOG_GROUP_STOP(stabilizer)

Code: Select all

PROJ_OBJ += mlModelDeck.o

Code: Select all

CFLAGS += -DDECK_FORCE=mlModelDeck