APM飞控程序解读

/////////////////////////////////////////// /////////////////////////////////////

// Blinking indicates GPS status

static uint8_t copter_leds_GPS_blink;

// Blinking indicates battery status

static uint8_t copter_leds_motor_blink;

// Navigation confirmation blinks

static int8_t copter_leds_nav_blink;

/////////////////////////// ////////////////////////////////////////////////// ///

// GPS variables

/////// ////////////////////////////////////////////////// ///////////////////////

// This is used to scale GPS values for EEPROM storage

// 10^7 times Decimal GPS means 1 == 1cm

// This approximation makes calculations integer and it's easy to read

static const float t7 = 10000000.0;

// We use atan2 and other trig techniques to calaculate angles

// We need to scale the longitude up to make these calcs work

// to account for decreasing distance between lines of longitude away from the equator

static float scaleLongUp = 1;

// Sometimes we need to remove the scaling for distance calcs

static float scaleLongDown = 1;

//////// ////////////////////////////////////////////////// //////////////////////

// Location & Navigation

//////////////////////////// ////////////////////////////////////////////////// //

// This is the angle from the copter to the next waypoint in centi-degrees

static int32_t wp_bearing;

// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint

static int32_t original_wp_bearing;

// The location of home in relation to the copter in centi-degrees

static int32_t home_bearing;

// distance between plane and home in cm

static int32_t home_distance;

// distance between plane and next waypoint in cm.

static uint32_t wp_distance;

// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WP

static uint8_t nav_mode;

// Register containing the index of the current navigation command in the mission script

static int16_t command_nav_index;

// Register containing the index of the previous navigation command in the mission script

// Used to manage the execution of conditional commands

static uint8_t prev_nav_index;

// Register containing the index of the current conditional command in the mission script

static uint8_t command_cond_index;

// Used to track the required WP navigation information

// options include

// NAV_ALTITUDE - have we reached the desired altitude?

// NAV_LOCATION - have we reached the desired location?

// NAV_DELAY - have we waited at the waypoint the desired time?

static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter

target position

static int16_t control_roll;

static int16_t control_pitch;

static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc)

static uint8_t land_state; // records state of land (flying to location, descending)

/////////////// ////////////////////////////////////////////////// ///////////////

// Orientation

< br>/////////////////////////////////////////////// /////////////////////////////////

// Convienience accessors for commonly used trig functions. These values are generated

// by the DCM through a few simple equations. They are used throughout the code where cos and sin

// would normally be used.

// The cos values are defaulted to 1 to get a decent initial value for a level state

static float cos_roll_x = 1.0;

static float cos_pitch_x = 1.0;

static float cos_yaw = 1.0;

static float sin_yaw;

static float sin_roll;

static float sin_pitch;

/////////////////////////// ////////////////////////////////////////////////// ///

// SIMPLE Mode

///////// ////////////////////////////////////////////////// /////////////////////

// Used to track the orientation of the copter for Simple mode. This value is reset at each arming

// or in SuperSimple mode when the copter leaves a 20m radius from home.

static float simple_cos_yaw = 1.0;

static float simple_sin_yaw;

static int32_t super_simple_last_bearing;

static float super_simple_cos_yaw = 1.0;

static float super_simple_sin_yaw;

// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable

static int32_t initial_armed_bearing;

/////////////// ////////////////////////////////////////////////// ///////////////

// Rate contoller targets

/////////////////////////////// /////////////////////////////////////////////////

static

uint8_t

rate_targets_frame = EARTH_FRAME; //

indicates whether rate targets provided

in earth

or

body

frame

static int32_t roll_rate_target_ef;

static int32_t pitch_rate_target_ef;

static int32_t yaw_rate_target_ef;

static int32_t roll_rate_target_bf; // body frame roll rate target

static int32_t pitch_rate_target_bf; // body frame pitch rate target

static int32_t yaw_rate_target_bf; // body frame yaw rate target

//////////////////// ////////////////////////////////////////////////// //////////

// Throttle variables

///////////////////////////////////////////// ///////////////////////////////////

static int16_t throttle_accel_target_ef; // earth frame throttle acceleration target

static bool throttle_accel_controller_active; // true when accel based throttle controller is active, false

when higher level throttle controllers are providing throttle output directly

static float throttle_avg; // le_cruise as a float

static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only

static float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)

///////////////////////////////////////////////// ///////////////////////////////

// ACRO Mode

////////////////////////////////// //////////////////////////////////////////////

// Used to control Axis lock

static int32_t acro_roll; // desired roll angle while sport mode

static int32_t acro_roll_rate; // desired roll rate while in acro mode

static int32_t acro_pitch; // desired pitch angle while sport mode

static int32_t acro_pitch_rate; // desired pitch rate while acro mode

static int32_t acro_yaw_rate; // desired yaw rate while acro mode

static float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input

from pilot

// Filters

#if FRAME_CONFIG == HELI_FRAME

//static LowPassFilterFloat rate_roll_filter; // Rate Roll filter

//static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter

#endif // HELI_FRAME

///////////////////////////////////////////// ///////////////////////////////////

// Circle Mode / Loiter control

////////// ////////////////////////////////////////////////// ////////////////////

Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon

// angle from the circle center to the copter's desired location. Incremented at circle_rate / second

static float circle_angle;

// the total angle (in radians) travelled

static float circle_angle_total;

// deg : how many times to circle as specified by mission command

static uint8_t circle_desired_rotations;

static float circle_angular_acceleration; // circle mode's angular acceleration

static float circle_angular_velocity; // circle mode's angular velocity

static float circle_angular_velocity_max; // circle mode's max angular velocity

// How long we should stay in Loiter Mode for mission scripting (time in seconds)

static uint16_t loiter_time_max;

// How long have we been loitering - The start time in millis

static uint32_t loiter_time;

/////////////// ////////////////////////////////////////////////// ///////////////

// CH7 and CH8 save waypoint control

////////////////////// ////////////////////////////////////////////////// ////////

// This register tracks the current Mission Command index when writing

// a mission using Ch7 or Ch8 aux switches in flight

static int8_t aux_switch_wp_index;

// Battery Sensors

/////////////////////// ////////////////////////////////////////////////// ///////

static AP_BattMonitor battery;

/////////////// ////////////////////////////////////////////////// ///////////////

// Altitude

////////////////////////////////////////////////// //////////////////////////////

// The (throttle) controller desired altitude in cm

static float controller_desired_alt;

// The cm we are off in altitude from next_

–

Positive value means we are below the WP

static int32_t altitude_error;

// The cm/s we are moving up or down based on filtered data - Positive = UP

static int16_t climb_rate;

// The altitude as reported by Sonar in cm

–

Values are 20 to 700 generally.

static int16_t sonar_alt;

static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar

static float target_sonar_alt; // desired altitude in cm above the ground

// The altitude as reported by Baro in cm

–

Values can be quite high

static int32_t baro_alt;

static int16_t saved_toy_throttle;

///////////////////////////////////////////// ///////////////////////////////////

// flight modes

////////////////////////// ////////////////////////////////////////////////// ////

// Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes

// Each Flight mode is a unique combination of these modes

//

// The current desired control scheme for Yaw

static uint8_t yaw_mode;

// The current desired control scheme for roll and pitch / navigation

static uint8_t roll_pitch_mode;

// The current desired control scheme for altitude hold

static uint8_t throttle_mode;

/////////////// ////////////////////////////////////////////////// ///////////////

// flight specific

/////////////////////////////////////////// /////////////////////////////////////

// An additional throttle added to keep the copter at the same altitude when banking

static int16_t angle_boost;

// counter to verify landings

static uint16_t land_detector;

/////////////// ////////////////////////////////////////////////// ///////////////

// 3D Location vectors

/////////////////////////////////////// /////////////////////////////////////////

// home location is stored when we have a good GPS lock and arm the copter

// Can be reset each the copter is re-armed

static struct Location home;

// Current location of the copter

static struct Location current_loc;

// Holds the current loaded command from the EEPROM for navigation

static struct Location command_nav_queue;

// Holds the current loaded command from the EEPROM for conditional scripts

static struct Location command_cond_queue;

/////////////////////////// ////////////////////////////////////////////////// ///

// Navigation Roll/Pitch functions

/////////////////////////////////////// /////////////////////////////////////////

// all angles are deg * 100 : target yaw angle

// The Commanded ROll from the autopilot.

static int32_t nav_roll;

// The Commanded pitch from the autopilot. negative Pitch means go forward.

static int32_t nav_pitch;

// The Commanded ROll from the autopilot based on optical flow sensor.

static int32_t of_roll;

// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.

static int32_t of_pitch;

/////////////// ////////////////////////////////////////////////// ///////////////

// Navigation Throttle control

/////////////////////////////// /////////////////////////////////////////////////

// The Commanded Throttle from the autopilot.

static int16_t nav_throttle; // 0-1000 for throttle control

// This is a simple counter to track the amount of throttle used during flight

// This could be useful later in determining and debuging current usage and predicting battery life

static uint32_t throttle_integrator;

/////////////// ////////////////////////////////////////////////// ///////////////

// Navigation Yaw control

/////////////////////////////// /////////////////////////////////////////////////

// The Commanded Yaw from the autopilot.

static int32_t nav_yaw;

static uint8_t yaw_timer;

// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION

static Vector3f yaw_look_at_WP;

// bearing from current location to the yaw_look_at_WP

static int32_t yaw_look_at_WP_bearing;

// yaw used for YAW_LOOK_AT_HEADING yaw_mode

static int32_t yaw_look_at_heading;

// Deg/s we should turn

static int16_t yaw_look_at_heading_slew;

/////////////////////////// ////////////////////////////////////////////////// ///

// Repeat Mission Scripting Command

/////////////////////////////////////// /////////////////////////////////////////

// The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc

static uint8_t event_id;

// Used to manage the timimng of repeating events

static uint32_t event_timer;

// How long to delay the next firing of event in millis

static uint16_t event_delay;

// how many times to fire : 0 = forever, 1 = do once, 2 = do twice

static int16_t event_repeat;

// per command value, such as PWM for servos

static int16_t event_value;

// the stored value used to undo commands - such as original PWM command

static int16_t event_undo_value;

/////////////////////////// ////////////////////////////////////////////////// ///

// Delay Mission Scripting Command

/////////////////////////////////////// /////////////////////////////////////////

static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)

static uint32_t condition_start;

/////////////// ////////////////////////////////////////////////// ///////////////

// IMU variables

///////////////////////////////////////////// ///////////////////////////////////

// Integration time (in seconds) for the gyros (DCM algorithm)

// Updated with the fast loop

static float G_Dt = 0.02;

/////////////////////////// ////////////////////////////////////////////////// ///

// Inertial Navigation

/ ////////////////////////////////////////////////// /////////////////////////////

static AP_InertialNav inertial_nav(&ahrs, &barometer, g_gps, gps_glitch);

/////// ////////////////////////////////////////////////// ///////////////////////

// Waypoint navigation object

// To-Do: move inertial nav up or other navigation variables down here

////////////////////////////////// //////////////////////////////////////////////

static AC_WPNav wp_nav(&inertial_nav, &ahrs, &_loiter_lat, &_loiter_lon, &_loiter_rate_lat,

&_loiter_rate_lon);

/////////////////////////// ////////////////////////////////////////////////// ///

// Performance monitoring

//////////////////////////////////////////////// ////////////////////////////////

// The number of GPS fixes we have had

static uint8_t gps_fix_count;

static int16_t pmTest1;

// System Timers

// --------------

// Time in microseconds of main control loop

static uint32_t fast_loopTimer;

// Counter of main loop executions. Used for performance monitoring and failsafe processing

static uint16_t mainLoop_count;

// Loiter timer - Records how long we have been in loiter

static uint32_t rtl_loiter_start_time;

// prevents duplicate GPS messages from entering system

static uint32_t last_gps_time;

// Used to exit the roll and pitch auto trim function

static uint8_t auto_trim_counter;

// Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7)

static AP_Relay relay;

//Reference to the camera object (it uses the relay object inside it)

#if CAMERA == ENABLED

static AP_Camera camera(&relay);

#endif

// a pin for reading the receiver RSSI voltage.

static AP_HAL::AnalogSource* rssi_analog_source;

// Input sources for battery voltage, battery current, board vcc

static AP_HAL::AnalogSource* board_vcc_analog_source;

#if CLI_ENABLED == ENABLED

static int8_t setup_show (uint8_t argc, const Menu::arg *argv);

#endif

// Camera/Antenna mount tracking and stabilisation stuff

// --------------------------------------

#if MOUNT == ENABLED

// current_loc uses the baro/gps soloution for altitude rather than gps only.

// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?

static AP_Mount camera_mount(¤t_loc, g_gps, ahrs, 0);

#endif

#if MOUNT2 == ENABLED

// current_loc uses the baro/gps soloution for altitude rather than gps only.

// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?

static AP_Mount camera_mount2(¤t_loc, g_gps, ahrs, 1);

#endif

///////// ////////////////////////////////////////////////// /////////////////////

// AC_Fence library to reduce fly-aways

/////////// ////////////////////////////////////////////////// ///////////////////

#if AC_FENCE == ENABLED

AC_Fence fence(&inertial_nav);

#endif

/////////////////////////// ////////////////////////////////////////////////// ///

// Crop Sprayer

//////// ////////////////////////////////////////////////// //////////////////////

#if SPRAYER == ENABLED

static AC_Sprayer sprayer(&inertial_nav);

#endif

/////////////////////////// ////////////////////////////////////////////////// ///

// function definitions to keep compiler from complaining about undeclared functions

///////////////////////////// ////////////////////////////////////////////////// /

void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate);

static void pre_arm_checks(bool display_failure);

/////////////////////////////////////// /////////////////////////////////////////

// Top-level logic

///////// ////////////////////////////////////////////////// /////////////////////

// setup the var_info table

AP_Param param_loader(var_info, WP_START_BYTE);

/*

scheduler table - all regular tasks apart from the fast_loop()

should be listed here, along with how often they should be called

(in 10ms units) and the maximum time they are expected to take (in

microseconds)

*/

static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {

{ throttle_loop, 2, 450 },

{ update_GPS, 2, 900 },

{ update_nav_mode, 1, 400 },

{ update_batt_compass, 10, 720 },

{ read_aux_switches, 10, 50 },

{ arm_motors_check, 10, 10 },

{ auto_trim, 10, 140 },

{ update_toy_throttle, 10, 50 },

{ update_altitude, 10, 1000 },

{ run_nav_updates, 10, 800 },

{ three_hz_loop, 33, 90 },

{ compass_accumulate, 2, 420 },

{ barometer_accumulate, 2, 250 },

{ update_notify, 2, 100 },

{ one_hz_loop, 100, 420 },

{ gcs_check_input,

2, 550 },

{ gcs_send_heartbeat, 100, 150 },

{ gcs_send_deferred, 2, 720 },

{ gcs_data_stream_send, 2, 950 },

#if COPTER_LEDS == ENABLED

{ update_copter_leds, 10, 55 },

#endif

{ update_mount, 2, 450 },

{ ten_hz_logging_loop, 10, 260 },

{ fifty_hz_logging_loop, 2, 220 },

{ perf_update, 1000, 200 },

{ read_receiver_rssi, 10, 50 },

#ifdef USERHOOK_FASTLOOP

{ userhook_FastLoop, 1, 100 },

#endif

#ifdef USERHOOK_50HZLOOP

{ userhook_50Hz, 2, 100 },

#endif

#ifdef USERHOOK_MEDIUMLOOP

{ userhook_MediumLoop, 10, 100 },

#endif

#ifdef USERHOOK_SLOWLOOP

{ userhook_SlowLoop, 30, 100 },

#endif

#ifdef USERHOOK_SUPERSLOWLOOP

{ userhook_SuperSlowLoop,100, 100 },

#endif

};

void setup() {

// this needs to be the first call, as it fills memory with

// sentinel values

memcheck_init();

cliSerial = e;

// Load the default values of variables listed in var_info[]s

AP_Param::setup_sketch_defaults();

// initialise notify system

();

// initialise battery monitor

();

#if CONFIG_SONAR == ENABLED

#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC

sonar_analog_source = new AP_ADC_AnalogSource(

&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);

#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN

sonar_analog_source = in->channel(

CONFIG_SONAR_SOURCE_ANALOG_PIN);

#else

#warning

#endif

sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,

&sonar_mode_filter);

#endif

rssi_analog_source = in->channel(_pin);

board_vcc_analog_source = in->channel(ANALOG_INPUT_BOARD_VCC);

init_ardupilot();

// initialise the main loop scheduler

(&scheduler_tasks[0], siz eof(scheduler_tasks)/sizeof(scheduler_tasks[0]));

}

/*

if the compass is enabled then try to accumulate a reading

*/

static void compass_accumulate(void)

{

if (s_enabled) {

late();

}

}

/*

try to accumulate a baro reading

*/

static void barometer_accumulate(void)

{

late();

}

static void perf_update(void)

{

if (_bitmask & MASK_LOG_PM)

Log_Write_Performance();

if (()) {

cliSerial->printf_P(PSTR(

(unsigned)perf_info_get_num_long_running(),

(unsigned)perf_info_get_num_loops(),

(unsigned long)perf_info_get_max_time());

}

perf_info_reset();

gps_fix_count = 0;

pmTest1 = 0;

}

void loop()

{

// wait for an INS sample

if (!_for_sample(1000)) {

Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY);

return;

}

uint32_t timer = micros();

// check loop time

perf_info_check_loop_time(timer - fast_loopTimer);

// used by PI Loops

G_Dt = (float)(timer - fast_loopTimer) / 1000000.f;

fast_loopTimer = timer;

// for mainloop failure monitoring

mainLoop_count++;

// Execute the fast loop

// ---------------------

fast_loop();

// tell the scheduler one tick has passed

();

// run all the tasks that are due to run. Note that we only

// have to call this once per loop, as the tasks are scheduled

// in multiples of the main loop tick. So if they don't run on

// the first call to the scheduler they won't run on a later

// call until () is called again

uint32_t time_available = (timer + 10000) - micros();

(time_available - 300);

}

// Main loop - 100hz

static void fast_loop()

{

// IMU DCM Algorithm

// --------------------

read_AHRS();

// reads all of the necessary trig functions for cameras, throttle, etc.

// ----------------- -------------------------------------------------- -

update_trig();

// Acrobatic control

if (_flip) {

if(abs(_l_in) < 4000) {

// calling roll_flip will override the desired roll rate and throttle output

roll_flip();

}else{

// force an exit from the loop if we are not hands off sticks.

_flip = false;

Log_Write_Event(DATA_EXIT_FLIP);

}

}

// run low level rate controllers that only require IMU data

run_rate_controllers();

// write out the servo PWM values

// ------------------------------

set_servos_4();

// Inertial Nav

// --------------------

read_inertia();

// optical flow

// --------------------

#if OPTFLOW == ENABLED

if(w_enabled) {

update_optical_flow();

}

#endif // OPTFLOW == ENABLED

// Read radio and 3-position switch on radio

// -----------------------------------------

read_radio();

read_control_switch();

// custom code/exceptions for flight modes

// ---------------------------------------

update_yaw_mode();

update_roll_pitch_mode();

// update targets to rate controllers

update_rate_contoller_targets();

}

// throttle_loop - should be run at 50 hz

// ---------------------------

static void throttle_loop()

{

// get altitude and climb rate from inertial lib

read_inertial_altitude();

// Update the throttle ouput

// -------------------------

update_throttle_mode();

// check if we've landed

update_land_detector();

#if TOY_EDF == ENABLED

edf_toy();

#endif

// check auto_armed status

update_auto_armed();

}

// update_mount - update camera mount position

// should be run at 50hz

static void update_mount()

{

#if MOUNT == ENABLED

// update camera mount's position

camera__mount_position();

#endif

#if MOUNT2 == ENABLED

// update camera mount's position

camera__mount_position();

#endif

#if CAMERA == ENABLED

r_pic_cleanup();

#endif

}

// update_batt_compass - read battery and compass

// should be called at 10hz

static void update_batt_compass(void)

{

// read battery before compass because it may be used for motor interference compensation

read_battery();

#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode

if(s_enabled) {

if (()) {

_offsets();

}

// log compass information

if (() && (_bitmask & MASK_LOG_COMPASS)) {

Log_Write_Compass();

}

}

#endif

// record throttle output

throttle_integrator += __out;

}

// ten_hz_logging_loop

// should be run at 10hz

static void ten_hz_logging_loop()

{

if(()) {

if (_bitmask & MASK_LOG_ATTITUDE_MED) {

Log_Write_Attitude();

}

if (_bitmask & MASK_LOG_MOTORS) {

Log_Write_Motors();

}

}

}

// fifty_hz_logging_loop

// should be run at 50hz

static void fifty_hz_logging_loop()

{

#if HIL_MODE != HIL_MODE_DISABLED

// HIL for a copter needs very fast update of the servo values

gcs_send_message(MSG_RADIO_OUT);

#endif

# if HIL_MODE == HIL_MODE_DISABLED

if (_bitmask & MASK_LOG_ATTITUDE_FAST && ()) {

Log_Write_Attitude();

}

if (_bitmask & MASK_LOG_IMU && ()) {

_Write_IMU(&ins);

}

#endif