[formation] conf and minor fixes

conf/airframes/LAAS/conf.xml

@@ -0,0 +1,35 @@
+<conf>
+  <aircraft
+   name="N1"
+   ac_id="21"
+   airframe="airframes/LAAS/mmlaas_N1.xml"
+   radio="radios/cockpitSX.xml"
+   telemetry="telemetry/default_fixedwing.xml"
+   flight_plan="flight_plans/form_leader.xml"
+   settings="settings/fixedwing_basic.xml"
+   settings_modules="modules/formation_flight.xml modules/infrared_adc.xml"
+   gui_color="blue"
+  />
+  <aircraft
+   name="N2"
+   ac_id="22"
+   airframe="airframes/LAAS/mmlaas_N2.xml"
+   radio="radios/cockpitSX.xml"
+   telemetry="telemetry/default_fixedwing.xml"
+   flight_plan="flight_plans/form_follow.xml"
+   settings="settings/fixedwing_basic.xml"
+   settings_modules="modules/formation_flight.xml modules/infrared_adc.xml"
+   gui_color="white"
+  />
+  <aircraft
+   name="N3"
+   ac_id="23"
+   airframe="airframes/LAAS/mmlaas_N3.xml"
+   radio="radios/cockpitSX.xml"
+   telemetry="telemetry/default_fixedwing.xml"
+   flight_plan="flight_plans/form_follow.xml"
+   settings="settings/fixedwing_basic.xml"
+   settings_modules="modules/formation_flight.xml modules/infrared_adc.xml"
+   gui_color="red"
+  />
+</conf>

conf/airframes/LAAS/mmlaas_N1.xml

@@ -45,9 +45,6 @@
   </modules>
 
 
-
-
-
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE"   no="0" min="1200" neutral="1200" max="2000"/>

conf/airframes/LAAS/mmlaas_N2.xml

@@ -221,7 +221,7 @@
   </section>
 
   <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+    <define name="YAW_RESPONSE_FACTOR" value="1."/>
   </section>
 
  <makefile>

conf/flight_plans/form_follow.xml

@@ -29,7 +29,7 @@
       <call fun="stop_formation()"/>
       <circle radius="nav_radius" until="FALSE" wp="WAIT2"/>
     </block>
-    <block name="formation"> <!-- start formation for a follower, leader has to be ready and in formation -->
+    <block name="formation" strip_button="formation" strip_icon="on.png"> <!-- start formation for a follower, leader has to be ready and in formation -->
       <call fun="start_formation()"/> <!-- inform other AC that formation is started -->
       <call fun="formation_flight()"/> <!-- formation control loop -->
     </block>

conf/modules/formation_flight.xml

@@ -8,11 +8,11 @@
     <dl_settings>
       <dl_settings name="formation">
         <dl_setting MAX="24" MIN="0" STEP="1" VAR="leader_id" module="multi/formation"/>
-        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_pos" module="multi/formation"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_pos" shortname="pos_gain" module="multi/formation" param="FORM_POS_PGAIN"/>
-        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_speed" module="multi/formation"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_speed" shortname="speed_gain" module="multi/formation" param="FORM_SPEED_PGAIN"/>
-        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_course" module="multi/formation"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_course" shortname="course_gain" module="multi/formation" param="FORM_COURSE_PGAIN"/>
-        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_alt" module="multi/formation"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="coef_form_alt" shortname="alt_gain" module="multi/formation" param="FORM_ALTITUDE_PGAIN"/>
-        <dl_setting MAX="1" MIN="0" STEP="1" VAR="form_mode" module="multi/formation" values="GLOBAL|LOCAL"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" VAR="form_mode" module="multi/formation" values="GLOBAL|LOCAL" param="FORM_MODE"/>
       </dl_settings>
     </dl_settings>
   </settings>

sw/airborne/modules/multi/formation.c

@@ -29,7 +29,7 @@ float coef_form_pos;
 float coef_form_speed;
 float coef_form_course;
 float coef_form_alt;
-int form_mode;
+uint8_t form_mode;
 uint8_t leader_id;
 float old_cruise, old_alt;
 
@@ -99,7 +99,7 @@ int start_formation(void)
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == IDLE) { formation[i].status = ACTIVE; }
   }
-  enum slot_status active = ACTIVE;
+  uint8_t active = ACTIVE;
   DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &active);
   // store current cruise and alt
   old_cruise = v_ctl_auto_throttle_cruise_throttle;
@@ -114,7 +114,7 @@ int stop_formation(void)
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == ACTIVE) { formation[i].status = IDLE; }
   }
-  enum slot_status idle = IDLE;
+  uint8_t idle = IDLE;
   DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &idle);
   // restore cruise and alt
   v_ctl_auto_throttle_cruise_throttle = old_cruise;
@@ -130,26 +130,27 @@ int formation_flight(void)
 
   static uint8_t _1Hz   = 0;
   uint8_t nb = 0, i;
-  float ch = cosf((*stateGetHorizontalSpeedDir_f()));
+  float hspeed_dir = (*stateGetHorizontalSpeedDir_f());
-  float sh = sinf((*stateGetHorizontalSpeedDir_f()));
+  float ch = cosf(hspeed_dir);
+  float sh = sinf(hspeed_dir);
   form_n = 0.;
   form_e = 0.;
   form_a = 0.;
   form_speed = (*stateGetHorizontalSpeedNorm_f());
-  form_speed_n = (*stateGetHorizontalSpeedNorm_f()) * ch;
+  form_speed_n = form_speed * ch;
-  form_speed_e = (*stateGetHorizontalSpeedNorm_f()) * sh;
+  form_speed_e = form_speed * sh;
 
   if (AC_ID == leader_id) {
     stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
     stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
   }
   // set info for this AC
-  SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()),
+  SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
-            stateGetPositionUtm_f()->alt, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow);
+            stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);
 
   // broadcast info
   uint8_t ac_id = AC_ID;
-  enum slot_status status = formation[the_acs_id[AC_ID]].status;
+  uint8_t status = formation[the_acs_id[AC_ID]].status;
   DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
   if (++_1Hz >= 4) {
     _1Hz = 0;
@@ -163,7 +164,10 @@ int formation_flight(void)
   // get leader info
   struct ac_info_ * leader = get_ac_info(leader_id);
   if (formation[the_acs_id[leader_id]].status == UNSET ||
-      formation[the_acs_id[leader_id]].status == IDLE) { return FALSE; } // leader not ready or not in formation
+      formation[the_acs_id[leader_id]].status == IDLE) {
+    // leader not ready or not in formation
+    return FALSE;
+  }
 
   // compute slots in the right reference frame
   struct slot_ form[NB_ACS];
@@ -188,9 +192,10 @@ int formation_flight(void)
       // if AC not responding for too long
       formation[i].status = LOST;
       continue;
-    } else { formation[i].status = ACTIVE; }
+    } else {
       // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
-    if (formation[i].status == ACTIVE && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox && ac->alt > 0) {
+      formation[i].status = ACTIVE;
+      if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
         form_e += (ac->east  + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
           - (form[i].east - form[the_acs_id[AC_ID]].east);
         form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
@@ -202,6 +207,7 @@ int formation_flight(void)
         ++nb;
       }
     }
+  }
   uint8_t _nb = Max(1, nb);
   form_n /= _nb;
   form_e /= _nb;
@@ -215,8 +221,11 @@ int formation_flight(void)
 
   // altitude loop
   float alt = 0.;
-  if (AC_ID == leader_id) { alt = nav_altitude; }
+  if (AC_ID == leader_id) {
-  else { alt = leader->alt - form[the_acs_id[leader_id]].alt; }
+    alt = nav_altitude;
+  } else {
+    alt = leader->alt - form[the_acs_id[leader_id]].alt;
+  }
   alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
   flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);
 
@@ -232,7 +241,7 @@ int formation_flight(void)
     desired_y = leader->north + dy;
     // lateral correction
     //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
-    //float diff_course = leader->course - (*stateGetHorizontalSpeedDir_f());
+    //float diff_course = leader->course - hspeed_dir;
     //NormRadAngle(diff_course);
     //h_ctl_roll_setpoint += coef_form_course * diff_course;
     //h_ctl_roll_setpoint += coef_form_course * diff_heading;

sw/airborne/modules/multi/formation.h

@@ -15,7 +15,7 @@
 
 extern float coef_form_alt, coef_form_pos, coef_form_speed, coef_form_course;
 extern float form_prox;
-extern int form_mode;
+extern uint8_t form_mode;
 extern uint8_t leader_id;
 
 enum slot_status {UNSET, ACTIVE, IDLE, LOST};