diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
index 5493a7a510..db96fad503 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml
@@ -152,11 +152,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -167,10 +167,10 @@
-
-
-
-
+
+
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
index 34f0e4355a..0caab89884 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
@@ -153,11 +153,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -168,10 +168,10 @@
-
-
-
-
+
+
+
+
@@ -277,6 +277,8 @@
+
+
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
index c1aacf5397..1d33f7a50c 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
@@ -145,11 +145,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -160,10 +160,10 @@
-
-
-
-
+
+
+
+
@@ -262,6 +262,8 @@
+
+
@@ -297,26 +299,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
index e39060d373..2f5e19619a 100644
--- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
@@ -116,11 +116,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -131,10 +131,10 @@
-
-
-
-
+
+
+
+
diff --git a/conf/userconf/tudelft/RW_control_panel.xml b/conf/userconf/tudelft/RW_control_panel.xml
index dd29300c81..23e8fab250 100644
--- a/conf/userconf/tudelft/RW_control_panel.xml
+++ b/conf/userconf/tudelft/RW_control_panel.xml
@@ -1290,6 +1290,13 @@
+
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 09567eb895..7e1fb07889 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -596,12 +596,12 @@
+ 25.0){
- du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
- du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
} else {
- du_min_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
- du_max_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
}
- du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_pref[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
break;
case COMMAND_FLAPS:
if(RW.skew.deg > 50.0){
- du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
- du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
} else {
- du_min_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
- du_max_1l[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i];
}
- du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_pref[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
break;
case COMMAND_ELEVATOR:
- du_min_1l[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
- du_max_1l[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
- u_pref[i] = RW.ele_pref;
- du_pref_1l[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i];
+ u_pref[i] = RW.ele_pref;
+ WLS_one_p.u_pref[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i];
break;
case COMMAND_ROLL:
- du_min_1l[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
- du_max_1l[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
- du_pref_1l[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_pref[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
break;
case COMMAND_PITCH:
if (rotwing_state_settings.stall_protection){
- du_min_1l[i] = (RadOfDeg(-17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
- du_max_1l[i] = (RadOfDeg(17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (RadOfDeg(-17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (RadOfDeg(17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
} else {
- du_min_1l[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
- du_max_1l[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_min[i] = (act_min[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_max[i] = (act_max[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
}
- du_pref_1l[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
+ WLS_one_p.u_pref[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i];
break;
}
}
// WLS Control Allocator
normalize_nu();
- number_iter = wls_alloc(andi_du_n, nu_n, du_min_1l, du_max_1l, bwls_1l, 0, 0, Wv_wls, Wu, du_pref_1l, gamma_wls, 10, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS);
+ wls_alloc(&WLS_one_p, bwls_1l, 0, 0, 10);
for (i = 0; i < ANDI_NUM_ACT_TOT; i++){
+ andi_du_n[i] = WLS_one_p.u[i];
andi_du[i] = (float)(andi_du_n[i] * ratio_u_un[i]);
}
@@ -1595,7 +1634,7 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des,
}
if (heading_manual){
psi_des_deg = DegOfRad(psi_des_rad);
- }
+ }
stabilization.cmd[COMMAND_ROLL] = (int16_t) (DegOfRad(eulers_zxy_des.phi ) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_PHI ));
stabilization.cmd[COMMAND_PITCH] = (int16_t) (DegOfRad(eulers_zxy_des.theta) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_THETA));
@@ -1694,15 +1733,15 @@ void calc_normalization(void){
float ratio_numerator = positive_non_zero(nu_norm_max);
float ratio_denominator = 1.0;
switch (i) {
- case (aN):
- case (aE):
- case (aD):
+ case (RW_aN):
+ case (RW_aE):
+ case (RW_aD):
ratio_denominator = positive_non_zero(max_j_nav);
ratio_vn_v[i] = ratio_numerator/ratio_denominator;
break;
- case (ap):
- case (aq):
- case (ar):
+ case (RW_ap):
+ case (RW_aq):
+ case (RW_ar):
ratio_denominator = positive_non_zero(max_j_ang);
ratio_vn_v[i] = ratio_numerator/ratio_denominator;
break;
@@ -1715,6 +1754,7 @@ void normalize_nu(void){
int8_t i;
for (i = 0; i < ANDI_OUTPUTS; i++){
nu_n[i] = nu[i] * ratio_vn_v[i];
+ WLS_one_p.v[i] = nu_n[i];
}
}
@@ -1953,9 +1993,6 @@ void reshape_wind(void)
VECT2_DIFF(windspeed, groundspeed, airspeed_v); // Wind speed in North and East frame
VECT2_DIFF(des_as_NE, NT_v_NE, windspeed); // Desired airspeed in North and East frame
float norm_des_as = FLOAT_VECT2_NORM(des_as_NE);
- //float norm_wind = FLOAT_VECT2_NORM(windspeed);
- //float norm_gs = FLOAT_VECT2_NORM(groundspeed);
- //float norm_nt = FLOAT_VECT2_NORM(NT_v_NE);
nav_target_new[0] = NT_v_NE.x;
nav_target_new[1] = NT_v_NE.y;
// if the desired airspeed is larger than the max airspeed or we are in force forward reshape gs des to cancel wind and fly at max airspeed
@@ -1981,20 +2018,14 @@ void reshape_wind(void)
NT_v_NE.y = groundspeed_factor * NT_v_NE.y;
}
norm_des_as = FLOAT_VECT2_NORM(des_as_NE); // Recalculate norm of desired airspeed
- //NT_v_B.x = cpsi * NT_v_NE.x + spsi * NT_v_NE.y; // Nav target in body x frame
- //NT_v_B.y = -spsi * NT_v_NE.x + cpsi * NT_v_NE.y; // Nav target in body y frame
des_as_B.x = norm_des_as; // Desired airspeed in body x frame
des_as_B.y = 0.0; // Desired airspeed in body y frame
- // If flying fast or if in force forward mode, make turns instead of straight lines
if (((airspeed > ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD) && (norm_des_as > (ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD+2.0f)))|| (rotwing_state_settings.force_forward)){
float delta_psi = atan2f(des_as_NE.y, des_as_NE.x) - psi;
FLOAT_ANGLE_NORMALIZE(delta_psi);
des_acc_B.y = delta_psi * 5.0;//gih_params.heading_bank_gain;
des_acc_B.x = (des_as_B.x - airspeed) * k_pos_rm.k2[0];//gih_params.speed_gain;
- //printf("des_acc_B: %f, des_acc_B.x: %f, des_acc_B.y: %f\n", sqrtf(des_acc_B.x*des_acc_B.x+des_acc_B.y*des_acc_B.y),des_acc_B.x, des_acc_B.y);
- //printf("max_a_nav: %f\n", max_a_nav);
acc_body_bound(&des_acc_B, max_a_nav); // Scale down side acceleration if norm is too large
- //printf("bnd_acc_B: %f, des_acc_B.x: %f, des_acc_B.y: %f\n", sqrtf(des_acc_B.x*des_acc_B.x+des_acc_B.y*des_acc_B.y),des_acc_B.x, des_acc_B.y);
nav_target_new[0] = cpsi * des_acc_B.x - spsi * des_acc_B.y;
nav_target_new[1] = spsi * des_acc_B.x + cpsi * des_acc_B.y;
} else {
@@ -2002,12 +2033,5 @@ void reshape_wind(void)
nav_target_new[1] = (NT_v_NE.y - groundspeed.y) * k_pos_rm.k2[1];
}
vect_bound_nd(nav_target_new, max_a_nav, 2);
- //printf("norm_as : %f, as_x : %f, as_y : %f\n", airspeed, airspeed_v.x, airspeed_v.y);
- //printf("norm_des_as: %f, des_as_x: %f, des_as_y: %f\n", sqrtf(des_as_NE.x*des_as_NE.x+des_as_NE.y*des_as_NE.y), des_as_NE.x, des_as_NE.y);
- //printf("norm_wind : %f, wind_x : %f, wind_y : %f\n", FLOAT_VECT2_NORM(windspeed), windspeed.x, windspeed.y);
- //printf("norm_gs : %f, gs_x : %f, gs_y : %f\n", FLOAT_VECT2_NORM(groundspeed), groundspeed.x, groundspeed.y);
- //printf("norm_nt : %f, nt_x : %f, nt_y : %f\n", FLOAT_VECT2_NORM(NT_v_NE), NT_v_NE.x, NT_v_NE.y);
- //printf("norm_nt_new: %f, nt_new_x: %f, nt_new_y: %f\n", sqrtf(nav_target_new[0]*nav_target_new[0] + nav_target_new[1]*nav_target_new[1]), nav_target_new[0], nav_target_new[1]);
}
-