APM飞控程序解读.docx
《APM飞控程序解读.docx》由会员分享,可在线阅读,更多相关《APM飞控程序解读.docx(13页珍藏版)》请在冰豆网上搜索。
APM飞控程序解读
Thisprogramisdistributedinthehopethatitwillbeuseful,
butWITHOUTANYWARRANTY;withouteventheimpliedwarrantyof
MERCHANTABILITYorFITNESSFORAPARTICULARPURPOSE.Seethe
GNUGeneralPublicLicenseformoredetails.
YoushouldhavereceivedacopyoftheGNUGeneralPublicLicense
alongwiththisprogram.Ifnot,see<*/
/*
*ArduCopterVersion
*Creator:
JasonShort
*LeadDeveloper:
RandyMackay
*BasedoncodeandideasfromtheArducopterteam:
PatHickey,JoseJulio,JaniHirvinen,AndrewTridgell,JustinBeech,AdamRivera,Jean-LouisNaudin,RobertoNavoni
*Thanksto:
ChrisAnderson,MikeSmith,JordiMunoz,DougWeibel,JamesGoppert,BenjaminPelletier,RobertLefebvre,MarcoRobustini
*
*SpecialThanksforContributors(inalphabeticalorderbyfirstname):
*
*AdamMRivera:
AutoCompassDeclination
*AmilcarLucas:
Cameramountlibrary
*AndrewTridgell:
Generaldevelopment,MavlinkSupport
*AngelFernandez:
Alphatesting
*DougWeibel:
Libraries
*ChristofSchmid:
Alphatesting
*DaniSaez:
VOctoSupport
*GregoryFletcher:
Cameramountorientationmath
*Guntars:
Armingsafetysuggestion
*HappyKillmore:
MavlinkGCS
*HeinHollander:
OctoSupport
*IgorvanAirde:
ControlLawoptimization
*LeonardHall:
FlightDynamics,Throttle,LoiterandNavigationControllers
*JonathanChallinger:
InertialNavigation
*Jean-LouisNaudin:
AutoLanding
*MaxLevine:
TriSupport,Graphics
*JackDunkle:
Alphatesting
*JamesGoppert:
MavlinkSupport
*JaniHiriven:
Testingfeedback
*JohnArneBirkeland:
PPMEncoder
*JoseJulio:
StabilizationControllaws
*MarcoRobustini:
Leadtester
*MichaelOborne:
MissionPlannerGCS
*MikeSmith:
Libraries,Codingsupport
*Oliver:
Piezosupport
*OlivierAdler:
PPMEncoder
*RobertLefebvre:
HeliSupport&LEDs
*SandroBenigno:
Camerasupport
*
*AndmuchmoresoPLEASEPMmeonDIYDRONEStoaddyourcontributiontotheList
*
*Requiresmodified"mrelax"versionofArduino,whichcanbefoundhere:
**
*/
Itmay
staticAP_HAL:
:
BetterStream*cliSerial;
weneedtokeepastaticdeclarationwhichisn'tguardedbymacros
Realsensorsareused.
Mostsensorsaredisabled,astheHIL
Syntheticsensorsareconfiguredthat
staticGPS*g_gps;
staticGPS_Glitchgps_glitch(g_gps);
#endif
#endif
#ifCONFIG_HAL_BOARD==HAL_BOARD_PX4
staticAP_Compass_PX4compass;
#else
staticAP_Compass_HMC5843compass;
#endif
#endif
#endif#endif*Seelibraries/RC_Channel/formoreinformation
*/
ifwe'veeverreceivedanupdate
};
uint32_tvalue;
}ap;
It'scurrentlytherawIMUrates
;
usedtopointthenoseofthecopteratthenextwaypoint
staticint32_toriginal_wp_bearing;
staticuint32_twp_distance;
Thesevaluesaregenerated
Theyareusedthroughoutthecodewherecosandsin
Thisvalueisresetateacharming
staticfloatsimple_cos_yaw=;
staticfloatsimple_sin_yaw;
staticint32_tsuper_simple_last_bearing;
staticfloatsuper_simple_cos_yaw=;
staticfloatsuper_simple_sin_yaw;
x=lat,y=lon
Incrementedatcircle_rate/second
staticfloatcircle_angle;
staticint16_tsonar_alt;
staticuint8_tsonar_alt_health;staticint32_tnav_roll;
negativePitchmeansgoforward.
staticint32_tnav_pitch;
staticint32_tof_roll;
negativePitchmeansgoforward.
staticint32_tof_pitch;
staticint16_tnav_throttle;staticint32_tnav_yaw;
staticuint8_tyaw_timer;
staticuint32_tcondition_start;
Usedforperformancemonitoringandfailsafeprocessing
staticuint16_tmainLoop_count;
staticAP_HAL:
:
AnalogSource*rssi_analog_source;
Notethatweonly
Soiftheydon'trunon
=false;
Log_Write_Event(DATA_EXIT_FLIP);
}
}
get_look_ahead_yawbreak;
#ifTOY_LOOKUP==TOY_EXTERNAL_MIXER
caseYAW_TOY:
headingtowardshome)tobe180degfromsimplemodeheading
super_simple_last_bearing=wrap_360_cd+18000);
super_simple_cos_yaw=simple_cos_yaw;
super_simple_sin_yaw=simple_sin_yaw;
northfacing)
rollx=-pitchx=+}else{
return;
#ifFRAME_CONFIG==HELI_FRAME
if(control_mode==STABILIZE){
=true;
}else{
=false;
}
(true);
}
get_throttle_althold_with_slew(),(),());
set_target_alt_for_reporting());//To-Do:
returnget_destination_altifweareflyingtoawaypoint
}else{
//pilot'sthrottlemustbeatzerosokeepmotorsoff
set_throttle_out(0,false);
//deactivateaccelbasedthrottlecontroller
throttle_accel_deactivate();
set_target_alt_for_reporting(0);
}
break;
caseTHROTTLE_LAND:
//landingthrottlecontroller
get_throttle_land();
set_target_alt_for_reporting(0);
break;
}
}
//set_target_alt_for_reporting-settargetaltitudeincmforreportingpurposes(logsandgcs)
staticvoidset_target_alt_for_reporting(floatalt_cm)
{
target_alt_for_reporting=alt_cm;
}
//get_target_alt_for_reporting-returnstargetaltitudeincmforreportingpurposes(logsandgcs)
staticfloatget_target_alt_for_reporting()
{
returntarget_alt_for_reporting;
}
staticvoidread_AHRS(void)
{
//PerformIMUcalculationsandgetattitudeinfo
//-----------------------------------------------
#ifHIL_MODE!
=HIL_MODE_DISABLED
//updatehilbeforeahrsupdate
gcs_check_input();
#endif
();
omega=();
}
staticvoidupdate_trig(void){
Vector2fyawvector;
constMatrix3f&temp=();
=//sin
=//cos
();
cos_pitch_x=safe_sqrt(1-*//level=1
cos_roll_x=/cos_pitch_x;//level=1
cos_pitch_x=constrain_float(cos_pitch_x,0,;
//thisreliesonconstrain_float()ofinfinitydoingtherightthing,
//whichitdoesdoinavr-libc
cos_roll_x=constrain_float(cos_roll_x,,;
sin_yaw=constrain_float,,;
cos_yaw=constrain_float,,;
//addedtoconvertearthframetobodyframeforratecontrollers
sin_pitch=sin_roll=/cos_pitch_x;
//updatewp_navcontrollerwithtrigvalues
(cos_yaw,sin_yaw,cos_pitch_x);
//flat:
//0°=cos_yaw:
sin_yaw:
//90°=cos_yaw:
sin_yaw:
//180=cos_yaw:
sin_yaw:
//270=cos_yaw:
sin_yaw:
}
//readbaroandsonaraltitudeat20hz
staticvoidupdate_altitude()
{
#ifHIL_MODE==HIL_MODE_ATTITUDE
//weareintheSIM,fakeoutthebaroandSonar
baro_alt=g_gps->altitude_cm;
if{
sonar_alt=baro_alt;
}
#else
//readinbaroaltitude
baro_alt=read_barometer();
//readinsonaraltitude
sonar_alt=read_sonar();
#endif//HIL_MODE==HIL_MODE_ATTITUDE
//writealtitudeinfotodataflashlogs
if(&MASK_LOG_CTUN)&&()){
Log_Write_Control_Tuning();
}
}
staticvoidtuning(){
//exitimmediatelywhenradiofailsafeisinvokedsotuningvaluesarenotsettozero
if||!
=0){
return;
}
tuning_value=(float)/;
//0to1
switch{
//Roll,Pitchtuning
caseCH6_STABILIZE_ROLL_PITCH_KP:
break;
caseCH6_RATE_ROLL_PITCH_KP:
break;
caseCH6_RATE_ROLL_PITCH_KI:
break;
caseCH6_RATE_ROLL_PITCH_KD:
break;
//Yawtuning
caseCH6_STABILIZE_YAW_KP:
break;
caseCH6_YAW_RATE_KP:
break;
caseCH6_YAW_RATE_KD:
break;
//Altitudeandthrottletuning
caseCH6_ALTITUDE_HOLD_KP:
break;
caseCH6_THROTTLE_RATE_KP:
break;
caseCH6_THROTTLE_RATE_KD:
break;
caseCH6_THROTTLE_ACCEL_KP:
break;
caseCH6_THROTTLE_ACCEL_KI:
break;
caseCH6_THROTTLE_ACCEL_KD:
break;
//Loiterandnavigationtuning
caseCH6_LOITER_POSITION_KP:
break;
caseCH6_LOITER_RATE_KP:
break;
caseCH6_LOITER_RATE_KI:
break;
caseCH6_LOITER_RATE_KD:
break;
caseCH6_WP_SPEED:
//setwaypointnavigationhorizontalspeedto0~1000cm/s
break;
//Acrorollpitchgain
caseCH6_ACRO_RP_KP:
=tuning_value;
break;
//Acroyawgain
caseCH6_ACRO_YAW_KP:
=tuning_value;
break;
caseCH6_RELAY:
if>525)();
if<475)();
break;
#ifFRAME_CONFIG==HELI_FRAME
caseCH6_HELI_EXTERNAL_GYRO:
=tuning_value;
break;
#endif
caseCH6_OPTFLOW_KP:
break;
caseCH6_OPTFLOW_KI:
break;
caseCH6_OPTFLOW_KD:
break;
#ifHIL_MODE!
=HIL_MODE_ATTITUDE//donotallowmodifying_kpor_kp_yawgainsinHILmode
caseCH6_AHRS_YAW_KP:
break;
caseCH6_AHRS_KP:
break;
#endif
caseCH6_INAV_TC:
//To-Do:
allowingtuningTCforxyandzseparately
(tuning_value);
(tuning_value);
break;
caseCH6_DECLINATION:
//setdeclinationto+-20degrees
(ToRad(*-/,false);//2ndparameterisfalsebecausewedonotwanttosavetoeeprombecausethiswouldhaveaperformanceimpact
break;
caseCH6_CIRCLE_RATE:
//setcirclerate
//allowapproximately45degreeturnrateineitherdirection
break;
caseCH6_SONAR_GAIN:
//setsonargain
break;
}
}
AP_HAL_MAIN();