Resolve "Separate native and teensy code"

include/Vehicle.h

@@ -41,6 +41,51 @@ struct Vehicle {
 
   double simTime;
   int stepSize;
+
+  int time = 0;
 };
 
+void init_Vehicle(Vehicle &State) {
+  // PID Gains
+  State.Kp = -6.8699;
+  State.Ki = 0;
+  State.Kd = -0.775;
+
+  // Initial Velocity
+  State.vx = 0; // [m/s]
+  State.vy = 0; // [m/s]
+  State.vz = 0; // [m/s]
+
+  // Initial YPR
+  State.yaw = 45 * M_PI / 180;   // [rad]
+  State.pitch = 45 * M_PI / 180; // [rad]
+  State.roll = 0 * M_PI / 180;   // [rad]
+
+  // Initial YPRdot
+  State.yawdot = 1 * M_PI / 180;    // [rad/s]
+  State.pitchdot = -1 * M_PI / 180; // [rad/s]
+  State.rolldot = 0 * M_PI / 180;   // [rad/s]
+
+  // Servo Limitation
+  State.maxServo = 7;       // [degs]
+  State.maxServoRate = 360; // [degs/sec]
+
+  // Vehicle Properties
+  State.massInitial = 1.2;       // [kg]
+  State.vehicleHeight = 0.5318;  // [m]
+  State.vehicleRadius = 0.05105; // [m]
+  State.momentArm = 0.145;       // [m]
+
+  // Sim Step Size
+  State.stepSize = 1; // [ms]
+
+  // Other Properties
+  State.massPropellant = 0.06;                                  // [kg]
+  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
+  State.burntime = 3.45 - 0.148;                                // [s]
+  State.mdot = State.massPropellant / State.burntime;           // [kg/s]
+  State.mass = State.massInitial;                               // [kg]
+  State.burnElapsed = 2000;                                     // [s]
+}
+
 #endif