apm无人机主程序分析.docx
- 文档编号:7238478
- 上传时间:2023-01-22
- 格式:DOCX
- 页数:11
- 大小:17.72KB
apm无人机主程序分析.docx
《apm无人机主程序分析.docx》由会员分享,可在线阅读,更多相关《apm无人机主程序分析.docx(11页珍藏版)》请在冰豆网上搜索。
apm无人机主程序分析
3.0.1
*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,LoiterandNavigation
Controllers*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;
2C2c#endif
#endif
#ifCONFIG_HAL_BOARD==HAL_BOARD_PX4staticAP_Compass_PX4compass;
#else
staticAP_Compass_HMC5843compass;
#endif
#endif
19g#endif#endif*Seelibraries/RC_Channel/formoreinformation*/It'scurrentlytherawIMUrates
3f1cm;
usedtopointthenoseofthecopteratthenextwaypointstaticint32_toriginal_wp_bearing;
staticuint32_twp_distance;
Thesevaluesaregenerated
Theyareusedthroughoutthecodewherecosandsin
Thisvalueisresetateacharming20mstaticint32_tinitial_simple_bearing;
3fx=lat,y=lon
Incrementedatcircle_rate/secondstaticfloatcircle_angle;
1.05fstaticint16_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;
3f
staticuint32_tcondition_start;
Usedforperformancemonitoringandfailsafeprocessing
staticuint16_tmainLoop_count;
staticAP_HAL:
:
AnalogSource*rssi_analog_source;
=false;
Log_Write_Event(DATA_EXIT_FLIP);
}
}
get_look_ahead_yawbreak;
#ifTOY_LOOKUP==TOY_EXTERNAL_MIXER
caseYAW_TOY:
controller_desired_alt=get_initial_alt_hold,climb_rate);return;
//donotrunthrottlecontrollersifmotorsdisarmed
if(!
()){
set_throttle_out(0,false);
throttle_accel_deactivate();//donotallowtheaccelbasedthrottletooverrideourcommand
set_target_alt_for_reporting(0);
return;
}
#ifFRAME_CONFIG==HELI_FRAME
if(control_mode==STABILIZE){
=true;
}else{
=false;
}
#endif//HELI_FRAME
switch(throttle_mode){
caseTHROTTLE_MANUAL:
//completelymanualthrottle
if<=0){
set_throttle_out(0,false);
}else{
//sendpilot'soutputdirectlytomotors
pilot_throttle_scaled=get_pilot_desired_throttleset_throttle_out(pilot_throttle_scaled,false);
//updateestimateofthrottlecruise
#ifFRAME_CONFIG==HELI_FRAME
update_throttle_cruise;
#else
update_throttle_cruise(pilot_throttle_scaled);
#endif//HELI_FRAME
//checkifwe'vetakenoffyet
if(!
&&()){
if(pilot_throttle_scaled>{
//wemustbeintheairbynow
set_takeoff_complete(true);
}
}
}
set_target_alt_for_reporting(0);
break;
caseTHROTTLE_MANUAL_TILT_COMPENSATED:
//manualthrottlebutwithangleboost
if<=0){
set_throttle_out(0,false);//noneedforangleboostwithzerothrottle
}else{
pilot_throttle_scaled=get_pilot_desired_throttleset_throttle_out(pilot_throttle_scaled,true);
//updateestimateofthrottlecruise
#ifFRAME_CONFIG==HELI_FRAME
update_throttle_cruise;
#else
update_throttle_cruise(pilot_throttle_scaled);
#endif//HELI_FRAME
if(!
&&()){
if(pilot_throttle_scaled>{
//wemustbeintheairbynow
set_takeoff_complete(true);
}
}
}
set_target_alt_for_reporting(0);
break;
caseTHROTTLE_HOLD:
if{
//altholdpluspilotinputofclimbrate
pilot_climb_rate=get_pilot_desired_climb_rateif(sonar_alt_health>=SONAR_ALT_HEALTH_MAX){
//ifsonarisok,usesurfacetrackingget_throttle_surface_tracking(pilot_climb_rate);//thisfunctioncallsset_target_alt_for_reportingforus
}else{
//ifnosonarfallbackstabilizeratecontroller
get_throttle_rate_stabilized(pilot_climb_rate);//thisfunctioncallsset_target_alt_for_reportingforus
}
}else{
//pilot'sthrottlemustbeatzerosokeepmotorsoff
set_throttle_out(0,false);
set_target_alt_for_reporting(0);
}
break;
caseTHROTTLE_AUTO:
//autopilotaltitudecontrollerwithtargetaltitudeheldin()
if{
get_throttle_althold_with_slew(),(),());
set_target_alt_for_reporting());//To-Do:
returnget_destination_altifweareflyingtoawaypoint
}else{
//pilot'sthrottlemustbeatzerosokeepmotorsoff
set_throttle_out(0,false);
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
//updatehilbeforeahrsupdategcs_check_input();
#endif
();
omega=();
#ifSECONDARY_DMP_ENABLED==ENABLED();
#endif
}
staticvoidupdate_trig(void){
Vector2fyawvector;
constMatrix3f&temp=();
=//sin
=//cos
();
cos_pitch_x=safe_sqrt(1-*//level=1cos_roll_x=/cos_pitch_x;//level=1cos_pitch_x=constrain_float(cos_pitch_x,0,;
//thisreliesonconstrain_float()ofinfinitydoingtherightthing,
//whichitdoesdoinavr-libccos_roll_x=constrain_float(cos_roll_x,,;
sin_yaw=constrain_float,,;
cos_yaw=constrain_float,,;
//addedtoconvertearthframetobodyframeforratecontrollerssin_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,fakeoutthebaroandSonarbaro_alt=g_gps->altitude_cm-gps_base_alt;
if{
sonar_alt=baro_alt;
}
#else
//readinbaroaltitudebaro_alt=read_barometer();
//readinsonaraltitudesonar_alt=read_sonar();
#endif//HIL_MODE==HIL_MODE_ATTITUDE//writealtitudeinfotodataflashlogsif(&MASK_LOG_CTUN)&&()){Log_Write_Control_Tuning();
}
}
staticvoidtuning(){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/sbreak;
//Acroandothertuning
caseCH6_ACRO_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((2.0f*-/100.0f),false);//2ndparameterisfalsebecausewedonotwanttosavetoeeprombecausethiswouldhaveaperformanceimpact
break;
caseCH6_CIRCLE_RATE:
//setcirclerate
//allowapproximately45degreeturnrateineitherdirection
break;
}
}
AP_HAL_MAIN();
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- apm 无人机 主程序 分析