diff --git a/conf/flight_plans/tudelft/oneloop_cyberzoo.xml b/conf/flight_plans/tudelft/oneloop_cyberzoo.xml index 51faba79da..6ae2d06b9d 100644 --- a/conf/flight_plans/tudelft/oneloop_cyberzoo.xml +++ b/conf/flight_plans/tudelft/oneloop_cyberzoo.xml @@ -8,7 +8,14 @@ #include "modules/radio_control/radio_control.h" #include "modules/ahrs/ahrs.h" #include "modules/gps/gps.h" - + inline static void set_demo_skew(void) { + float amplitude_skew_demo = (rotwing_demo_max_skew - rotwing_demo_min_skew) / 2; + float offset_skew_demo = (rotwing_demo_max_skew + rotwing_demo_min_skew) / 2; + float time_skew_demo = (float) rotwing_demo_timestep / NAVIGATION_FREQUENCY; + float angle_skew_demo = amplitude_skew_demo * (-cosf(2 * M_PI * rotwing_demo_freq_skew * time_skew_demo)) + offset_skew_demo; + rotwing_state.sp_skew_angle_deg = angle_skew_demo; + rotwing_demo_timestep++; + } @@ -39,6 +46,12 @@ + + + + + + + + +