From 4d8f85960d55b482afc8c2181561a31a7c7f985a Mon Sep 17 00:00:00 2001 From: Matteo Pignataro <matteo.pignataro@tiscali.it> Date: Tue, 13 Sep 2022 18:58:46 +0200 Subject: [PATCH] [Payload] Removed old TODO and added servo angles to telemetry --- src/boards/Payload/Configs/WingConfig.h | 2 +- src/boards/Payload/TMRepository/TMRepository.cpp | 10 +++++++++- src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp | 2 -- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h index c9c0d1b7b..61651be6e 100644 --- a/src/boards/Payload/Configs/WingConfig.h +++ b/src/boards/Payload/Configs/WingConfig.h @@ -35,7 +35,7 @@ namespace WingConfig // Algorithm configuration constexpr uint32_t WING_UPDATE_PERIOD = 100; // [ms] -constexpr uint8_t WING_CONTROLLER_ID = 100; // TODO define a correct ID +constexpr uint8_t WING_CONTROLLER_ID = 100; // Arm, start and flare thresholds constexpr float WING_ALGORITHM_ARM_ALTITUDE = 250; // [m] diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp index 6121cf8a6..e548b8878 100644 --- a/src/boards/Payload/TMRepository/TMRepository.cpp +++ b/src/boards/Payload/TMRepository/TMRepository.cpp @@ -183,6 +183,9 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, tm.pressure_dpl = sensors.getDplPressureLastSample().pressure; tm.airspeed_pitot = sensors.getPitotLastSample().airspeed; + // Altitude agl + tm.altitude_agl = -nasState.d; + // IMU tm.acc_x = imuData.accelerationX; tm.acc_y = imuData.accelerationY; @@ -225,7 +228,12 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, tm.pin_nosecone = PinHandler::getInstance().getPinsData()[NOSECONE_PIN].lastState; - // TODO: Add servo positions + // Servo positions + tm.left_servo_angle = + Actuators::getInstance().getServoAngle(PARAFOIL_LEFT_SERVO); + + tm.right_servo_angle = + Actuators::getInstance().getServoAngle(PARAFOIL_RIGHT_SERVO); // Board status tm.vbat = sensors.getBatteryVoltageLastSample().batVoltage; diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp index 897032c3b..6543ea5dc 100644 --- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp @@ -41,7 +41,6 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo2) : WingAlgorithm(servo1, servo2) { - // TODO define umin and umax for antiwindup purposes controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f, -2.09439, 2.09439); } @@ -60,7 +59,6 @@ void AutomaticWingAlgorithm::step() // UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample(); // Target direction in respect to the current one - // TODO to be logged ReferenceValues reference = NASController::getInstance().getReferenceValues(); Vector2f startingPosition = -- GitLab