diff --git a/src/boards/Parafoil/Configs/ActuatorsConfigs.h b/src/boards/Parafoil/Configs/ActuatorsConfigs.h index b7a73497871927f232cf9639847b7292c801207e..6a1c53675cafa9d96fcbd199f6e0da08c4b41158 100644 --- a/src/boards/Parafoil/Configs/ActuatorsConfigs.h +++ b/src/boards/Parafoil/Configs/ActuatorsConfigs.h @@ -36,36 +36,24 @@ static TIM_TypeDef* const SERVO_1_TIMER = TIM3; constexpr Boardcore::TimerUtils::Channel SERVO_1_PWM_CH = Boardcore::TimerUtils::Channel::CHANNEL_1; -constexpr float LEFT_SERVO_ROTATION = 180; // [deg] -constexpr float LEFT_SERVO_MIN_PULSE = 900; // [us] -constexpr float LEFT_SERVO_MAX_PULSE = - LEFT_SERVO_MIN_PULSE + 10 * LEFT_SERVO_ROTATION; // [us] +constexpr float SERVO_ROTATION = 180; + +constexpr float LEFT_SERVO_ROTATION = SERVO_ROTATION; // [deg] +constexpr float LEFT_SERVO_MIN_PULSE = 2500; // [us] +constexpr float LEFT_SERVO_MAX_PULSE = 500; // [us] // Right servo static TIM_TypeDef* const SERVO_2_TIMER = TIM3; constexpr Boardcore::TimerUtils::Channel SERVO_2_PWM_CH = Boardcore::TimerUtils::Channel::CHANNEL_2; -constexpr float RIGHT_SERVO_ROTATION = 180; // [deg] -constexpr float RIGHT_SERVO_MIN_PULSE = 2100; // [us] -constexpr float RIGHT_SERVO_MAX_PULSE = - RIGHT_SERVO_MIN_PULSE - 10 * RIGHT_SERVO_ROTATION; // [us] +constexpr float RIGHT_SERVO_ROTATION = SERVO_ROTATION; // [deg] +constexpr float RIGHT_SERVO_MIN_PULSE = 500; // [us] +constexpr float RIGHT_SERVO_MAX_PULSE = 2500; // [us] // Parafoil twirl constexpr float SERVO_TWIRL_RADIUS = 0.75; // [%] -// Buzzer configs -static TIM_TypeDef* const BUZZER_TIMER = TIM1; -constexpr Boardcore::TimerUtils::Channel BUZZER_CHANNEL = - Boardcore::TimerUtils::Channel::CHANNEL_1; -constexpr uint32_t BUZZER_FREQUENCY = 1000; -constexpr float BUZZER_DUTY_CYCLE = 0.5; - -constexpr uint32_t BUZZER_UPDATE_PERIOD = 100; // [ms] - -constexpr uint32_t ROCKET_SS_ARMED_PERIOD = 500; -constexpr uint32_t ROCKET_SS_LAND_PERIOD = 1000; // [ms] - } // namespace ActuatorsConfigs } // namespace Parafoil diff --git a/src/boards/Parafoil/Configs/WingConfig.h b/src/boards/Parafoil/Configs/WingConfig.h index 9c5fc145cbe7f0598ae57ee91ba0cd09a75c8374..0aa8d462db34576dd2bb8d43a0db6400d9080b36 100644 --- a/src/boards/Parafoil/Configs/WingConfig.h +++ b/src/boards/Parafoil/Configs/WingConfig.h @@ -34,7 +34,7 @@ namespace WingConfig #if defined(CLOSED_LOOP) constexpr int SELECTED_ALGORITHM = 0; #elif EARLY_MANEUVER -constexpr int SELECTED_ALGORITHM = 1; +constexpr int SELECTED_ALGORITHM = 1; #elif SEQUENCE constexpr int SELECTED_ALGORITHM = 2; #else @@ -53,8 +53,8 @@ constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000; // [ms] constexpr int WING_UPDATE_PERIOD = 1000; // [ms] -constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = 2.09439; -constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -2.09439; +constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = 1; +constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -1; constexpr int GUIDANCE_CONFIDENCE = 15; constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD = 250; //[m] @@ -62,14 +62,14 @@ constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD = 150; //[m] constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50; //[m] // TODO check this parameter preflight -constexpr float KP = 0.0075; //[m] -constexpr float KI = 0.0007; //[m] +constexpr float KP = 0.0075; +constexpr float KI = 0.0007; constexpr float ALTITUDE_TRIGGER_DEPLOYMENT_ALTITUDE = 300; // [meters] constexpr int ALTITUDE_TRIGGER_CONFIDENCE = 10; // [number of sample] constexpr int ALTITUDE_TRIGGER_PERIOD = 100; //[ms] -constexpr float MAX_SERVO_APERTURE = 0.5; //[%] +constexpr float MAX_SERVO_APERTURE = 0.8; //[%] struct WingConfigStruct { diff --git a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp index d259f0d934d71e1f770a6c263985cad0eb9306f3..d60fd5f45028204080a5f5e0981a0bbb7130cb53 100644 --- a/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp +++ b/src/boards/Parafoil/Wing/AutomaticWingAlgorithm.cpp @@ -108,19 +108,28 @@ float AutomaticWingAlgorithm::algorithmStep(NASState state, Vector2f windNED) float targetAngle = guidance.calculateTargetAngle(currentPosition, heading); - Vector2f relativeVelocity(state.vn - windNED[0], state.ve - windNED[1]); + Vector2f relativeVelocity(state.vn, state.ve); + // Remove WES for this test // Compute the angle of the current velocity float velocityAngle; - // In case of a 0 north velocity i force the angle to 90 + // 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) { - velocityAngle = (relativeVelocity[0] > 0 ? 1 : -1) * Constants::PI / 2; + // If we are not moving in the E O axis we interpret velocity[0] + velocityAngle = (relativeVelocity[0] > 0 ? 0 : 1) * Constants::PI; } else {