Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 21:07:40 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[formation] conf and minor fixes

Browse Source
This commit is contained in:
Felix Ruess
2015-06-10 15:07:06 +02:00
parent fe38164f26
commit 64165e27cc
7 changed files with 78 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/LAAS/conf.xml
+35
View File
@@ -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
-3
View File
@@ -45,9 +45,6 @@
</modules> </modules>
<!-- commands section --> <!-- commands section -->
<servos> <servos>
<servo name="THROTTLE" no="0" min="1200" neutral="1200" max="2000"/> <servo name="THROTTLE" no="0" min="1200" neutral="1200" max="2000"/>
conf/airframes/LAAS/mmlaas_N2.xml
+1 -1
View File
@@ -221,7 +221,7 @@
</section> </section>
<section name="SIMU"> <section name="SIMU">
<define name="YAW_RESPONSE_FACTOR" value="0.5"/> <define name="YAW_RESPONSE_FACTOR" value="1."/>
</section> </section>
<makefile> <makefile>
conf/flight_plans/form_follow.xml
+1 -1
View File
@@ -29,7 +29,7 @@
<call fun="stop_formation()"/> <call fun="stop_formation()"/>
<circle radius="nav_radius" until="FALSE" wp="WAIT2"/> <circle radius="nav_radius" until="FALSE" wp="WAIT2"/>
</block> </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="start_formation()"/> <!-- inform other AC that formation is started -->
<call fun="formation_flight()"/> <!-- formation control loop --> <call fun="formation_flight()"/> <!-- formation control loop -->
</block> </block>
conf/modules/formation_flight.xml
+5 -5
View File
@@ -8,11 +8,11 @@
<dl_settings> <dl_settings>
<dl_settings name="formation"> <dl_settings name="formation">
<dl_setting MAX="24" MIN="0" STEP="1" VAR="leader_id" module="multi/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>
</dl_settings> </dl_settings>
</settings> </settings>
sw/airborne/modules/multi/formation.c
+35 -26
View File
@@ -29,7 +29,7 @@ float coef_form_pos;
float coef_form_speed; float coef_form_speed;
float coef_form_course; float coef_form_course;
float coef_form_alt; float coef_form_alt;
int form_mode; uint8_t form_mode;
uint8_t leader_id; uint8_t leader_id;
float old_cruise, old_alt; float old_cruise, old_alt;
@@ -99,7 +99,7 @@ int start_formation(void)
for (i = 0; i < NB_ACS; ++i) { for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == IDLE) { formation[i].status = ACTIVE; } 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); DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &active);
// store current cruise and alt // store current cruise and alt
old_cruise = v_ctl_auto_throttle_cruise_throttle; old_cruise = v_ctl_auto_throttle_cruise_throttle;
@@ -114,7 +114,7 @@ int stop_formation(void)
for (i = 0; i < NB_ACS; ++i) { for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == ACTIVE) { formation[i].status = IDLE; } 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); DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &idle);
// restore cruise and alt // restore cruise and alt
v_ctl_auto_throttle_cruise_throttle = old_cruise; v_ctl_auto_throttle_cruise_throttle = old_cruise;
@@ -130,26 +130,27 @@ int formation_flight(void)
static uint8_t _1Hz = 0; static uint8_t _1Hz = 0;
uint8_t nb = 0, i; 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_n = 0.;
form_e = 0.; form_e = 0.;
form_a = 0.; form_a = 0.;
form_speed = (*stateGetHorizontalSpeedNorm_f()); 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) { if (AC_ID == leader_id) {
stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east; stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
} }
// set info for this AC // 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 // broadcast info
uint8_t ac_id = AC_ID; 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); DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
if (++_1Hz >= 4) { if (++_1Hz >= 4) {
_1Hz = 0; _1Hz = 0;
@@ -163,7 +164,10 @@ int formation_flight(void)
// get leader info // get leader info
struct ac_info_ * leader = get_ac_info(leader_id); struct ac_info_ * leader = get_ac_info(leader_id);
if (formation[the_acs_id[leader_id]].status == UNSET || 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 // compute slots in the right reference frame
struct slot_ form[NB_ACS]; struct slot_ form[NB_ACS];
@@ -188,18 +192,20 @@ int formation_flight(void)
// if AC not responding for too long // if AC not responding for too long
formation[i].status = LOST; formation[i].status = LOST;
continue; continue;
} else { formation[i].status = ACTIVE; } } else {
// compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) // 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;
form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x) if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
- (form[i].east - form[the_acs_id[AC_ID]].east); form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y) - (form[i].east - form[the_acs_id[AC_ID]].east);
- (form[i].north - form[the_acs_id[AC_ID]].north); form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); - (form[i].north - form[the_acs_id[AC_ID]].north);
form_speed += ac->gspeed; form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
//form_speed_e += ac->gspeed * sinf(ac->course); form_speed += ac->gspeed;
//form_speed_n += ac->gspeed * cosf(ac->course); //form_speed_e += ac->gspeed * sinf(ac->course);
++nb; //form_speed_n += ac->gspeed * cosf(ac->course);
++nb;
}
} }
} }
uint8_t _nb = Max(1, nb); uint8_t _nb = Max(1, nb);
@@ -215,8 +221,11 @@ int formation_flight(void)
// altitude loop // altitude loop
float alt = 0.; 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; alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT); flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);
@@ -232,7 +241,7 @@ int formation_flight(void)
desired_y = leader->north + dy; desired_y = leader->north + dy;
// lateral correction // lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy)); //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); //NormRadAngle(diff_course);
//h_ctl_roll_setpoint += coef_form_course * diff_course; //h_ctl_roll_setpoint += coef_form_course * diff_course;
//h_ctl_roll_setpoint += coef_form_course * diff_heading; //h_ctl_roll_setpoint += coef_form_course * diff_heading;
sw/airborne/modules/multi/formation.h
+1 -1
View File
@@ -15,7 +15,7 @@
extern float coef_form_alt, coef_form_pos, coef_form_speed, coef_form_course; extern float coef_form_alt, coef_form_pos, coef_form_speed, coef_form_course;
extern float form_prox; extern float form_prox;
extern int form_mode; extern uint8_t form_mode;
extern uint8_t leader_id; extern uint8_t leader_id;
enum slot_status {UNSET, ACTIVE, IDLE, LOST}; enum slot_status {UNSET, ACTIVE, IDLE, LOST};