removed compensation ratio and G matrix with heavier blades

paparazzi · Aug 8, 2016 · c40c0d0 · c40c0d0
1 parent 895909d
commit c40c0d0
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 12 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -289,7 +289,7 @@ void stabilization_attitude_run(bool enable_integrator)
 
   // Add euler dynamics compensation
     // Add euler dynamics compensation
-  float compensation_ratio = 0.5;
+  float compensation_ratio = 0.0;
   stabilization_cmd[COMMAND_ROLL] =  stabilization_cmd[COMMAND_ROLL]  + 299*3.43*body_rate_f->q  * compensation_ratio;
   stabilization_cmd[COMMAND_PITCH] = stabilization_cmd[COMMAND_PITCH] + 120.1*-7.45*body_rate_f->p * compensation_ratio;
 

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
@@ -236,7 +236,7 @@ void stabilization_rate_run(bool in_flight)
   stabilization_cmd[COMMAND_YAW]   = stabilization_rate_fb_cmd.r;
 
   // Add euler dynamics compensation
-  float compensation_ratio = 0.5;
+  float compensation_ratio = 0.0;
   stabilization_cmd[COMMAND_ROLL] =  stabilization_cmd[COMMAND_ROLL]  + 299*3.43*body_rate->q  * compensation_ratio;
   stabilization_cmd[COMMAND_PITCH] = stabilization_cmd[COMMAND_PITCH] + 120.1*-7.45*body_rate->p * compensation_ratio;
 

diff --git a/sw/airborne/modules/helicopter/swashplate_mixing.c b/sw/airborne/modules/helicopter/swashplate_mixing.c
@@ -85,16 +85,19 @@ void swashplate_mixing_run(pprz_t in_cmd[])
   int16_t cmd_roll;
   int16_t cmd_pitch;
 
-  if(radio_control.values[7]<-4500) {
-    // Add advance compensation with G matrix
-    cmd_roll  = 0.8578*in_cmd[COMMAND_ROLL] + -0.3276*in_cmd[COMMAND_PITCH];
-    cmd_pitch = 0.5139*in_cmd[COMMAND_ROLL] + 0.9448*in_cmd[COMMAND_PITCH];
-  }
-  else {
-    // Add advance compensation with G matrix
-    cmd_roll  = 0.7753*in_cmd[COMMAND_ROLL] + -0.6635*in_cmd[COMMAND_PITCH];
-    cmd_pitch = 0.6316*in_cmd[COMMAND_ROLL] + 0.7482*in_cmd[COMMAND_PITCH];
-  }
+  cmd_roll = in_cmd[COMMAND_ROLL];
+  cmd_pitch = in_cmd[COMMAND_PITCH];
+
+//  if(radio_control.values[7]<-4500) {
+//    // Add advance compensation with G matrix
+//    cmd_roll  = 0.8578*in_cmd[COMMAND_ROLL] + -0.3276*in_cmd[COMMAND_PITCH];
+//    cmd_pitch = 0.5139*in_cmd[COMMAND_ROLL] + 0.9448*in_cmd[COMMAND_PITCH];
+//  }
+//  else {
+//    // Add advance compensation with G matrix
+//    cmd_roll  = 0.7753*in_cmd[COMMAND_ROLL] + -0.6635*in_cmd[COMMAND_PITCH];
+//    cmd_pitch = 0.6316*in_cmd[COMMAND_ROLL] + 0.7482*in_cmd[COMMAND_PITCH];
+//  }
 
   // Go trough all the motors and calculate the command
   for (i = 0; i < SW_NB; i++) {