Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 07:06:51 +00:00
Code Releases Activity

Merge branch '31-avionics-integration-test' into 'main'

Browse Source 
Print Timing Info

See merge request lander-team/lander-cpp!24
This commit is contained in:
Anson Biggs 2021-11-12 22:23:30 +00:00
commit 289f2e7a43
3 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@ -41,6 +41,7 @@ struct Vehicle {
double simTime; double simTime;
double stepSize; double stepSize;
double stepDuration;
double time = 0.0; double time = 0.0;
}; };

12
include/teensy.h
View File

@ -75,7 +75,7 @@ void initFile() {
// File Header // File Header
dataFile.println( dataFile.println(
"t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot," "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
"Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"); "Servo1,Servo2,thrustFiring,thrust,simResponse");
} }
void thrustInfo(Vehicle &State) { void thrustInfo(Vehicle &State) {
@ -168,14 +168,12 @@ void write2CSV(Vehicle &State) {
dataFile.print(String(State.thrustFiring, 5)); dataFile.print(String(State.thrustFiring, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(String(State.PIDx, 5));
dataFile.print(",");
dataFile.print(String(State.PIDy, 5));
dataFile.print(",");
dataFile.print(String(State.thrust, 5)); dataFile.print(String(State.thrust, 5));
dataFile.print(",");
dataFile.print(String(State.stepDuration, 5));
dataFile.print("\n"); dataFile.print("\n");
} }
void printSimResults(Vehicle &State) { void printSimResults(Vehicle &State) {

19
src/main.cpp
View File

@ -12,7 +12,7 @@
#include <Arduino.h> #include <Arduino.h>
int BUILTIN_LED = 13; int BUILTIN_LED = 13;
unsigned long last; unsigned long last, initTime;
#endif #endif
#include "Vehicle.h" #include "Vehicle.h"
@ -75,6 +75,8 @@ void setup() {
delay(1000); delay(1000);
initFile(); initFile();
delay(1000); delay(1000);
initTime = micros();
} }
#endif #endif
@ -100,19 +102,28 @@ void loop() {
#elif defined(TEENSY) #elif defined(TEENSY)
void loop() { void loop() {
last = millis(); last = micros();
vehicleDynamics(State, PrevState); vehicleDynamics(State, PrevState);
thrustInfo(State); thrustInfo(State);
pidController(State, PrevState); pidController(State, PrevState);
TVC(State, PrevState); TVC(State, PrevState);
processTVC(State, loadCells); processTVC(State, loadCells);
State.stepDuration = micros() - last;
write2CSV(State);
// Set "prev" values for next timestep
PrevState = State;
// state2vec(State, PrevState, stateVector); // state2vec(State, PrevState, stateVector);
State.time += State.stepSize; State.time += State.stepSize;
if ((State.z < 0.0) && (State.thrustFiring == 2)) { if ((State.z < 0.0) && (State.thrustFiring == 2)) {
printSimResults(State); printSimResults(State);
Serial.println("Run duration:" + String(millis() - last) + " ms"); Serial.println("Run duration:" + String(micros() - initTime) + " us");
closeFile(); closeFile();
delay(20000); delay(20000);
@ -121,6 +132,8 @@ void loop() {
Serial.println("Aborting Sim"); Serial.println("Aborting Sim");
teensyAbort(); teensyAbort();
} }
delay(20 - ((micros() - last) * 1000.0));
} }
#endif #endif