diff --git a/src/boards/Parafoil/Sensors/Sensors.cpp b/src/boards/Parafoil/Sensors/Sensors.cpp index 682aee410792e05ed4ef138133680396971e1bf3..a3352b24beec1b5040d1c17028696cdb3ec943d7 100644 --- a/src/boards/Parafoil/Sensors/Sensors.cpp +++ b/src/boards/Parafoil/Sensors/Sensors.cpp @@ -115,10 +115,12 @@ BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() Sensors::Sensors(TaskScheduler* sched) : scheduler(sched), sensorsCounter(0) {} +// TODO check calibration of gyro +// TODO check axis of bmx + bool Sensors::start() { // Read the magnetometer calibration from predefined file - magCalibration.fromFile("magCalibration.csv"); // Init all the sensors bmx160Init(); diff --git a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp index 0a86b129ff209c0dc9afc26e634e9ba245a5bee9..46a119d4cef2303175298933568d730923929da7 100644 --- a/src/boards/Parafoil/StateMachines/WingController/WingController.cpp +++ b/src/boards/Parafoil/StateMachines/WingController/WingController.cpp @@ -180,7 +180,6 @@ State WingController::state_controlled_descent(const Boardcore::Event& event) case EV_ENTRY: // start automatic algorithm { logStatus(WingControllerState::ALGORITHM_CONTROLLED); - selectAlgorithm(0); setEarlyManeuverPoints( convertTargetPositionToNED(targetPositionGEO), {ModuleManager::getInstance() @@ -403,6 +402,7 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED, float distFromCenterline = 20; // the distance that the M1 and M2 points // must have from the center line + // TODO add parameter // Calculate the angle between the lines <NED Origin, target> and <NED // Origin, M1> This angle is the same for M2 since is symmetric to M1 @@ -426,6 +426,9 @@ void WingController::setEarlyManeuverPoints(Eigen::Vector2f targetNED, currentPosNED; emGuidance.setPoints(targetNED, emcPosition, m1Position, m2Position); + + clGuidance.setPoints(targetNED); + WingTargetPositionData data; data.receivedLat = targetPositionGEO[0]; diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp index d60fd5f45028204080a5f5e0981a0bbb7130cb53..e56bd236e788bd60de821f058a2be529f81ef34a 100644 --- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp @@ -65,17 +65,17 @@ void AutomaticWingAlgorithm::step() modules.get<WindEstimation>()->getWindEstimationScheme()); // Actuate the result - if (result > 0) + if (result >= 0 && result <= 1) { // Activate the servo1 and reset servo2 - modules.get<Actuators>()->setServoAngle(servo1, result); - modules.get<Actuators>()->setServoAngle(servo2, 0); + modules.get<Actuators>()->setServo(servo1, result); + modules.get<Actuators>()->setServo(servo2, 0); } - else + else if (result < 0 && result <= -1) { // Activate the servo2 and reset servo1 - modules.get<Actuators>()->setServoAngle(servo1, 0); - modules.get<Actuators>()->setServoAngle(servo2, result * -1); + modules.get<Actuators>()->setServo(servo1, 0); + modules.get<Actuators>()->setServo(servo2, result * -1); } // Log the servo positions @@ -116,25 +116,30 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) // All angle are computed as angle from the north direction - if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0) - { - // If we are not moving velocityAngle is 0 - velocityAngle = 0; - } - else if (relativeVelocity[0] == 0) - { - // If we are not moving in the N S axis we interpret velocity[1] - velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI / 2; - } - else if (relativeVelocity[1] == 0) - { - // If we are not moving in the E O axis we interpret velocity[0] - velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI; - } - else - { - velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]); - } + // if (relativeVelocity[0] == 0 && relativeVelocity[1] == 0) + // { + // // If we are not moving velocityAngle is 0 + // velocityAngle = 0; + // } + // else if (relativeVelocity[0] == 0) + // { + // // If we are not moving in the N S axis we interpret velocity[1] + // velocityAngle = (relativeVelocity[1] > 0 ? 1 : -1) * Constants::PI / + // 2; + // } + // else if (relativeVelocity[1] == 0) + // { + // // If we are not moving in the E O axis we interpret velocity[0] + // velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI; + // } + // else + // { + // // angle in radiants of the velocity + // velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]); + // } + + // TODO test if this is safe + velocityAngle = atan2(relativeVelocity[1], relativeVelocity[0]); // Compute the angle difference float error = angleDiff(targetAngle, velocityAngle); @@ -147,7 +152,8 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) result = result * (180.f / Constants::PI); // Flip the servo orientation - result *= -1; + // result *= -1; + // TODO check if this is needed // Logs the outputs { @@ -167,7 +173,7 @@ float AutomaticWingAlgorithm::angleDiff(float a, float b) { float diff = a - b; - // Angle difference + // Angle difference if (diff < -Constants::PI || Constants::PI < diff) { diff += Constants::PI;