APM飞控程序解读(3)

// 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_WP.alt – 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, &g.pi_loiter_lat, &g.pi_loiter_lon, &g.pid_loiter_rate_lat, &g.pid_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 = hal.console;

// Load the default values of variables listed in var_info[]s AP_Param::setup_sketch_defaults();

// initialise notify system notify.init();