Code changes after testing directly on the boat; APM forbids a parallel...

Code changes after testing directly on the boat; APM forbids a parallel utilisation so a solution with mavlink must be found with the same design

main.cpp

@@ -97,9 +97,7 @@ int main(int argc, char *argv[])
 
     rcin->initialize();
 
-    auto pwm1 = get_rcout();
-    auto pwm2 = get_rcout();
-    auto pwm3 = get_rcout();
+    auto pwm = get_rcout();
 
     //Sécurité sur le bon index de commande des servos
     int iTempIndex = atoi(argv[POSITION_INDEX_CMD]);
@@ -140,34 +138,34 @@ int main(int argc, char *argv[])
         return 1;
     }
 
-    if( !(pwm1->initialize(CHANNEL_SERVO1)) ) {
+    if( !(pwm->initialize(CHANNEL_SERVO1)) ) {
         printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO1);
         return 1;
     }
 
-    if( !(pwm2->initialize(CHANNEL_SERVO2)) ) {
+    if( !(pwm->initialize(CHANNEL_SERVO2)) ) {
         printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO2);
         return 1;
     }
 
-    if( !(pwm3->initialize(CHANNEL_SERVO3)) ) {
+    if( !(pwm->initialize(CHANNEL_SERVO3)) ) {
         printf("Erreur d'initialisation sur le channel de commande servo : %d \n", CHANNEL_SERVO3);
         return 1;
     }
 
-    pwm1->set_frequency(CHANNEL_SERVO1, 50);
-    pwm2->set_frequency(CHANNEL_SERVO2, 50);
-    pwm3->set_frequency(CHANNEL_SERVO3, 50);
+    pwm->set_frequency(CHANNEL_SERVO1, 50);
+    pwm->set_frequency(CHANNEL_SERVO2, 50);
+    pwm->set_frequency(CHANNEL_SERVO3, 50);
 
-    if ( !(pwm1->enable(CHANNEL_SERVO1)) ) {
+    if ( !(pwm->enable(CHANNEL_SERVO1)) ) {
         printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO1);
         return 1;
     }
-    if ( !(pwm2->enable(CHANNEL_SERVO2)) ) {
+    if ( !(pwm->enable(CHANNEL_SERVO2)) ) {
         printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO2);
         return 1;
     }
-    if ( !(pwm3->enable(CHANNEL_SERVO3)) ) {
+    if ( !(pwm->enable(CHANNEL_SERVO3)) ) {
         printf("Erreur d'activation sur le channel de commande servo : %d \n", CHANNEL_SERVO3);
         return 1;
     }
@@ -190,16 +188,20 @@ int main(int argc, char *argv[])
         printf("%d\n", period);
 
         iCmdServo1 = period - DEFAULT_TRIM + iTrimServo1;
-        iCmdServo2 = period - DEFAULT_TRIM + iTrimServo2;
-        iCmdServo3 = period - DEFAULT_TRIM + iTrimServo3;
-
-        //Ecriture sur servos
-        pwm1->set_duty_cycle(CHANNEL_SERVO1, iCmdServo1);
-        pwm2->set_duty_cycle(CHANNEL_SERVO2, iCmdServo2);
-        pwm3->set_duty_cycle(CHANNEL_SERVO3, iCmdServo3);
+        iCmdServo3 = period - DEFAULT_TRIM + iTrimServo2;
+        iCmdServo2 = -(period - DEFAULT_TRIM) + iTrimServo3;
+
+	//Ecriture sur servos
+	pwm->set_duty_cycle(CHANNEL_SERVO1, iCmdServo1);
+	pwm->set_duty_cycle(CHANNEL_SERVO2, iCmdServo2);
+	pwm->set_duty_cycle(CHANNEL_SERVO3, iCmdServo3);
+	
+	printf(" Servo 1 : %d\n",iCmdServo1);
+	printf(" Servo 2 : %d\n",iCmdServo2);
+	printf(" Servo 3 : %d\n",iCmdServo3);
 
         //Freq de 100Hz
-        sleep(1);
+        sleep(0);
     }
 
     return 0;