zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Cleaned up and documented each global vavriable 

Added Wind compensation for Stability
Acro mode fixes
master
Jason Short 13 years ago
parent
2e83e42764
commit
76cfdca6cc
1 changed files with 482 additions and 249 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 711
      ArduCopter/ArduCopter.pde

711
ArduCopter/ArduCopter.pde
Unescape View File

@ -304,291 +304,508 @@ static const char* flight_mode_strings[] = { @@ -304,291 +304,508 @@ static const char* flight_mode_strings[] = {
8 TBD
*/
// temp
//Documentation of GLobals:
////////////////////////////////////////////////////////////////////////////////
// The GPS based velocity calculated by offsetting the Latitude and Longitude
// updated after GPS read - 5-10hz
static int16_t x_GPS_speed;
static int16_t y_GPS_speed;
// The synthesized velocity calculated by fancy filtering and fusion
// updated at 50hz
static int16_t x_actual_speed;
static int16_t y_actual_speed;
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t x_rate_error;
static int16_t y_rate_error;
////////////////////////////////////////////////////////////////////////////////
// Radio
// -----
////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
static byte control_mode = STABILIZE;
// This is the state of simple mode.
// Set in the control_mode.pde file when the control switch is read
static bool do_simple = false;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
static byte oldSwitchPosition;
// This is used to look for change in the control switch
static byte old_control_mode = STABILIZE;
static byte oldSwitchPosition; // for remembering the control mode switch
////////////////////////////////////////////////////////////////////////////////
// Motor Output
////////////////////////////////////////////////////////////////////////////////
// This is the array of PWM values being sent to the motors
static int16_t motor_out[11];
static int16_t motor_filtered[11]; // added to try and deal with biger motors
static bool do_simple = false;
// This is the array of PWM values being sent to the motors that has been lightly filtered with a simple LPF
// This was added to try and deal with biger motors
static int16_t motor_filtered[11];
////////////////////////////////////////////////////////////////////////////////
// Mavlink/HIL control
////////////////////////////////////////////////////////////////////////////////
// Used to track the GCS based control input
// Allow override of RC channel values for HIL
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
// Status flag that tracks whether we are under GCS control
static bool rc_override_active = false;
// Status flag that tracks whether we are under GCS control
static uint32_t rc_override_fs_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// Heli
// ----
////////////////////////////////////////////////////////////////////////////////
#if FRAME_CONFIG == HELI_FRAME
static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos
static int16_t heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly
static int32_t heli_servo_out[4]; // used for servo averaging for analog servos
static int16_t heli_servo_out_count = 0; // use for servo averaging
static int16_t heli_servo_out_count; // use for servo averaging
#endif
////////////////////////////////////////////////////////////////////////////////
// Failsafe
// --------
static boolean failsafe; // did our throttle dip below the failsafe value?
static boolean ch3_failsafe;
////////////////////////////////////////////////////////////////////////////////
// A status flag for the failsafe state
// did our throttle dip below the failsafe value?
static boolean failsafe;
// A status flag for arming the motors. This is the arming that is performed when the
// Yaw control is held right or left while throttle is low.
static boolean motor_armed;
static boolean motor_auto_armed; // if true,
// A status flag for whether or not we should allow AP to take over copter
// This is tied to the throttle. If the throttle = 0 or low/nuetral, then we do not allow
// the APM to take control of the copter.
static boolean motor_auto_armed;
////////////////////////////////////////////////////////////////////////////////
// PIDs
// ----
////////////////////////////////////////////////////////////////////////////////
// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates
// and not the adjusted omega rates, but the name is stuck
static Vector3f omega;
// This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value;
////////////////////////////////////////////////////////////////////////////////
// LED output
// ----------
static boolean motor_light; // status of the Motor safety
static boolean GPS_light; // status of the GPS light
////////////////////////////////////////////////////////////////////////////////
// status of LED based on the motor_armed variable
// Flashing indicates we are not armed
// Solid indicates Armed state
static boolean motor_light;
// Flashing indicates we are reading the GPS Strings
// Solid indicates we have full 3D lock and can navigate
static boolean GPS_light;
// This is current status for the LED lights state machine
// setting this value changes the output of the LEDs
static byte led_mode = NORMAL_LEDS;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
// -------------
static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
static float scaleLongUp = 1; // used to reverse longitude scaling
static float scaleLongDown = 1; // used to reverse longitude scaling
static byte ground_start_count = 10; // have we achieved first lock and set Home?
static bool did_ground_start = false; // have we ground started after first arming
////////////////////////////////////////////////////////////////////////////////
// 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
static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
static float scaleLongDown = 1;
// Location & Navigation
// ---------------------
static bool nav_ok;
////////////////////////////////////////////////////////////////////////////////
// Mavlink specific
////////////////////////////////////////////////////////////////////////////////
// Used by Mavlink for unknow reasons
static const float radius_of_earth = 6378100; // meters
// Used by Mavlink for unknow reasons
static const float gravity = 9.81; // meters/ sec^2
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t nav_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t home_bearing; // used to track difference in angle
static byte wp_control; // used to control - navgation or loiter
static byte command_nav_index; // current command memory location
static byte prev_nav_index;
static byte command_cond_index; // current command memory location
//static byte command_nav_ID; // current command ID
//static byte command_cond_ID; // current command ID
static byte wp_verify_byte; // used for tracking state of navigating waypoints
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// Status flag indicating we have data that can be used to navigate
// Set by a GPS read with 3D fix, or an optical flow read
static bool nav_ok;
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// This is the angle from the copter to the "home" location in degrees * 100
static int32_t home_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control;
// Register containing the index of the current navigation command in the mission script
static uint8_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 uint8_t wp_verify_byte; // used for tracking state of navigating waypoints
// used to limit the speed ramp up of WP navigation
// Acceleration is limited to .5m/s/s
static int16_t waypoint_speed_gov;
// Used to track how many cm we are from the "next_WP" location
static int32_t long_error, lat_error;
////////////////////////////////////////////////////////////////////////////////
// Orientation
////////////////////////////////////////////////////////////////////////////////
// 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;
static float cos_pitch_x = 1;
static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static int32_t initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static int8_t jump = -10; // used to track loops in jump command
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////
// 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 int32_t initial_simple_bearing;
////////////////////////////////////////////////////////////////////////////////
// Circle Mode / Loiter control
////////////////////////////////////////////////////////////////////////////////
// used to determin the desired location in Circle mode
// increments at circle_rate / second
static float circle_angle;
// replace with param
// used to control the speed of Circle mode
// units are in radians, default is 5° per second
static const float circle_rate = 0.0872664625;
// used to track the delat in Circle Mode
static int32_t old_target_bearing;
// deg : how many times to circle * 360 for Loiter/Circle Mission command
static int16_t loiter_total;
// deg : how far we have turned around a waypoint
static int16_t loiter_sum;
// How long we should stay in Loiter Mode for mission scripting
static uint16_t loiter_time_max;
// How long have we been loitering - The start time in millis
static uint32_t loiter_time;
// The synthetic location created to make the copter do circles around a WP
static struct Location circle_WP;
// Acro
////////////////////////////////////////////////////////////////////////////////
// CH7 control
////////////////////////////////////////////////////////////////////////////////
// Used to enable Jose's flip code
// when true the Roll/Pitch/Throttle control is sent to the flip state machine
#if CH7_OPTION == CH7_FLIP
static bool do_flip = false;
#endif
// Used to track the CH7 toggle state.
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
// This allows advanced functionality to know when to execute
static boolean trim_flag;
// This register tracks the current Mission Command index when writing
// a mission using CH7 in flight
static int8_t CH7_wp_index;
// Airspeed
// --------
static int16_t airspeed; // m/s * 100
static float thrust = .005; // for estimating the velocity
// Location Errors
// ---------------
static int32_t long_error, lat_error; // temp for debugging
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
// ---------------
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage = LOW_VOLTAGE * 1.05;
// Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total;
// Used to track if the battery is low - LED output flashes when the batt is low
static bool low_batt = false;
// Barometer Sensor variables
// --------------------------
static int32_t abs_pressure;
////////////////////////////////////////////////////////////////////////////////
// Altitude
////////////////////////////////////////////////////////////////////////////////
// The pressure at home location - calibrated at arming
static int32_t ground_pressure;
// The ground temperature at home location - calibrated at arming
static int16_t ground_temperature;
// Altitude Sensor variables
// ----------------------
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int32_t altitude_error; // meters * 100 we are off in altitude
static int16_t climb_rate; // m/s * 100
// 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 - 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;
// The previous altitude as reported by Sonar in cm for calculation of Climb Rate
static int16_t old_sonar_alt;
// The climb_rate as reported by sonar in cm/s
static int16_t sonar_rate;
// The altitude as reported by Baro in cm – Values can be quite high
static int32_t baro_alt;
// The previous altitude as reported by Baro in cm for calculation of Climb Rate
static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate;
// flight mode specific
// --------------------
////////////////////////////////////////////////////////////////////////////////
// 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 byte yaw_mode;
// The current desired control scheme for roll and pitch / navigation
static byte roll_pitch_mode;
// The current desired control scheme for altitude hold
static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls
static int32_t takeoff_timer; // time since we throttled up
static boolean land_complete;
static int32_t old_alt; // used for managing altitude rates
static int16_t velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
static int16_t manual_boost; // used in adjust altitude to make changing alt faster
static int16_t angle_boost; // used in adjust altitude to make changing alt faster
// Loiter management
// -----------------
static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP
static int32_t old_target_bearing; // used to track difference in angle
static int16_t loiter_total; // deg : how many times to loiter * 360
static int16_t loiter_sum; // deg : how far we have turned around a waypoint
static uint32_t loiter_time; // millis : when we started LOITER mode
static unsigned loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions
// ----------------------------------------------------
static int32_t nav_roll; // deg * 100 : target roll angle
static int32_t nav_pitch; // deg * 100 : target pitch angle
static int32_t nav_yaw; // deg * 100 : target yaw angle
static int32_t home_to_copter_bearing; // deg * 100 : target yaw angle
static int32_t auto_yaw; // deg * 100 : target yaw angle
static int16_t nav_lat; // for error calcs
static int16_t nav_lon; // for error calcs
static int16_t nav_lat_p; // for error calcs
static int16_t nav_lon_p; // for error calcs
static int16_t nav_throttle; // 0-1000 for throttle control
////////////////////////////////////////////////////////////////////////////////
// flight specific
////////////////////////////////////////////////////////////////////////////////
// Flag for monitoring the status of flight
// We must be in the air with throttle for 5 seconds before this flag is true
// This flag is reset when we are in a manual throttle mode with 0 throttle or disarmed
static boolean takeoff_complete;
// Used to record the most recent time since we enaged the throttle to take off
static int32_t takeoff_timer;
// Used to see if we have landed and if we should shut our engines - not fully implemented
static boolean land_complete = true;
// used to manually override throttle in interactive Alt hold modes
static int16_t manual_boost;
// An additional throttle added to keep the copter at the same altitude when banking
static int16_t angle_boost;
////////////////////////////////////////////////////////////////////////////////
// Navigation general
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read
static int32_t home_to_copter_bearing;
// distance between plane and home in meters (not cm!!!)
static int32_t home_distance;
// distance between plane and next_WP in meters (not cm!!!)
static int32_t wp_distance;
////////////////////////////////////////////////////////////////////////////////
// 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;
// Flag for if we have g_gps lock and have set the home location
static boolean home_is_set;
// Current location of the copter
static struct Location current_loc;
// Next WP is the desired location of the copter - the next waypoint or loiter location
static struct Location next_WP;
// Prev WP is used to get the optimum path from one WP to the next
static struct Location prev_WP;
// 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;
// Holds the current loaded command from the EEPROM for guided mode
static struct Location guided_WP;
////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life
////////////////////////////////////////////////////////////////////////////////
// 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 desired bank towards North (Positive) or South (Negative)
// Don't be fooled by the fact that Pitch is reversed from Roll in its sign!
static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon;
// This may go away, but for now I'm tracking the desired bank before we apply the Wind compensation I term
// This is mainly for debugging
static int16_t nav_lat_p;
static int16_t nav_lon_p;
////////////////////////////////////////////////////////////////////////////////
// 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;
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
// that is generated when the climb rate is within a certain threshold
static float throttle_avg = THROTTLE_CRUISE;
static bool invalid_throttle; // used to control when we calculate nav_throttle
//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
// This is a flag used to trigger the updating of nav_throttle at 10hz
static bool invalid_throttle;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
static int32_t command_yaw_start; // what angle were we to begin with
static uint32_t command_yaw_start_time; // when did we start turning
////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw;
// A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw;
// Used to manage the Yaw hold capabilities -
// Options include: no tracking, point at next wp, or at a target
static byte yaw_tracking = MAV_ROI_WPNEXT;
// In AP Mission scripting we have a fine level of control for Yaw
// This is our initial angle for relative Yaw movements
static int32_t command_yaw_start;
// Timer values used to control the speed of Yaw movements
static uint32_t command_yaw_start_time;
static uint16_t command_yaw_time; // how long we are turning
static int32_t command_yaw_end; // what angle are we trying to be
static int32_t command_yaw_delta; // how many degrees will we turn
static int16_t command_yaw_speed; // how fast to turn
// how many degrees will we turn
static int32_t command_yaw_delta;
// Deg/s we should turn
static int16_t command_yaw_speed;
// Direction we will turn – 1 = CW, 0 or -1 = CCW
static byte command_yaw_dir;
// Direction we will turn – 1 = relative, 0 = Absolute
static byte command_yaw_relative;
// Yaw will point at this location if yaw_tracking is set to MAV_ROI_LOCATION
static struct Location target_WP;
static int16_t auto_level_counter;
// Waypoints
// ---------
static int32_t home_distance; // meters - distance between plane and next waypoint
static int32_t wp_distance; // meters - distance between plane and next waypoint
static int32_t wp_totalDistance; // meters - distance between old and next waypoint
//static byte next_wp_index; // Current active command index
// repeating event control
// -----------------------
static byte event_id; // what to do - see defines
static uint32_t event_timer; // when the event was asked for in ms
static uint16_t event_delay; // how long to delay the next firing of event in millis
static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int16_t event_value; // per command value, such as PWM for servos
static int16_t event_undo_value; // the value used to undo commands
//static byte repeat_forever;
//static byte undo_event; // counter for timing the undo
// delay command
// --------------
////////////////////////////////////////////////////////////////////////////////
// Repeat Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
// The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc
static byte 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 int32_t condition_start;
// land command
// ------------
static int32_t land_start; // when we intiated command in millis()
static int32_t original_alt; // altitide reference for start of command
// 3D Location vectors
// -------------------
static struct Location home; // home location
static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint
static struct Location target_WP; // where do we want to you towards?
static struct Location command_nav_queue; // command preloaded
static struct Location command_cond_queue; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static int32_t target_altitude; // used for
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
////////////////////////////////////////////////////////////////////////////////
// Auto Landing
////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate
static int32_t land_start;
// The orginal altitude used to base our new altitude during decent
static int32_t original_alt;
Vector3f accels_rot;
// this is just me playing with the sensors
// the 2 code is not functioning and you should try 1 instead
#if ACCEL_ALT_HOLD == 2
static float Z_integrator;
static float Z_gain = 3;
static float Z_offset = 0;
#endif
////////////////////////////////////////////////////////////////////////////////
// IMU variables
// -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
////////////////////////////////////////////////////////////////////////////////
// Integration time for the gyros (DCM algorithm)
// Updated with th efast loop
static float G_Dt = 0.02;
// The rotated accelerometer values
// Used by Z accel control, updated at 10hz
Vector3f accels_rot;
////////////////////////////////////////////////////////////////////////////////
// Performance monitoring
// ----------------------
static int32_t perf_mon_timer;
//static float imu_health; // Metric based on accel gain deweighting
////////////////////////////////////////////////////////////////////////////////
// Used to manage the rate of performance logging messages
static int16_t perf_mon_counter;
// The number of GPS fixes we have had
static int16_t gps_fix_count;
// gps_watchdog check for bad reads and if we miss 12 in a row, we stop navigating
// by lowering nav_lat and navlon to 0 gradually
static byte gps_watchdog;
static int pmTest1;
// System Timers
// --------------
static uint32_t fast_loopTimer; // Time in miliseconds of main control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
// Time in microseconds of main control loop
static uint32_t fast_loopTimer;
// Time in microseconds of 50hz control loop
static uint32_t fiftyhz_loopTimer;
// Counters for branching from 10 hz control loop
static byte medium_loopCounter;
// Counters for branching from 3 1/3hz control loop
static byte slow_loopCounter;
// Counters for branching at 1 hz
static byte counter_one_herz;
// Stat machine counter for Simple Mode
static byte simple_counter;
// used to track the elapsed time between GPS reads
static uint32_t nav_loopTimer;
// Delta Time in milliseconds for navigation computations, updated with every good GPS read
static float dTnav;
// Counters for branching from 4 minute control loop used to save Compass offsets
static int16_t superslow_loopCounter;
static byte simple_timer; // for limiting the execution of flight mode thingys
static float dTnav; // Delta Time in milliseconds for navigation computations
static uint32_t nav_loopTimer; // used to track the elapsed ime for GPS nav
static byte counter_one_herz;
// 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;
// Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame;
// Used to auto exit the in-flight leveler
static int16_t auto_level_counter;
// Reference to the AP relay object - APM1 only
AP_Relay relay;
// APM2 only
#if USB_MUX_PIN > 0
static bool usb_connected;
#endif
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
@ -645,13 +862,13 @@ void loop() @@ -645,13 +862,13 @@ void loop()
super_slow_loop();
counter_one_herz = 0;
}
if (millis() - perf_mon_timer > 1200 /*20000*/) {
perf_mon_counter++;
if (perf_mon_counter > 600 ) {
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
gps_fix_count = 0;
perf_mon_timer = millis();
perf_mon_counter = 0;
}
//PORTK &= B10111111;
}
@ -1034,6 +1251,9 @@ static void update_optical_flow(void) @@ -1034,6 +1251,9 @@ static void update_optical_flow(void)
static void update_GPS(void)
{
// A counter that is used to grab at least 10 reads before commiting the Home location
static byte ground_start_count = 10;
g_gps->update();
update_GPS_light();
@ -1044,6 +1264,7 @@ static void update_GPS(void) @@ -1044,6 +1264,7 @@ static void update_GPS(void)
if(gps_watchdog < 12){
gps_watchdog++;
}else{
// after 12 reads we guess we may have lost GPS signal, stop navigating
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll >>= 1;
nav_pitch >>= 1;
@ -1145,6 +1366,9 @@ void update_yaw_mode(void) @@ -1145,6 +1366,9 @@ void update_yaw_mode(void)
void update_roll_pitch_mode(void)
{
int control_roll, control_pitch;
// hack to do auto_flip - need to remove, no one is using.
#if CH7_OPTION == CH7_FLIP
if (do_flip){
roll_flip();
@ -1152,45 +1376,34 @@ void update_roll_pitch_mode(void) @@ -1152,45 +1376,34 @@ void update_roll_pitch_mode(void)
}
#endif
int control_roll = 0, control_pitch = 0;
//read_radio();
if(do_simple && new_radio_frame){
new_radio_frame = false;
simple_timer++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_timer == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_timer > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_timer = 0;
}
// Rotate input by the initial bearing
control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
switch(roll_pitch_mode){
case ROLL_PITCH_ACRO:
// ACRO does not get SIMPLE mode ability
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
break;
case ROLL_PITCH_STABLE:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
#if WIND_COMP_STAB == 1
// in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
#else
// in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
#endif
break;
case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
@ -1198,6 +1411,38 @@ void update_roll_pitch_mode(void) @@ -1198,6 +1411,38 @@ void update_roll_pitch_mode(void)
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break;
}
// clear new radio frame info
new_radio_frame = false;
}
// new radio frame is used to make sure we only call this at 50hz
void update_simple_mode(void)
{
float simple_sin_y, simple_cos_x;
// used to manage state machine
// which improves speed of function
simple_counter++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_counter == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_counter > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_counter = 0;
}
// Rotate input by the initial bearing
int control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
int control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
#define THROTTLE_FILTER_SIZE 4
@ -1214,9 +1459,14 @@ void update_throttle_mode(void) @@ -1214,9 +1459,14 @@ void update_throttle_mode(void)
#if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in);
#else
if (control_mode == ACRO){
g.rc_3.servo_out = g.rc_3.control_in;
}else{
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
}
#endif
// calc average throttle
if ((g.rc_3.control_in > MINIMUM_THROTTLE)){
//throttle_avg = throttle_avg * .98 + rc_3.control_in * .02;
@ -1226,13 +1476,11 @@ void update_throttle_mode(void) @@ -1226,13 +1476,11 @@ void update_throttle_mode(void)
// Code to manage the Copter state
if ((millis() - takeoff_timer) > 5000){
// we must be in the air by now
// stop resetting rate_I()
takeoff_complete = true;
land_complete = false;
}else{
// reset these I terms to prevent flips on takeoff
g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I();
reset_rate_I();
}
}else{
// we are on the ground
@ -1247,10 +1495,7 @@ void update_throttle_mode(void) @@ -1247,10 +1495,7 @@ void update_throttle_mode(void)
// reset out i terms and takeoff timer
// -----------------------------------
g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I();
g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I();
reset_rate_I();
// remember our time since takeoff
// -------------------------------
@ -1422,9 +1667,8 @@ static void update_navigation() @@ -1422,9 +1667,8 @@ static void update_navigation()
break;
case STABILIZE:
//wp_control = NO_NAV_MODE;
//update_nav_wp();
wp_control = NO_NAV_MODE;
update_nav_wp();
break;
}
@ -1490,12 +1734,6 @@ static void update_trig(void){ @@ -1490,12 +1734,6 @@ static void update_trig(void){
// updated at 10hz
static void update_altitude()
{
altitude_sensor = BARO;
//current_loc.alt = g_gps->altitude - gps_base_alt;
//climb_rate = (g_gps->altitude - old_baro_alt) * 10;
//old_baro_alt = g_gps->altitude;
//baro_alt = g_gps->altitude;
#if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar
int fake_relative_alt = g_gps->altitude - gps_base_alt;
@ -1611,16 +1849,8 @@ static void tuning(){ @@ -1611,16 +1849,8 @@ static void tuning(){
switch(g.radio_tuning){
/*
case CH6_THRUST:
g.rc_6.set_range(0,1000); // 0 to 1
//Z_gain = tuning_value * 5;
thrust = tuning_value * .2;
break;*/
case CH6_DAMP:
g.rc_6.set_range(0,1500); // 0 to 1
//thrust = tuning_value * .2;
g.stablize_d.set(tuning_value);
break;
@ -1745,11 +1975,11 @@ static void update_nav_wp() @@ -1745,11 +1975,11 @@ static void update_nav_wp()
if (circle_angle > 6.28318531)
circle_angle -= 6.28318531;
target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
circle_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
circle_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
// calc the lat and long error to the target
calc_location_error(&target_WP);
calc_location_error(&circle_WP);
// use error as the desired rate towards the target
// nav_lon, nav_lat is calculated
@ -1759,25 +1989,31 @@ static void update_nav_wp() @@ -1759,25 +1989,31 @@ static void update_nav_wp()
// rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll();
// debug
//int angleTest = degrees(circle_angle);
//int nroll = nav_roll;
//int npitch = nav_pitch;
//Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
}else if(wp_control == WP_MODE){
int16_t speed = calc_desired_speed(g.waypoint_speed_max);
// use error as the desired rate towards the target
calc_nav_rate(g.waypoint_speed_max);
calc_nav_rate(speed);
// rotate pitch and roll to the copter frame of reference
//calc_nav_pitch_roll();
calc_loiter_pitch_roll();
}else if(wp_control == NO_NAV_MODE){
nav_roll = 0;
nav_pitch = 0;
// calc the Iterms for Loiter based on velicity
//calc_position_hold();
// calc the Iterms for Loiter based on velocity
//if ((x_actual_speed + y_actual_speed) == 0)
if (g_gps->ground_speed < 50)
calc_wind_compensation();
else
reduce_wind_compensation();
// rotate nav_lat, nav_lon to roll and pitch
calc_loiter_pitch_roll();
}
}
@ -1797,6 +2033,3 @@ static void update_auto_yaw() @@ -1797,6 +2033,3 @@ static void update_auto_yaw()
}
// MAV_ROI_NONE = basic Yaw hold
}

Write Preview
Loading…
Cancel
Save