apm无人机主程序分析

3.0.1 *

* Special

Thanks

for

Contributors

(in

alphabetical

order

by

first

name):

*

* Adam M Rivera

* Amilcar Lucas

* Andrew Tridgell

* Angel Fernandez

* Doug Weibel

:Auto Compass Declination

:Camera mount library

:General development, Mavlink Support

:Alpha testing

:Libraries

:Alpha testing

* Christof Schmid

* Dani Saez :V Octo Support

* Gregory Fletcher

:Camera mount orientation math

* Guntars

:Arming safety suggestion

:Mavlink GCS

* HappyKillmore

* Hein Hollander :Octo Support

* Igor van Airde :Control Law optimization

* Leonard Hall

Controllers

* Jonathan Challinger :Inertial Navigation

* Jean-Louis Naudin :Auto Landing

* Max Levine

* Jack Dunkle

:Tri Support, Graphics

:Alpha testing

:Mavlink Support

:Flight Dynamics, Throttle, Loiter and Navigation

* James Goppert

* Jani Hiriven

:Testing feedback

* John Arne Birkeland

:PPM Encoder

* Jose Julio

:Stabilization Control laws

:Lead tester

:Mission Planner GCS

:Libraries, Coding support

:Piezo support

* Marco Robustini

* Michael Oborne

* Mike Smith

* Oliver

* Olivier Adler :PPM Encoder

* Robert Lefebvre

:Heli Support & LEDs

* Sandro Benigno :Camera support

*

* And much more so PLEASE PM me on DIYDRONES to add your contribution

to the List

*

* Requires

modified

version

of

Arduino,

which

can

be

found

here:

*

*/

It may

static AP_HAL::BetterStream* cliSerial;

we need to keep a static declaration which isn't guarded by macros

Real sensors are used.

Most sensors are disabled, as the HIL

Synthetic sensors are configured that

static GPS *g_gps;

2C2c #endif

#endif

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4

static AP_Compass_PX4 compass;

#else

static AP_Compass_HMC5843 compass;

#endif

#endif

19 g

#endif

#endif * See

libraries/RC_Channel/

for

more

information

*/

It's currently the raw IMU rates

3f1cm used to point the nose of the copter at the next waypoint

static int32_t original_wp_bearing;

static uint32_t wp_distance;

These values are generated

They are used throughout the code where cos and sin

This value is reset at each arming

20mstatic int32_t initial_simple_bearing;

3f x = lat, y = lon

Incremented at circle_rate / second

static float circle_angle;

1.05fstatic int16_t sonar_alt;

static uint8_t sonar_alt_health; static int32_t nav_roll;

negative Pitch means go forward.

static int32_t nav_pitch;

static int32_t of_roll;

negative Pitch means go forward.

static int32_t of_pitch;

static int16_t nav_throttle; static int32_t nav_yaw;

static uint8_t yaw_timer;

3f

static uint32_t condition_start;

Used for performance monitoring and failsafe processing

static uint16_t mainLoop_count;

static AP_HAL::AnalogSource* rssi_analog_source;

= false;

Log_Write_Event(DATA_EXIT_FLIP);

}

}

get_look_ahead_yaw break;

#if TOY_LOOKUP == TOY_EXTERNAL_MIXER

case YAW_TOY:

controller_desired_alt

climb_rate); return;

// do not run throttle controllers if motors disarmed

if( !() ) {

set_throttle_out(0, false);

throttle_accel_deactivate(); // do not allow the accel based

throttle to override our command

set_target_alt_for_reporting(0);

return;

}

#if FRAME_CONFIG == HELI_FRAME

if (control_mode == STABILIZE){

= true;

=

get_initial_alt_hold,

} else {

}

= false;

#endif // HELI_FRAME

switch(throttle_mode) {

case THROTTLE_MANUAL:

// completely manual throttle

if <= 0){

set_throttle_out(0, false);

}else{

// send pilot's output directly to motors

pilot_throttle_scaled

=

get_pilot_desired_throttle

set_throttle_out(pilot_throttle_scaled, false);

// update estimate of throttle cruise

#if FRAME_CONFIG == HELI_FRAME

update_throttle_cruise;

#else

update_throttle_cruise (pilot_throttle_scaled);

#endif //HELI_FRAME

// check if we've taken off yet

if (! && ()) {

if (pilot_throttle_scaled > {

// we must be in the air by now

set_takeoff_complete(true);

}

}

}

set_target_alt_for_reporting(0);

break;

case THROTTLE_MANUAL_TILT_COMPENSATED:

// manual throttle but with angle boost

if <= 0) {

set_throttle_out(0, false); // no need for angle boost with

zero throttle

}else{

pilot_throttle_scaled

=

get_pilot_desired_throttle

set_throttle_out(pilot_throttle_scaled, true);

// update estimate of throttle cruise

#if FRAME_CONFIG == HELI_FRAME

update_throttle_cruise;

#else

update_throttle_cruise (pilot_throttle_scaled);

#endif //HELI_FRAME

if (! && ()) {

if (pilot_throttle_scaled > {

// we must be in the air by now

set_takeoff_complete(true);

}

}

}

set_target_alt_for_reporting(0);

break;

case THROTTLE_HOLD:

if {

// alt hold plus pilot input of climb rate

pilot_climb_rate

=

get_pilot_desired_climb_rate

if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {

// if sonar is ok, use surface tracking

get_throttle_surface_tracking(pilot_climb_rate); //

this function calls set_target_alt_for_reporting for us

}else{

// if no sonar fall back stabilize rate controller

get_throttle_rate_stabilized(pilot_climb_rate); //

this function calls set_target_alt_for_reporting for us

}

}else{

// pilot's throttle must be at zero so keep motors off

set_throttle_out(0, false);

set_target_alt_for_reporting(0);

}

break;

case THROTTLE_AUTO:

//

auto

pilot

altitude

controller

with

target

altitude held in

()