|
|
|
@ -336,7 +336,7 @@ static bool rc_override_active = false;
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// A tracking variable for type of failsafe active
|
|
|
|
|
// Used for failsafe based on loss of RC signal or GCS signal
|
|
|
|
|
static int failsafe;
|
|
|
|
|
static int16_t failsafe;
|
|
|
|
|
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
|
|
|
|
|
// RC receiver should be set up to output a low throttle value when signal is lost
|
|
|
|
|
static bool ch3_failsafe;
|
|
|
|
@ -368,7 +368,7 @@ static byte ground_start_count = 5;
|
|
|
|
|
// Used to compute a speed estimate from the first valid gps fixes to decide if we are
|
|
|
|
|
// on the ground or in the air. Used to decide if a ground start is appropriate if we
|
|
|
|
|
// booted with an air start.
|
|
|
|
|
static int ground_start_avg;
|
|
|
|
|
static int16_t ground_start_avg;
|
|
|
|
|
// Tracks if GPS is enabled based on statup routine
|
|
|
|
|
// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
|
|
|
|
|
static bool GPS_enabled = false;
|
|
|
|
@ -381,18 +381,18 @@ const float radius_of_earth = 6378100; // meters
|
|
|
|
|
const float gravity = 9.81; // meters/ sec^2
|
|
|
|
|
// This is the currently calculated direction to fly.
|
|
|
|
|
// deg * 100 : 0 to 360
|
|
|
|
|
static long nav_bearing;
|
|
|
|
|
static int32_t nav_bearing;
|
|
|
|
|
// This is the direction to the next waypoint or loiter center
|
|
|
|
|
// deg * 100 : 0 to 360
|
|
|
|
|
static long target_bearing;
|
|
|
|
|
static int32_t target_bearing;
|
|
|
|
|
//This is the direction from the last waypoint to the next waypoint
|
|
|
|
|
// deg * 100 : 0 to 360
|
|
|
|
|
static long crosstrack_bearing;
|
|
|
|
|
static int32_t crosstrack_bearing;
|
|
|
|
|
// A gain scaler to account for ground speed/headwind/tailwind
|
|
|
|
|
static float nav_gain_scaler = 1;
|
|
|
|
|
// Direction held during phases of takeoff and landing
|
|
|
|
|
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
|
|
|
|
|
static long hold_course = -1; // deg * 100 dir of plane
|
|
|
|
|
static int32_t hold_course = -1; // deg * 100 dir of plane
|
|
|
|
|
|
|
|
|
|
// There may be two active commands in Auto mode.
|
|
|
|
|
// This indicates the active navigation command by index number
|
|
|
|
@ -407,37 +407,37 @@ static byte non_nav_command_ID = NO_COMMAND;
|
|
|
|
|
// Airspeed
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// The current airspeed estimate/measurement in centimeters per second
|
|
|
|
|
static int airspeed;
|
|
|
|
|
static int16_t airspeed;
|
|
|
|
|
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
|
|
|
|
|
// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
|
|
|
|
|
static long target_airspeed;
|
|
|
|
|
// Also used for flap deployment criteria. Centimeters per second.static int32_t target_airspeed;
|
|
|
|
|
static int32_t target_airspeed;
|
|
|
|
|
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
|
|
|
|
|
static float airspeed_error;
|
|
|
|
|
// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
|
|
|
|
|
// Used by the throttle controller
|
|
|
|
|
static long energy_error;
|
|
|
|
|
static int32_t energy_error;
|
|
|
|
|
// kinetic portion of energy error (m^2/s^2)
|
|
|
|
|
static long airspeed_energy_error;
|
|
|
|
|
static int32_t airspeed_energy_error;
|
|
|
|
|
// An amount that the airspeed should be increased in auto modes based on the user positioning the
|
|
|
|
|
// throttle stick in the top half of the range. Centimeters per second.
|
|
|
|
|
static int airspeed_nudge;
|
|
|
|
|
static int16_t airspeed_nudge;
|
|
|
|
|
// Similar to airspeed_nudge, but used when no airspeed sensor.
|
|
|
|
|
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
|
|
|
|
static int throttle_nudge = 0;
|
|
|
|
|
static int16_t throttle_nudge = 0;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Ground speed
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// The amount current ground speed is below min ground speed. Centimeters per second
|
|
|
|
|
static long groundspeed_undershoot = 0;
|
|
|
|
|
static int32_t groundspeed_undershoot = 0;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Location Errors
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Difference between current bearing and desired bearing. Hundredths of a degree
|
|
|
|
|
static long bearing_error;
|
|
|
|
|
static int32_t bearing_error;
|
|
|
|
|
// Difference between current altitude and desired altitude. Centimeters
|
|
|
|
|
static long altitude_error;
|
|
|
|
|
static int32_t altitude_error;
|
|
|
|
|
// Distance perpandicular to the course line that we are off trackline. Meters
|
|
|
|
|
static float crosstrack_error;
|
|
|
|
|
|
|
|
|
@ -468,7 +468,7 @@ static float airspeed_pressure;
|
|
|
|
|
// Altitude Sensor variables
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Altitude from the sonar sensor. Meters. Not yet implemented.
|
|
|
|
|
static int sonar_alt;
|
|
|
|
|
static int16_t sonar_alt;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// flight mode specific
|
|
|
|
@ -479,35 +479,35 @@ static bool takeoff_complete = true;
|
|
|
|
|
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
|
|
|
|
|
static bool land_complete;
|
|
|
|
|
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
|
|
|
|
|
static long takeoff_altitude;
|
|
|
|
|
static int32_t takeoff_altitude;
|
|
|
|
|
// Pitch to hold during landing command in the no airspeed sensor case. Hundredths of a degree
|
|
|
|
|
static int landing_pitch;
|
|
|
|
|
static int16_t landing_pitch;
|
|
|
|
|
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
|
|
|
|
|
static int takeoff_pitch;
|
|
|
|
|
static int16_t takeoff_pitch;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Loiter management
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
|
|
|
|
|
static long old_target_bearing;
|
|
|
|
|
static int32_t old_target_bearing;
|
|
|
|
|
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
|
|
|
|
|
static int loiter_total;
|
|
|
|
|
static int16_t loiter_total;
|
|
|
|
|
// The amount in degrees we have turned since recording old_target_bearing
|
|
|
|
|
static int loiter_delta;
|
|
|
|
|
static int16_t loiter_delta;
|
|
|
|
|
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
|
|
|
|
|
static int loiter_sum;
|
|
|
|
|
static int16_t loiter_sum;
|
|
|
|
|
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
|
|
|
|
|
static long loiter_time;
|
|
|
|
|
static int32_t loiter_time;
|
|
|
|
|
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
|
|
|
|
|
static int loiter_time_max;
|
|
|
|
|
static int16_t loiter_time_max;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Navigation control variables
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// The instantaneous desired bank angle. Hundredths of a degree
|
|
|
|
|
static long nav_roll;
|
|
|
|
|
static int32_t nav_roll;
|
|
|
|
|
// The instantaneous desired pitch angle. Hundredths of a degree
|
|
|
|
|
static long nav_pitch;
|
|
|
|
|
static int32_t nav_pitch;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Waypoint distances
|
|
|
|
@ -516,7 +516,7 @@ static long nav_pitch;
|
|
|
|
|
// is not static because AP_Camera uses it
|
|
|
|
|
long wp_distance;
|
|
|
|
|
// Distance between previous and next waypoint. Meters
|
|
|
|
|
static long wp_totalDistance;
|
|
|
|
|
static int32_t wp_totalDistance;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// repeating event control
|
|
|
|
@ -524,27 +524,27 @@ static long wp_totalDistance;
|
|
|
|
|
// Flag indicating current event type
|
|
|
|
|
static byte event_id;
|
|
|
|
|
// when the event was started in ms
|
|
|
|
|
static long event_timer;
|
|
|
|
|
static int32_t event_timer;
|
|
|
|
|
// how long to delay the next firing of event in millis
|
|
|
|
|
static uint16_t event_delay;
|
|
|
|
|
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
|
|
|
|
|
static int event_repeat = 0;
|
|
|
|
|
static int16_t event_repeat = 0;
|
|
|
|
|
// per command value, such as PWM for servos
|
|
|
|
|
static int event_value;
|
|
|
|
|
static int16_t event_value;
|
|
|
|
|
// the value used to cycle events (alternate value to event_value)
|
|
|
|
|
static int event_undo_value;
|
|
|
|
|
static int16_t event_undo_value;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Conditional command
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// A value used in condition commands (eg delay, change alt, etc.)
|
|
|
|
|
// For example in a change altitude command, it is the altitude to change to.
|
|
|
|
|
static long condition_value;
|
|
|
|
|
static int32_t condition_value;
|
|
|
|
|
// A starting value used to check the status of a conditional command.
|
|
|
|
|
// For example in a delay command the condition_start records that start time for the delay
|
|
|
|
|
static long condition_start;
|
|
|
|
|
static uint32_t condition_start;
|
|
|
|
|
// A value used in condition commands. For example the rate at which to change altitude.
|
|
|
|
|
static int condition_rate;
|
|
|
|
|
static int16_t condition_rate;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// 3D Location vectors
|
|
|
|
@ -571,9 +571,9 @@ static struct Location next_nonnav_command;
|
|
|
|
|
// Altitude / Climb rate control
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
|
|
|
|
|
static long target_altitude;
|
|
|
|
|
static int32_t target_altitude;
|
|
|
|
|
// Altitude difference between previous and current waypoint. Centimeters
|
|
|
|
|
static long offset_altitude;
|
|
|
|
|
static int32_t offset_altitude;
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// IMU variables
|
|
|
|
@ -586,30 +586,30 @@ static float G_Dt = 0.02;
|
|
|
|
|
// Performance monitoring
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Timer used to accrue data and trigger recording of the performanc monitoring log message
|
|
|
|
|
static long perf_mon_timer;
|
|
|
|
|
static int32_t perf_mon_timer;
|
|
|
|
|
// The maximum main loop execution time recorded in the current performance monitoring interval
|
|
|
|
|
static int G_Dt_max = 0;
|
|
|
|
|
// The number of gps fixes recorded in the current performance monitoring interval
|
|
|
|
|
static int gps_fix_count = 0;
|
|
|
|
|
static int16_t gps_fix_count = 0;
|
|
|
|
|
// A variable used by developers to track performanc metrics.
|
|
|
|
|
// Currently used to record the number of GCS heartbeat messages received
|
|
|
|
|
static int pmTest1 = 0;
|
|
|
|
|
static int16_t pmTest1 = 0;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// System Timers
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
// Time in miliseconds of start of main control loop. Milliseconds
|
|
|
|
|
static unsigned long fast_loopTimer;
|
|
|
|
|
static uint32_t fast_loopTimer;
|
|
|
|
|
// Time Stamp when fast loop was complete. Milliseconds
|
|
|
|
|
static unsigned long fast_loopTimeStamp;
|
|
|
|
|
static uint32_t fast_loopTimeStamp;
|
|
|
|
|
// Number of milliseconds used in last main loop cycle
|
|
|
|
|
static uint8_t delta_ms_fast_loop;
|
|
|
|
|
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
|
|
|
|
static uint16_t mainLoop_count;
|
|
|
|
|
|
|
|
|
|
// Time in miliseconds of start of medium control loop. Milliseconds
|
|
|
|
|
static unsigned long medium_loopTimer;
|
|
|
|
|
static uint32_t medium_loopTimer;
|
|
|
|
|
// Counters for branching from main control loop to slower loops
|
|
|
|
|
static byte medium_loopCounter;
|
|
|
|
|
// Number of milliseconds used in last medium loop cycle
|
|
|
|
@ -623,9 +623,9 @@ static byte superslow_loopCounter;
|
|
|
|
|
static byte counter_one_herz;
|
|
|
|
|
|
|
|
|
|
// used to track the elapsed time for navigation PID integral terms
|
|
|
|
|
static unsigned long nav_loopTimer;
|
|
|
|
|
static uint32_t nav_loopTimer;
|
|
|
|
|
// Elapsed time since last call to navigation pid functions
|
|
|
|
|
static unsigned long dTnav;
|
|
|
|
|
static uint32_t dTnav;
|
|
|
|
|
// % MCU cycles used
|
|
|
|
|
static float load;
|
|
|
|
|
|
|
|
|
@ -734,7 +734,7 @@ static void fast_loop()
|
|
|
|
|
|
|
|
|
|
# if HIL_MODE == HIL_MODE_DISABLED
|
|
|
|
|
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
|
|
|
|
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
|
|
|
|
|
Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor);
|
|
|
|
|
|
|
|
|
|
if (g.log_bitmask & MASK_LOG_RAW)
|
|
|
|
|
Log_Write_Raw();
|
|
|
|
@ -852,7 +852,7 @@ Serial.println(tempaccel.z, DEC);
|
|
|
|
|
|
|
|
|
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
|
|
|
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
|
|
|
|
|
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor);
|
|
|
|
|
Log_Write_Attitude(ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor);
|
|
|
|
|
|
|
|
|
|
if (g.log_bitmask & MASK_LOG_CTUN)
|
|
|
|
|
Log_Write_Control_Tuning();
|
|
|
|
|