mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-15 22:56:53 +00:00
Merge branch '31-avionics-integration-test' into 'main'
Resolve "Avionics Integration Test" Closes #31 See merge request lander-team/lander-cpp!25
This commit is contained in:
Brendan McGeeney
commit
f7acf25479
@ -1,37 +0,0 @@
|
||||
|
||||
#include <HX711.h>
|
||||
|
||||
struct LoadCells {
|
||||
HX711 loadcell_0;
|
||||
HX711 loadcell_1;
|
||||
HX711 loadcell_2;
|
||||
HX711 loadcell_3;
|
||||
|
||||
double lc0Value;
|
||||
double lc1Value;
|
||||
double lc2Value;
|
||||
double lc3Value;
|
||||
|
||||
double lc0Calibration;
|
||||
double lc1Calibration;
|
||||
double lc2Calibration;
|
||||
double lc3Calibration;
|
||||
};
|
||||
|
||||
double loadCellCalibrate(HX711 loadCell) {
|
||||
// place code to calibrate load cells in here
|
||||
double loadTotal = 0.0;
|
||||
for (int t = 0; t == 10; ++t) {
|
||||
loadTotal += loadCell.read();
|
||||
delay(15);
|
||||
}
|
||||
return loadTotal / 10.0;
|
||||
}
|
||||
|
||||
void init_LoadCells(LoadCells &loadCells) {
|
||||
|
||||
loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0);
|
||||
loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1);
|
||||
loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2);
|
||||
loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3);
|
||||
}
|
||||
@ -22,7 +22,7 @@ struct Vehicle {
|
||||
double thrust, burnElapsed, burnStart;
|
||||
int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
|
||||
|
||||
double PIDx, PIDy, Fx, Fy, Fz;
|
||||
double PIDx, PIDy, Fx, Fy, Fz, F;
|
||||
double momentX, momentY, momentZ;
|
||||
|
||||
double I11, I22, I33;
|
||||
@ -44,6 +44,8 @@ struct Vehicle {
|
||||
double stepDuration;
|
||||
|
||||
double time = 0.0;
|
||||
|
||||
double lc0, lc1, lc2, lc3;
|
||||
};
|
||||
|
||||
void init_Vehicle(Vehicle &State) {
|
||||
@ -78,7 +80,7 @@ void init_Vehicle(Vehicle &State) {
|
||||
State.momentArm = 0.145; // [m]
|
||||
|
||||
// Sim Step Size
|
||||
State.stepSize = 1.0; // [ms]
|
||||
State.stepSize = 5.0; // [ms]
|
||||
|
||||
// Other Properties
|
||||
State.massPropellant = 0.06; // [kg]
|
||||
|
||||
@ -117,10 +117,10 @@ void printSimResults(Vehicle &State) {
|
||||
std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
|
||||
<< std::endl;
|
||||
|
||||
if (landing_velocity < 0.5) {
|
||||
std::cout << "Landing Velocity < 0.5 m/s | PASS | ";
|
||||
if (landing_velocity < 1.0) {
|
||||
std::cout << "Landing Velocity < 1.0 m/s | PASS | ";
|
||||
} else {
|
||||
std::cout << "Landing Velocity < 0.5 m/s | FAIL | ";
|
||||
std::cout << "Landing Velocity < 1.0 m/s | FAIL | ";
|
||||
}
|
||||
std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
|
||||
<< State.vz << "]" << std::endl;
|
||||
|
||||
@ -1,12 +1,14 @@
|
||||
#include "Vehicle.h"
|
||||
#include <Arduino.h>
|
||||
#include <PWMServo.h>
|
||||
#include <SD.h>
|
||||
#include <SPI.h>
|
||||
|
||||
void testGimbal(class PWMServo &, class PWMServo &);
|
||||
double loadCellCalibrate();
|
||||
void initFile();
|
||||
void thrustInfo(struct Vehicle &);
|
||||
void processTVC(struct Vehicle &);
|
||||
void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
|
||||
void write2CSV(struct Vehicle &);
|
||||
void printSimResults(struct Vehicle &);
|
||||
void teensyAbort();
|
||||
@ -14,14 +16,51 @@ void teensyAbort();
|
||||
const int chipSelect = BUILTIN_SDCARD;
|
||||
File dataFile;
|
||||
|
||||
double loadCellCalibrate() {
|
||||
// place code to calibrate load cells in here
|
||||
double loadTotal = 0.0;
|
||||
for (double t = 0.0; t == 10.0; ++t) {
|
||||
loadTotal += 1.0;
|
||||
delay(15);
|
||||
void testGimbal(PWMServo &servo1, PWMServo &servo2) {
|
||||
int servoTest = 0;
|
||||
|
||||
servo1.write(servoTest);
|
||||
servo2.write(servoTest);
|
||||
|
||||
// Servo 1 Test
|
||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
||||
servo1.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
return loadTotal / 10.0;
|
||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
||||
servo1.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
|
||||
delay(1000);
|
||||
|
||||
// Servo 2 Test
|
||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
||||
servo2.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
||||
servo2.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
|
||||
delay(1000);
|
||||
|
||||
// Servo 1 & 2 Test
|
||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
||||
servo1.write(servoTest);
|
||||
servo2.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
||||
servo1.write(servoTest);
|
||||
servo2.write(servoTest);
|
||||
delay(30);
|
||||
}
|
||||
|
||||
delay(30);
|
||||
servo1.write(0);
|
||||
servo2.write(0);
|
||||
}
|
||||
|
||||
void initFile() {
|
||||
@ -75,7 +114,7 @@ void initFile() {
|
||||
// File Header
|
||||
dataFile.println(
|
||||
"t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
|
||||
"Servo1,Servo2,thrustFiring,thrust,simResponse");
|
||||
"Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3");
|
||||
}
|
||||
|
||||
void thrustInfo(Vehicle &State) {
|
||||
@ -103,16 +142,21 @@ void thrustInfo(Vehicle &State) {
|
||||
}
|
||||
}
|
||||
|
||||
void processTVC(Vehicle &State, LoadCells &loadCells) {
|
||||
void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
|
||||
if (State.time == 0) {
|
||||
Serial.println("WARNING: processTVC not implemented for TEENSY");
|
||||
}
|
||||
|
||||
double r = 3.0;
|
||||
double R = 5.0;
|
||||
// Vector math to aqcuire thrust vector components
|
||||
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
|
||||
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
|
||||
State.Fx = loadCells.lc1Value + loadCells.lc2Value;
|
||||
State.Fy = loadCells.lc0Value + loadCells.lc3Value;
|
||||
State.Fx = (State.lc1 - State.lc2) * r / R;
|
||||
State.Fy = (State.lc0 - State.lc3) * r / R;
|
||||
State.Fz = State.lc0 + State.lc1 + State.lc2 + State.lc3;
|
||||
|
||||
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
|
||||
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
|
||||
State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
|
||||
(State.mass * g);
|
||||
|
||||
@ -120,6 +164,11 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
|
||||
State.momentX = State.Fx * State.momentArm;
|
||||
State.momentY = State.Fy * State.momentArm;
|
||||
State.momentZ = 0.0;
|
||||
|
||||
State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
|
||||
|
||||
servo1.write(State.xServoDegs);
|
||||
servo2.write(State.yServoDegs);
|
||||
}
|
||||
|
||||
void write2CSV(Vehicle &State) {
|
||||
@ -172,8 +221,17 @@ void write2CSV(Vehicle &State) {
|
||||
dataFile.print(",");
|
||||
|
||||
dataFile.print(String(State.stepDuration, 5));
|
||||
dataFile.print("\n");
|
||||
dataFile.print(",");
|
||||
|
||||
dataFile.print(String(State.lc0, 5));
|
||||
dataFile.print(",");
|
||||
dataFile.print(String(State.lc1, 5));
|
||||
dataFile.print(",");
|
||||
dataFile.print(String(State.lc2, 5));
|
||||
dataFile.print(",");
|
||||
dataFile.print(String(State.lc3, 5));
|
||||
|
||||
dataFile.print("\n");
|
||||
}
|
||||
|
||||
void printSimResults(Vehicle &State) {
|
||||
@ -195,10 +253,10 @@ void printSimResults(Vehicle &State) {
|
||||
Serial.print("Final Angles: [" + String(State.yaw) + ", " +
|
||||
String(State.pitch) + "]\n");
|
||||
|
||||
if (landing_velocity < 0.5) {
|
||||
Serial.print("Landing Velocity < 0.5 m/s | PASS | ");
|
||||
if (landing_velocity < 1.0) {
|
||||
Serial.print("Landing Velocity < 1.0 m/s | PASS | ");
|
||||
} else {
|
||||
Serial.print("Landing Velocity < 0.5 m/s | FAIL | ");
|
||||
Serial.print("Landing Velocity < 1.0 m/s | FAIL | ");
|
||||
}
|
||||
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
|
||||
String(State.vy) + ", " + String(State.vz) + "]\n");
|
||||
|
||||
212
src/main.cpp
212
src/main.cpp
@ -1,6 +1,4 @@
|
||||
#define M_PI 3.14159265359
|
||||
|
||||
#if defined(NATIVE) || defined(_WIN32)
|
||||
#if defined(_WIN32) || defined(NATIVE)
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
@ -10,142 +8,190 @@
|
||||
#include <vector>
|
||||
#elif defined(TEENSY)
|
||||
#include <Arduino.h>
|
||||
|
||||
int BUILTIN_LED = 13;
|
||||
unsigned long last, initTime;
|
||||
#endif
|
||||
|
||||
#define M_PI 3.14159265359
|
||||
|
||||
#include "Vehicle.h"
|
||||
#include "sim.h"
|
||||
|
||||
#if defined(NATIVE) || defined(_WIN32)
|
||||
#include "native.h"
|
||||
outVector stateVector;
|
||||
#elif defined(TEENSY)
|
||||
|
||||
#include "LoadCells.h"
|
||||
LoadCells loadCells;
|
||||
|
||||
#include "teensy.h"
|
||||
const int lc_clock = 23;
|
||||
|
||||
const int lc_data_0 = 0;
|
||||
const int lc_data_1 = 1;
|
||||
const int lc_data_2 = 2;
|
||||
const int lc_data_3 = 3;
|
||||
|
||||
#endif
|
||||
|
||||
Vehicle State;
|
||||
Vehicle PrevState;
|
||||
|
||||
#if defined(NATIVE) || defined(_WIN32)
|
||||
void setup() {
|
||||
init_Vehicle(State);
|
||||
#if defined(TEENSY)
|
||||
|
||||
int BUILTIN_LED = 13;
|
||||
unsigned long last, initTime;
|
||||
|
||||
#include "teensy.h"
|
||||
#include <HX711.h>
|
||||
#include <PWMServo.h>
|
||||
|
||||
const int lc_clock = 23;
|
||||
|
||||
const int pin_lc0 = 14;
|
||||
const int pin_lc1 = 15;
|
||||
const int pin_lc2 = 19;
|
||||
const int pin_lc3 = 20;
|
||||
|
||||
void init_loadCells();
|
||||
void read_lc0();
|
||||
void read_lc1();
|
||||
void read_lc2();
|
||||
void read_lc3();
|
||||
|
||||
HX711 lc0;
|
||||
HX711 lc1;
|
||||
HX711 lc2;
|
||||
HX711 lc3;
|
||||
|
||||
PWMServo servo1;
|
||||
PWMServo servo2;
|
||||
|
||||
const int pin_servo1 = 33;
|
||||
const int pin_servo2 = 29;
|
||||
|
||||
int servoPos = 0; // variable to store the servo position;
|
||||
|
||||
// Determine when to burn
|
||||
burnStartTimeCalc(State);
|
||||
}
|
||||
#elif defined(TEENSY)
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
|
||||
// Configure clock pin with high impedance to protect pin (if this doesn't
|
||||
// work, change to OUTPUT)
|
||||
pinMode(lc_clock, INPUT);
|
||||
// Configure load cell data pins as inputs
|
||||
pinMode(lc_data_0, INPUT);
|
||||
pinMode(lc_data_1, INPUT);
|
||||
pinMode(lc_data_2, INPUT);
|
||||
pinMode(lc_data_3, INPUT);
|
||||
delay(5000);
|
||||
Serial.println("Simulation Countdown:");
|
||||
for (int i = 0; i < 10; i++) {
|
||||
Serial.println(10 - i);
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
delay(1000);
|
||||
init_loadCells();
|
||||
init_Vehicle(State);
|
||||
State.lc0 = lc0.get_value();
|
||||
State.lc1 = lc1.get_value();
|
||||
State.lc2 = lc2.get_value();
|
||||
State.lc3 = lc3.get_value();
|
||||
|
||||
servo1.attach(pin_servo1);
|
||||
servo2.attach(pin_servo2);
|
||||
testGimbal(servo1, servo2);
|
||||
Serial.println("Servos Tested");
|
||||
|
||||
delay(3000);
|
||||
Serial.println("Simulated Vehicle Initalized");
|
||||
delay(1000);
|
||||
|
||||
// Determine when to burn
|
||||
burnStartTimeCalc(State);
|
||||
Serial.println("Starting Height Calculated");
|
||||
delay(1000);
|
||||
|
||||
init_LoadCells(loadCells);
|
||||
|
||||
Serial.println("Load Cells Calibrated");
|
||||
delay(1000);
|
||||
initFile();
|
||||
delay(1000);
|
||||
|
||||
initTime = micros();
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(NATIVE) || defined(_WIN32)
|
||||
void loop() {
|
||||
vehicleDynamics(State, PrevState);
|
||||
thrustInfo(State);
|
||||
pidController(State, PrevState);
|
||||
TVC(State, PrevState);
|
||||
processTVC(State);
|
||||
state2vec(State, PrevState, stateVector);
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
PrevState = State;
|
||||
State.time += State.stepSize;
|
||||
|
||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||
write2CSV(stateVector, State);
|
||||
printSimResults(State);
|
||||
init_Vehicle(State);
|
||||
}
|
||||
}
|
||||
#elif defined(TEENSY)
|
||||
void loop() {
|
||||
|
||||
last = micros();
|
||||
|
||||
vehicleDynamics(State, PrevState);
|
||||
thrustInfo(State);
|
||||
pidController(State, PrevState);
|
||||
TVC(State, PrevState);
|
||||
|
||||
processTVC(State, loadCells);
|
||||
processTVC(State, servo1, servo2);
|
||||
|
||||
State.stepDuration = micros() - last;
|
||||
write2CSV(State);
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
// Set previous values for next timestep
|
||||
PrevState = State;
|
||||
|
||||
// state2vec(State, PrevState, stateVector);
|
||||
|
||||
|
||||
State.time += State.stepSize;
|
||||
|
||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||
printSimResults(State);
|
||||
Serial.println("Run duration:" + String(micros() - initTime) + " us");
|
||||
Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
|
||||
" seconds");
|
||||
|
||||
closeFile();
|
||||
delay(20000);
|
||||
delay(3000);
|
||||
|
||||
Serial.println("SUCCESS");
|
||||
Serial.println("Aborting Sim");
|
||||
Serial.println("Exiting Sim");
|
||||
teensyAbort();
|
||||
}
|
||||
|
||||
delay(20 - ((micros() - last) * 1000.0));
|
||||
delay(State.stepSize - ((micros() - last) / 1000.0));
|
||||
}
|
||||
|
||||
void init_loadCells() {
|
||||
// Configure clock pin with high impedance to protect
|
||||
// pin (if this doesn't work, change to OUTPUT)
|
||||
pinMode(lc_clock, INPUT);
|
||||
|
||||
// Configure load cell data pins as inputs
|
||||
pinMode(pin_lc0, INPUT);
|
||||
pinMode(pin_lc1, INPUT);
|
||||
pinMode(pin_lc2, INPUT);
|
||||
pinMode(pin_lc3, INPUT);
|
||||
lc0.begin(pin_lc0, lc_clock);
|
||||
lc1.begin(pin_lc1, lc_clock);
|
||||
lc2.begin(pin_lc2, lc_clock);
|
||||
lc3.begin(pin_lc3, lc_clock);
|
||||
|
||||
// Attach ISRs to load cell data pins to read data when available
|
||||
attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
|
||||
|
||||
Serial.println("Load Cells Initialized");
|
||||
|
||||
Serial.print("Calibrating");
|
||||
lc0.tare();
|
||||
Serial.print(".");
|
||||
lc1.tare();
|
||||
Serial.print(".");
|
||||
lc2.tare();
|
||||
Serial.print(".");
|
||||
lc3.tare();
|
||||
Serial.println(".");
|
||||
Serial.println("Load Cells Calibrated");
|
||||
}
|
||||
|
||||
// ISRs to print data to serial monitor
|
||||
void read_lc0() { State.lc0 = lc0.get_value(); }
|
||||
void read_lc1() { State.lc1 = lc1.get_value(); }
|
||||
void read_lc2() { State.lc2 = lc2.get_value(); }
|
||||
void read_lc3() { State.lc3 = lc3.get_value(); }
|
||||
#endif
|
||||
|
||||
#if defined(_WIN32) || defined(NATIVE)
|
||||
|
||||
#include "native.h"
|
||||
outVector stateVector;
|
||||
|
||||
int main() {
|
||||
|
||||
setup();
|
||||
init_Vehicle(State);
|
||||
|
||||
// Determine when to burn
|
||||
burnStartTimeCalc(State);
|
||||
|
||||
do {
|
||||
loop();
|
||||
vehicleDynamics(State, PrevState);
|
||||
thrustInfo(State);
|
||||
pidController(State, PrevState);
|
||||
TVC(State, PrevState);
|
||||
processTVC(State);
|
||||
state2vec(State, PrevState, stateVector);
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
PrevState = State;
|
||||
State.time += State.stepSize;
|
||||
|
||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||
write2CSV(stateVector, State);
|
||||
printSimResults(State);
|
||||
init_Vehicle(State);
|
||||
}
|
||||
} while ((State.z > 0.0) || (State.thrustFiring != 2));
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user