seg fault resolved, airspeed quick fix, controller data-logging, and …

…controller output hooks added.

glue-codes/kitefast/kitefast_build.sh

@@ -100,7 +100,7 @@ ln -s $openfast_directory/build/modules-local/kitefast-controller/libkitefastcon
 # configure and build mbdyn
 export LDFLAGS=-rdynamic
 cd $mbdyn_directory
-./configure --enable-runtime-loading --with-module="kitefastmbd"
+./configure --enable-runtime-loading --with-module="kitefastmbd" --enable-debug
 sudo make                      # build mbdyn
 cd modules                     # move to the module directory
 sudo make                      # build the user defined element

modules-local/kitefast-controller/CMakeLists.txt

@@ -49,6 +49,7 @@ set(DYLIB_SOURCES
 	src/control/system_types.c
 	src/control/tether_util.c
 	src/control/system_params.c
+	src/control/control_log.c
 	src/control/crosswind/crosswind.c
 	src/control/crosswind/crosswind_inner.c
 	src/control/crosswind/crosswind_types.c

modules-local/kitefast-controller/src/KiteFastController.c

@@ -24,8 +24,6 @@ void kfc_dll_step(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
                   int *errStat, char *errMsg)
 {
     printf("  debug marker - pre controller_step() \n");
-    printf("  value dcm_g2b_c = %d \n",&dcm_g2b_c[0]);
-	printf("  value pqr_c = %d \n",&pqr_c);
     controller_step(dcm_g2b_c, pqr_c, acc_norm_c,
                  Xg_c, Vg_c, Vb_c, Ag_c,
                  Ab_c, rho_c, apparent_wind_c,

modules-local/kitefast-controller/src/KiteFastController_Driver.f90

@@ -51,7 +51,7 @@ program KiteFastController_Driver
    u%Ag = (/ 1, 2, 3 /) !reshape((/ 1, 2, 3 /), shape(u%Ag))
    u%Ab = (/ 1, 2, 3 /) !reshape((/ 1, 2, 3 /), shape(u%Ab))
    u%rho = 1.0        
-   u%apparent_wind = (/ 1, 1, 1 /) !reshape((/ 1, 2, 3 /), shape(u%apparent_wind))
+   u%apparent_wind = (/ -30, 1, 1 /) !reshape((/ 1, 2, 3 /), shape(u%apparent_wind))
    u%tether_forceb = (/ 1, 1, 1 /) !reshape((/ 1, 2, 3 /), shape(u%tether_forceb))
    u%wind_g = 1.0 !reshape((/ 1, 2, 3 /), shape(u%wind_g))
 !    y%GenSPyRtr = reshape((/ 1, 2, 3, 4 /), shape(y%GenSPyRtr))

modules-local/kitefast-controller/src/control/control_log.c

@@ -0,0 +1,141 @@
+#include "control/control_log.h"
+#include <time.h>
+
+void ControlLogInit(char* controllerVerNumber){
+	//Init Data SavestateEstLog
+    char timeStr[100];
+    time_t now = time(NULL);
+    struct tm *t = localtime(&now);
+    strftime(timeStr, sizeof(timeStr)-1, "%d/%m/%Y %H:%M \n", t);
+
+	FILE * fp;
+	fp = fopen("controller_save_data.txt", "w+");
+	fprintf(fp, "Controller Output Text File \nFile Created on: %s",timeStr);
+	fprintf(fp, "controller version: %s \n", controllerVerNumber);
+    fprintf(fp, "Header:  ");
+    fprintf(fp, "dcm[0,0], dcm[0,1], dcm[0,2], dcm[1,0], dcm[1,1], dcm[1,2], dcm[2,0], dcm[2,1], dcm[2,2],");
+    fprintf(fp, "pqr_f.x, pqr_f.y, pqr_f.z, Xg.x, Xg.y, Xg.z, Vg.x, Vg.y, Vg.z, Vb.x, Vb.y, Vb.z, Ag.x, Ag.y, Ag.z,");
+    fprintf(fp, "Ab_f.x, Ab_f.y, Ab_f.z, rho, sph_f.v, sph_f.alpha, sph_f.beta, tether_force_b.vector_f.x, tether_force_b.vector_f.y, tether_force_b.vector_f.z,");
+    fprintf(fp, "wind_g.vector.x, wind_g.vector.y, wind_g.vector.z, flaps[0], flaps[1], flaps[2], flaps[3], flaps[4], flaps[5], flaps[6], flaps[7],");
+    fprintf(fp, "rotors[0], rotors[1], rotors[2], rotors[3], rotors[4], rotors[5], rotors[6], rotors[7]");  
+    fprintf(fp, "\n");
+	fclose(fp);
+    printf("   Control Logging Initiated: File saved as - controller_save_data.txt \n");
+}
+
+void ControlLogEntry(ControlLog* control_log){
+    //assemble data for save file
+    printf("   Control Logging : Saving Step data\n");
+    char assembledStr[1024];
+    char tempStr[80];
+    // dcm 
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.dcm_g2b.d[0][0],
+        control_log->stateEstLog.dcm_g2b.d[1][0],
+        control_log->stateEstLog.dcm_g2b.d[2][0],
+        control_log->stateEstLog.dcm_g2b.d[0][1],
+        control_log->stateEstLog.dcm_g2b.d[1][1],
+        control_log->stateEstLog.dcm_g2b.d[2][1],
+        control_log->stateEstLog.dcm_g2b.d[0][2],
+        control_log->stateEstLog.dcm_g2b.d[1][2],
+        control_log->stateEstLog.dcm_g2b.d[1][2]);
+    strcpy( assembledStr, tempStr);
+    // pqr
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.pqr_f.x,
+        control_log->stateEstLog.pqr_f.y,
+        control_log->stateEstLog.pqr_f.z);
+    strcat( assembledStr, tempStr);
+    // acc_norm_c
+    sprintf(tempStr, "%0.4f\t", control_log->stateEstLog.acc_norm_f);
+    strcat( assembledStr, tempStr);
+    // Xg_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.Xg.x,        
+        control_log->stateEstLog.Xg.y,
+        control_log->stateEstLog.Xg.z);
+    strcat( assembledStr, tempStr);
+    // Vg_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.Vg.x,        
+        control_log->stateEstLog.Vg.y,
+        control_log->stateEstLog.Vg.z);
+    strcat( assembledStr, tempStr);
+    // Vb_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.Vb.x,        
+        control_log->stateEstLog.Vb.y,
+        control_log->stateEstLog.Vb.z);
+    strcat( assembledStr, tempStr);
+    // Ag_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.Ag.x,        
+        control_log->stateEstLog.Ag.y,
+        control_log->stateEstLog.Ag.z);
+    strcat( assembledStr, tempStr);
+    // Ab_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.Ab_f.x,        
+        control_log->stateEstLog.Ab_f.y,
+        control_log->stateEstLog.Ab_f.z);
+    strcat( assembledStr, tempStr);
+	// rho
+    sprintf(tempStr, "%0.4f\t", control_log->stateEstLog.rho);
+    strcat( assembledStr, tempStr); 
+	//apparent_wind_c_v
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.apparent_wind.sph_f.v,        
+        control_log->stateEstLog.apparent_wind.sph_f.alpha,
+        control_log->stateEstLog.apparent_wind.sph_f.beta);
+    strcat( assembledStr, tempStr);
+	//tether_force_c
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.tether_force_b.vector_f.x,        
+        control_log->stateEstLog.tether_force_b.vector_f.y,
+        control_log->stateEstLog.tether_force_b.vector_f.z);
+    strcat( assembledStr, tempStr);
+	//wind_g
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t",
+        control_log->stateEstLog.wind_g.vector.x,        
+        control_log->stateEstLog.wind_g.vector.y,
+        control_log->stateEstLog.wind_g.vector.z);
+    strcat( assembledStr, tempStr);   
+    // kFlaps[]
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t",
+        control_log->controlOutputLog.flaps[0],    
+        control_log->controlOutputLog.flaps[1],
+        control_log->controlOutputLog.flaps[2],
+        control_log->controlOutputLog.flaps[3],
+        control_log->controlOutputLog.flaps[4],
+        control_log->controlOutputLog.flaps[5],
+        control_log->controlOutputLog.flaps[6],
+        control_log->controlOutputLog.flaps[7]);
+    strcat( assembledStr, tempStr);
+    // rotors 
+    sprintf(tempStr, "%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t%0.4f\t",
+        control_log->controlOutputLog.rotors[0],    
+        control_log->controlOutputLog.rotors[1],
+        control_log->controlOutputLog.rotors[2],
+        control_log->controlOutputLog.rotors[3],
+        control_log->controlOutputLog.rotors[4],
+        control_log->controlOutputLog.rotors[5],
+        control_log->controlOutputLog.rotors[6],
+        control_log->controlOutputLog.rotors[7]);
+    strcat( assembledStr, tempStr); 
+
+    FILE * fp;
+	fp = fopen("controller_save_data.txt", "a+");
+
+    // double tempVal = ;
+    // sprintf(str, "%0.4f\t", control_log->stateEstLog.Ag.x);
+    // sprintf(str + strlen(str), "%0.4f\t", control_log->stateEstLog.Ag.y);
+    // sprintf(str + strlen(str), "%d\t", control_log->stateEstLog.Ag.z);
+    // char tempLogStr[];
+
+
+	fprintf(fp, "%s \n", assembledStr);
+	// fprintf(fp,"controller version: %s \n",controllerVerNumber);
+	fclose(fp);
+
+    printf("   Control Logging Initiated: File saved as - controller_save_data.txt \n");
+}

modules-local/kitefast-controller/src/control/control_log.h

@@ -0,0 +1,17 @@
+#ifndef _CONTROL_LOG_H_
+#define _CONTROL_LOG_H_
+
+#include <stdio.h>
+#include "control/control_types.h"
+#include "control/estimator/estimator_types.h"
+
+typedef struct {
+
+    ControlOutput controlOutputLog;
+    StateEstimate stateEstLog;
+
+} ControlLog;
+
+void ControlLogInit(char* controllerVerNumber);
+void ControlLogEntry(ControlLog* control_log);
+#endif

modules-local/kitefast-controller/src/control/controller_conversion.c

@@ -5,19 +5,28 @@
 #include "control/control_types.h"
 #include "control/estimator/estimator_types.h"
 
+
+
 void AssignInputs(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 	double Xg_c[], double Vg_c[], double Vb_c[], double Ag_c[],
 	double Ab_c[], double *rho_c, double apparent_wind_c[],
 	double tether_force_c[], double wind_g_c[],
 	double kFlapA_c[], double Motor_c[],
 	int *errStat, char *errMsg, StateEstimate* state_est){
 
+	bool printInputs = true;
+	if (printInputs){
+
+	}
+
 	//dcm_2gb - convert and copy value into state_est->dcm_g2b
 	Mat3 dcm_g2b_tmp = { { { dcm_g2b_c[0], dcm_g2b_c[1], dcm_g2b_c[2] },
 						   { dcm_g2b_c[3], dcm_g2b_c[4], dcm_g2b_c[5] },
 						   { dcm_g2b_c[6], dcm_g2b_c[7], dcm_g2b_c[8] } } };;
 	memcpy(&state_est->dcm_g2b, &dcm_g2b_tmp, sizeof(state_est->dcm_g2b));
-
+	printf("  value dcm_g2b_c = %0.4f \n",dcm_g2b_c[0]);
+	printf("  value pqr_c = %0.4f \n",&pqr_c[0]);
+
 	//pqr_c - convert and copy value into state_est->pqr_c
 	Vec3 pqr_c_tmp = { pqr_c[0], pqr_c[1], pqr_c[2] };
 	memcpy(&state_est->pqr_f, &pqr_c_tmp, sizeof(state_est->pqr_f));
@@ -51,6 +60,9 @@ void AssignInputs(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 	memcpy(&state_est->rho, &rho_tmp, sizeof(state_est->rho));
 
 	//apparent_wind_c_v
+	if(apparent_wind_c[0] <= 0.0){  //added by Justin Miller - STI 
+      apparent_wind_c[0] = -apparent_wind_c[0];
+    } // TODO - Check reference frames of kitefast vs CSIM - Airspeed is coming in (-), assertions fail if airspeed is (-)
 	state_est->apparent_wind.sph_f.v = apparent_wind_c[0];
 	state_est->apparent_wind.sph_f.alpha = apparent_wind_c[1];
 	state_est->apparent_wind.sph_f.beta = apparent_wind_c[2];
@@ -67,3 +79,26 @@ void AssignInputs(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 
 }
 
+void AssignOutputs(double kFlapA_c[], double Motor_c[],
+	int *errStat, char *errMsg, ControlOutput* raw_control_output){
+
+		//kFlap_A
+		kFlapA_c[0] = raw_control_output->flaps[0];
+		kFlapA_c[1] = raw_control_output->flaps[1];
+		kFlapA_c[2] = raw_control_output->flaps[2];
+		kFlapA_c[3] = raw_control_output->flaps[3];
+		kFlapA_c[4] = raw_control_output->flaps[4];
+		kFlapA_c[5] = raw_control_output->flaps[5];
+		kFlapA_c[6] = raw_control_output->flaps[6];
+		kFlapA_c[7] = raw_control_output->flaps[7];
+
+		//Motor_c
+		Motor_c[0] = raw_control_output->rotors[0];
+		Motor_c[1] = raw_control_output->rotors[1];
+		Motor_c[2] = raw_control_output->rotors[2];
+		Motor_c[3] = raw_control_output->rotors[3];
+		Motor_c[4] = raw_control_output->rotors[4];
+		Motor_c[5] = raw_control_output->rotors[5];
+		Motor_c[6] = raw_control_output->rotors[6];
+		Motor_c[7] = raw_control_output->rotors[7];
+	}

modules-local/kitefast-controller/src/control/controller_conversion.h

@@ -14,5 +14,7 @@ void AssignInputs(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 	double kFlapA_c[], double Motor_c[],
 	int *errStat, char *errMsg, StateEstimate *state_est);
 
+void AssignOutputs(double kFlapA_c[], double Motor_c[],
+	int *errStat, char *errMsg, ControlOutput* raw_control_output);
 
 #endif

modules-local/kitefast-controller/src/control/crosswind/crosswind.c

@@ -315,6 +315,26 @@ printf("    debug marker - pre crosswind_inner \n");
       &state->inner, lateral_gains, &deltas, &thrust_moment);
 
 printf("    debug marker - post crosswind_inner \n");
+
+// PLACE HOLDER FOR CONTROL OUTPUTS
+control_output->flaps[0] = 0;
+control_output->flaps[1] = 0;
+control_output->flaps[2] = 0;
+control_output->flaps[3] = 0;
+control_output->flaps[4] = 0;
+control_output->flaps[5] = 0;
+control_output->flaps[6] = 0;
+control_output->flaps[7] = 0;
+
+control_output->rotors[0] = 0;
+control_output->rotors[1] = 0;
+control_output->rotors[2] = 0;
+control_output->rotors[3] = 0;
+control_output->rotors[4] = 0;
+control_output->rotors[5] = 0;
+control_output->rotors[6] = 0;
+control_output->rotors[7] = 0;
+// end placeholder - Justin Miller - STI
   //// Convert control variables to actuator commands.
   //CrosswindOutputStep(params->loop_dir, loop_angle, flaring, &thrust_moment,
   //                    &deltas, state_est, &path_center_g, &params->output,

modules-local/kitefast-controller/src/control/crosswind/crosswind_inner.c

@@ -23,6 +23,7 @@
 static void ScheduleLongitudinalGains(
     double airspeed, const CrosswindInnerParams *params,
     double longitudinal_gains[][kNumCrosswindLongitudinalStates]) {
+  printf("airpseed = %0.4f\n",airspeed );
   assert(airspeed >= 0.0);
   assert(params != NULL && longitudinal_gains != NULL);
 
@@ -192,6 +193,7 @@ static void CalcLateralFeedback(
     const Vec3 *pqr_cmd, const Vec3 *pqr, double int_tether_roll_error,
     double int_beta_error, double lateral_gains[][kNumCrosswindLateralStates],
     double *delta_aileron, double *delta_rudder, double *moment_z) {
+    printf("beta = %0.4f, beta_cmd = %0.4f\n",beta, beta_cmd);
   assert(-PI / 2.0 <= tether_roll_cmd && tether_roll_cmd <= PI / 2.0);
   assert(-PI / 2.0 <= tether_roll && tether_roll <= PI / 2.0);
   assert(-PI / 2.0 <= beta_cmd && beta_cmd <= PI / 2.0);

modules-local/kitefast-controller/src/control/kfc.c

@@ -1,9 +1,11 @@
 #include "control/kfc.h"
+
 #include "control/crosswind/crosswind.h"
 #include "control/controller_conversion.h"
 #include "control/controller_util.h"
 #include "control/crosswind/crosswind_types.h"
 #include "control/control_types.h"
+#include "control/control_log.h"
 
 void controller_init(int *errStat, char *errMsg)
 {
@@ -18,6 +20,10 @@ void controller_init(int *errStat, char *errMsg)
 
 	//initalize parameters here!! (outputs)
 	//CrosswindInit(state_est, flaps_z1, 0.0, 0, &params->crosswind, &state->crosswind);
+
+	// Init Control Logging
+	ControlLogInit((char*)controllerVerNumber);
+
 	char tmp[] = "   controller initializing";
 	int i;
 	for (i = 0; i < sizeof(tmp); i++)
@@ -40,6 +46,7 @@ void controller_step(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 	ControlState state = {};
 	ControlOutput raw_control_output = {};
 	FlightStatus flight_status = {};
+	ControlLog control_log;
 	loadcontroller(&params);
 
 	//Convert the inputs from controller_step and assins the values that correspond to the inputs of CSim
@@ -50,6 +57,7 @@ void controller_step(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 				 kFlapA_c, Motor_c,
 				 errStat, errMsg, &state_est);
 
+	control_log.stateEstLog = state_est; 
 	// Other modes to be added here
 
 	// flight_status -> struct FlightStatus
@@ -62,17 +70,18 @@ void controller_step(double dcm_g2b_c[], double pqr_c[], double *acc_norm_c,
 				  &state.crosswind, &raw_control_output);
 
 	printf("   debug marker - post crosswindstep \n");
-	//void CrosswindStep(const FlightStatus *flight_status,
-	//	const StateEstimate *state_est,
-	//	const CrosswindParams *params, CrosswindState *state,
-	//	ControlOutput *control_output) {
 
 	char tmp[] = "   controller stepping";
 	int i;
 	for (i = 0; i < sizeof(tmp); i++)
 	{
 		errMsg[i] = tmp[i];
 	}
+	// Saves all variables for this time step in ControlLog struct to txt file for analysis
+	ControlLogEntry(&control_log);
+	// Connects values that are in ControlOutput data struct to the final outputs that Kitefast is expecting.
+	AssignOutputs(kFlapA_c, Motor_c,
+	errStat, errMsg, &raw_control_output);
 }
 
 void controller_end(int *errStat, char *errMsg)