diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index 8aef200788..3dd2dc551c 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -1,7 +1,9 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // set_home_state - update home state -void set_home_state(enum HomeState new_home_state) +void Copter::set_home_state(enum HomeState new_home_state) { // if no change, exit immediately if (ap.home_state == new_home_state) @@ -17,13 +19,13 @@ void set_home_state(enum HomeState new_home_state) } // home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin) -bool home_is_set() +bool Copter::home_is_set() { return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED); } // --------------------------------------------- -void set_auto_armed(bool b) +void Copter::set_auto_armed(bool b) { // if no change, exit immediately if( ap.auto_armed == b ) @@ -36,7 +38,7 @@ void set_auto_armed(bool b) } // --------------------------------------------- -void set_simple_mode(uint8_t b) +void Copter::set_simple_mode(uint8_t b) { if(ap.simple_mode != b){ if(b == 0){ @@ -53,7 +55,7 @@ void set_simple_mode(uint8_t b) } // --------------------------------------------- -static void set_failsafe_radio(bool b) +void Copter::set_failsafe_radio(bool b) { // only act on changes // ------------------- @@ -80,20 +82,20 @@ static void set_failsafe_radio(bool b) // --------------------------------------------- -void set_failsafe_battery(bool b) +void Copter::set_failsafe_battery(bool b) { failsafe.battery = b; AP_Notify::flags.failsafe_battery = b; } // --------------------------------------------- -static void set_failsafe_gcs(bool b) +void Copter::set_failsafe_gcs(bool b) { failsafe.gcs = b; } // --------------------------------------------- -void set_land_complete(bool b) +void Copter::set_land_complete(bool b) { // if no change, exit immediately if( ap.land_complete == b ) @@ -110,7 +112,7 @@ void set_land_complete(bool b) // --------------------------------------------- // set land complete maybe flag -void set_land_complete_maybe(bool b) +void Copter::set_land_complete_maybe(bool b) { // if no change, exit immediately if (ap.land_complete_maybe == b) @@ -124,7 +126,7 @@ void set_land_complete_maybe(bool b) // --------------------------------------------- -void set_pre_arm_check(bool b) +void Copter::set_pre_arm_check(bool b) { if(ap.pre_arm_check != b) { ap.pre_arm_check = b; @@ -132,21 +134,21 @@ void set_pre_arm_check(bool b) } } -void set_pre_arm_rc_check(bool b) +void Copter::set_pre_arm_rc_check(bool b) { if(ap.pre_arm_rc_check != b) { ap.pre_arm_rc_check = b; } } -void set_using_interlock(bool b) +void Copter::set_using_interlock(bool b) { if(ap.using_interlock != b) { ap.using_interlock = b; } } -void set_motor_emergency_stop(bool b) +void Copter::set_motor_emergency_stop(bool b) { if(ap.motor_emergency_stop != b) { ap.motor_emergency_stop = b; @@ -158,4 +160,4 @@ void set_motor_emergency_stop(bool b) } else { Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED); } -} \ No newline at end of file +} diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 487f24fb2c..a5b428dd6d 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -1,6 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "APM:Copter V3.4-dev" /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -74,637 +73,9 @@ * */ -//////////////////////////////////////////////////////////////////////////////// -// Header includes -//////////////////////////////////////////////////////////////////////////////// +#include "Copter.h" -#include -#include -#include - -// Common dependencies -#include -#include -#include -#include -#include -// AP_HAL -#include -#include -#include -#include -#include -#include -#include -#include - -// Application dependencies -#include -#include // MAVLink GCS definitions -#include // Serial manager library -#include // ArduPilot GPS library -#include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Analog to Digital Converter Library -#include -#include -#include // ArduPilot Mega Magnetometer Library -#include // ArduPilot Mega Vector/Matrix math Library -#include // Curve used to linearlise throttle pwm to thrust -#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library -#include -#include -#include // Mission command library -#include // Rally point library -#include // PID library -#include // PID library (2-axis) -#include // Heli specific Rate PID library -#include // P library -#include // Attitude control library -#include // Attitude control library for traditional helicopter -#include // Position control library -#include // RC Channel Library -#include // AP Motors library -#include // Range finder library -#include // Optical Flow library -#include // Filter library -#include // APM FIFO Buffer -#include // APM relay -#include -#include // Photo or video camera -#include // Camera/Antenna mount -#include // needed for AHRS build -#include // needed for AHRS build -#include // ArduPilot Mega inertial navigation library -#include // ArduCopter waypoint navigation library -#include // circle navigation library -#include // ArduPilot Mega Declination Helper Library -#include // Arducopter Fence library -#include // software in the loop support -#include // main loop scheduler -#include // RC input mapping library -#include // Notify library -#include // Battery monitor library -#include // board configuration library -#include -#if SPRAYER == ENABLED -#include // crop sprayer library -#endif -#if EPM_ENABLED == ENABLED -#include // EPM cargo gripper stuff -#endif -#if PARACHUTE == ENABLED -#include // Parachute release library -#endif -#include // Landing Gear library -#include - -// AP_HAL to Arduino compatibility layer -#include "compat.h" -// Configuration -#include "defines.h" -#include "config.h" -#include "config_channels.h" - -// key aircraft parameters passed to multiple libraries -static AP_Vehicle::MultiCopter aparm; - -// Local modules -#include "Parameters.h" - -// Heli modules -#include "heli.h" - -//////////////////////////////////////////////////////////////////////////////// -// cliSerial -//////////////////////////////////////////////////////////////////////////////// -// cliSerial isn't strictly necessary - it is an alias for hal.console. It may -// be deprecated in favor of hal.console in later releases. -static AP_HAL::BetterStream* cliSerial; - -// N.B. we need to keep a static declaration which isn't guarded by macros -// at the top to cooperate with the prototype mangler. - -//////////////////////////////////////////////////////////////////////////////// -// AP_HAL instance -//////////////////////////////////////////////////////////////////////////////// - -const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; - -//////////////////////////////////////////////////////////////////////////////// -// Parameters -//////////////////////////////////////////////////////////////////////////////// -// -// Global parameters are all contained within the 'g' class. -// -static Parameters g; - -// main loop scheduler -static AP_Scheduler scheduler; - -// AP_Notify instance -static AP_Notify notify; - -// used to detect MAVLink acks from GCS to stop compassmot -static uint8_t command_ack_counter; - -// has a log download started? -static bool in_log_download; - -// primary input control channels -static RC_Channel *channel_roll; -static RC_Channel *channel_pitch; -static RC_Channel *channel_throttle; -static RC_Channel *channel_yaw; - -//////////////////////////////////////////////////////////////////////////////// -// prototypes -//////////////////////////////////////////////////////////////////////////////// -static void update_events(void); -static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); -static void gcs_send_text_fmt(const prog_char_t *fmt, ...); - - -//////////////////////////////////////////////////////////////////////////////// -// Dataflash -//////////////////////////////////////////////////////////////////////////////// -#if defined(HAL_BOARD_LOG_DIRECTORY) -static DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY); -#else -static DataFlash_Empty DataFlash; -#endif - - -//////////////////////////////////////////////////////////////////////////////// -// the rate we run the main loop at -//////////////////////////////////////////////////////////////////////////////// -static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_400HZ; - -//////////////////////////////////////////////////////////////////////////////// -// Sensors -//////////////////////////////////////////////////////////////////////////////// -// -// There are three basic options related to flight sensor selection. -// -// - Normal flight mode. Real sensors are used. -// - HIL Attitude mode. Most sensors are disabled, as the HIL -// protocol supplies attitude information directly. -// - HIL Sensors mode. Synthetic sensors are configured that -// supply data from the simulation. -// - -static AP_GPS gps; - -// flight modes convenience array -static AP_Int8 *flight_modes = &g.flight_mode1; - -static AP_Baro barometer; - -static Compass compass; - -AP_InertialSensor ins; - -//////////////////////////////////////////////////////////////////////////////// -// SONAR -#if CONFIG_SONAR == ENABLED -static RangeFinder sonar; -static bool sonar_enabled = true; // enable user switch for sonar -#endif - -// Inertial Navigation EKF -#if AP_AHRS_NAVEKF_AVAILABLE -AP_AHRS_NavEKF ahrs(ins, barometer, gps, sonar); -#else -AP_AHRS_DCM ahrs(ins, barometer, gps); -#endif - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -SITL sitl; -#endif - -// Mission library -// forward declaration to keep compiler happy -static bool start_command(const AP_Mission::Mission_Command& cmd); -static bool verify_command(const AP_Mission::Mission_Command& cmd); -static void exit_mission(); -AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission); - -//////////////////////////////////////////////////////////////////////////////// -// Optical flow sensor -//////////////////////////////////////////////////////////////////////////////// -#if OPTFLOW == ENABLED -static OpticalFlow optflow; -#endif - -// gnd speed limit required to observe optical flow sensor limits -static float ekfGndSpdLimit; -// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise -static float ekfNavVelGainScaler; - -//////////////////////////////////////////////////////////////////////////////// -// GCS selection -//////////////////////////////////////////////////////////////////////////////// -static AP_SerialManager serial_manager; -static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; -static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; - -//////////////////////////////////////////////////////////////////////////////// -// User variables -//////////////////////////////////////////////////////////////////////////////// -#ifdef USERHOOK_VARIABLES - #include USERHOOK_VARIABLES -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Global variables -//////////////////////////////////////////////////////////////////////////////// - -/* Radio values - * Channel assignments - * 1 Ailerons (rudder if no ailerons) - * 2 Elevator - * 3 Throttle - * 4 Rudder (if we have ailerons) - * 5 Mode - 3 position switch - * 6 User assignable - * 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second) - * 8 TBD - * Each Aux channel can be configured to have any of the available auxiliary functions assigned to it. - * See libraries/RC_Channel/RC_Channel_aux.h for more information - */ - -//Documentation of GLobals: -static union { - struct { - uint8_t unused1 : 1; // 0 - uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE - uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully - uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed - uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised - uint8_t logging_started : 1; // 6 // true if dataflash logging has started - uint8_t land_complete : 1; // 7 // true if we have detected a landing - uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio - uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection - uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update - uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration - uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test - uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) - uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock - uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS - uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only) - enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked) - uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use - uint8_t motor_emergency_stop: 1; // 21 // motor estop switch, shuts off motors when enabled - }; - uint32_t value; -} ap; - -//////////////////////////////////////////////////////////////////////////////// -// Radio -//////////////////////////////////////////////////////////////////////////////// -// This is the state of the flight control system -// There are multiple states defined such as STABILIZE, ACRO, -static int8_t control_mode = STABILIZE; -// Structure used to detect changes in the flight mode control switch -static struct { - int8_t debounced_switch_position; // currently used switch position - int8_t last_switch_position; // switch position in previous iteration - uint32_t last_edge_time_ms; // system time that switch position was last changed -} control_switch_state; - -static struct { - bool running; - float speed; - uint32_t start_ms; - uint32_t time_ms; -} takeoff_state; - -static RCMapper rcmap; - -// board specific config -static AP_BoardConfig BoardConfig; - -// receiver RSSI -static uint8_t receiver_rssi; - -//////////////////////////////////////////////////////////////////////////////// -// Failsafe -//////////////////////////////////////////////////////////////////////////////// -static struct { - uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station - uint8_t radio : 1; // 1 // A status flag for the radio failsafe - uint8_t battery : 1; // 2 // A status flag for the battery failsafe - uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe - uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred - - int8_t radio_counter; // number of iterations with throttle below throttle_fs_value - - uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe -} failsafe; - -//////////////////////////////////////////////////////////////////////////////// -// Motor Output -//////////////////////////////////////////////////////////////////////////////// -#if FRAME_CONFIG == QUAD_FRAME - #define MOTOR_CLASS AP_MotorsQuad -#elif FRAME_CONFIG == TRI_FRAME - #define MOTOR_CLASS AP_MotorsTri -#elif FRAME_CONFIG == HEXA_FRAME - #define MOTOR_CLASS AP_MotorsHexa -#elif FRAME_CONFIG == Y6_FRAME - #define MOTOR_CLASS AP_MotorsY6 -#elif FRAME_CONFIG == OCTA_FRAME - #define MOTOR_CLASS AP_MotorsOcta -#elif FRAME_CONFIG == OCTA_QUAD_FRAME - #define MOTOR_CLASS AP_MotorsOctaQuad -#elif FRAME_CONFIG == HELI_FRAME - #define MOTOR_CLASS AP_MotorsHeli -#elif FRAME_CONFIG == SINGLE_FRAME - #define MOTOR_CLASS AP_MotorsSingle -#elif FRAME_CONFIG == COAX_FRAME - #define MOTOR_CLASS AP_MotorsCoax -#else - #error Unrecognised frame type -#endif - -#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments -static MOTOR_CLASS motors(g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE); -#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing -static MOTOR_CLASS motors(MAIN_LOOP_RATE); -#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps -static MOTOR_CLASS motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE); -#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps -static MOTOR_CLASS motors(g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE); -#else -static MOTOR_CLASS motors(MAIN_LOOP_RATE); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// GPS variables -//////////////////////////////////////////////////////////////////////////////// -// 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 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; -static uint8_t land_state; // records state of land (flying to location, descending) - -//////////////////////////////////////////////////////////////////////////////// -// Auto -//////////////////////////////////////////////////////////////////////////////// -static AutoMode auto_mode; // controls which auto controller is run - -//////////////////////////////////////////////////////////////////////////////// -// Guided -//////////////////////////////////////////////////////////////////////////////// -static GuidedMode guided_mode; // controls which controller is run (pos or vel) - -//////////////////////////////////////////////////////////////////////////////// -// RTL -//////////////////////////////////////////////////////////////////////////////// -RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) -bool rtl_state_complete; // set to true if the current state is completed - -//////////////////////////////////////////////////////////////////////////////// -// Circle -//////////////////////////////////////////////////////////////////////////////// -bool circle_pilot_yaw_override; // true if pilot is overriding yaw - -//////////////////////////////////////////////////////////////////////////////// -// 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; - -//////////////////////////////////////////////////////////////////////////////// -// Throttle variables -//////////////////////////////////////////////////////////////////////////////// -static float throttle_average; // estimated throttle required to hover -static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only - - -//////////////////////////////////////////////////////////////////////////////// -// ACRO Mode -//////////////////////////////////////////////////////////////////////////////// -static float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot - -//////////////////////////////////////////////////////////////////////////////// -// Loiter control -//////////////////////////////////////////////////////////////////////////////// -static uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) -static uint32_t loiter_time; // How long have we been loitering - The start time in millis - -//////////////////////////////////////////////////////////////////////////////// -// Flip -//////////////////////////////////////////////////////////////////////////////// -static Vector3f flip_orig_attitude; // original copter attitude before flip - -//////////////////////////////////////////////////////////////////////////////// -// Battery Sensors -//////////////////////////////////////////////////////////////////////////////// -static AP_BattMonitor battery; - -//////////////////////////////////////////////////////////////////////////////// -// FrSky telemetry support -#if FRSKY_TELEM_ENABLED == ENABLED -static AP_Frsky_Telem frsky_telemetry(ahrs, battery); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Altitude -//////////////////////////////////////////////////////////////////////////////// -// 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 -static int32_t baro_alt; // barometer altitude in cm above home -static float baro_climbrate; // barometer climbrate in cm/s -static LowPassFilterVector3f land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF); // accelerations for land detector test - -//////////////////////////////////////////////////////////////////////////////// -// 3D Location vectors -//////////////////////////////////////////////////////////////////////////////// -// Current location of the copter (altitude is relative to home) -static struct Location current_loc; - - -//////////////////////////////////////////////////////////////////////////////// -// Navigation Yaw control -//////////////////////////////////////////////////////////////////////////////// -// auto flight mode's yaw mode -static uint8_t auto_yaw_mode = AUTO_YAW_LOOK_AT_NEXT_WP; -// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI -static Vector3f roi_WP; -// bearing from current location to the yaw_look_at_WP -static float 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; -// heading when in yaw_look_ahead_bearing -static float yaw_look_ahead_bearing; - - - -//////////////////////////////////////////////////////////////////////////////// -// 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_NavEKF inertial_nav(ahrs); - -//////////////////////////////////////////////////////////////////////////////// -// Attitude, Position and Waypoint navigation objects -// To-Do: move inertial nav up or other navigation variables down here -//////////////////////////////////////////////////////////////////////////////// -#if FRAME_CONFIG == HELI_FRAME -AC_AttitudeControl_Heli attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, - g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); -#else -AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, - g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); -#endif -AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control, - g.p_alt_hold, g.p_vel_z, g.pid_accel_z, - g.p_pos_xy, g.pi_vel_xy); -static AC_WPNav wp_nav(inertial_nav, ahrs, pos_control, attitude_control); -static AC_Circle circle_nav(inertial_nav, ahrs, pos_control); - -//////////////////////////////////////////////////////////////////////////////// -// Performance monitoring -//////////////////////////////////////////////////////////////////////////////// -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; - -// Used to exit the roll and pitch auto trim function -static uint8_t auto_trim_counter; - -// Reference to the relay object -static AP_Relay relay; - -// handle repeated servo and relay events -static AP_ServoRelayEvents ServoRelayEvents(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; - -#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. -static AP_Mount camera_mount(ahrs, current_loc); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// AC_Fence library to reduce fly-aways -//////////////////////////////////////////////////////////////////////////////// -#if AC_FENCE == ENABLED -AC_Fence fence(inertial_nav); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Rally library -//////////////////////////////////////////////////////////////////////////////// -#if AC_RALLY == ENABLED -AP_Rally rally(ahrs); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Crop Sprayer -//////////////////////////////////////////////////////////////////////////////// -#if SPRAYER == ENABLED -static AC_Sprayer sprayer(&inertial_nav); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// EPM Cargo Griper -//////////////////////////////////////////////////////////////////////////////// -#if EPM_ENABLED == ENABLED -static AP_EPM epm; -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Parachute release -//////////////////////////////////////////////////////////////////////////////// -#if PARACHUTE == ENABLED -static AP_Parachute parachute(relay); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Landing Gear Controller -//////////////////////////////////////////////////////////////////////////////// -static AP_LandingGear landinggear; - -//////////////////////////////////////////////////////////////////////////////// -// terrain handling -#if AP_TERRAIN_AVAILABLE -AP_Terrain terrain(ahrs, mission, rally); -#endif - -//////////////////////////////////////////////////////////////////////////////// -// function definitions to keep compiler from complaining about undeclared functions -//////////////////////////////////////////////////////////////////////////////// -static bool pre_arm_checks(bool display_failure); - -//////////////////////////////////////////////////////////////////////////////// -// Top-level logic -//////////////////////////////////////////////////////////////////////////////// - -// setup the var_info table -AP_Param param_loader(var_info); +#define SCHED_TASK(func) FUNCTOR_BIND_VOID(&copter, &Copter::func, void) /* scheduler table for fast CPUs - all regular tasks apart from the fast_loop() @@ -722,66 +93,67 @@ AP_Param param_loader(var_info); 4000 = 0.1hz */ -static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { - { rc_loop, 4, 130 }, // 0 - { throttle_loop, 8, 75 }, // 1 - { update_GPS, 8, 200 }, // 2 +const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = { + { SCHED_TASK(rc_loop), 4, 130 }, // 0 + { SCHED_TASK(throttle_loop), 8, 75 }, // 1 + { SCHED_TASK(update_GPS), 8, 200 }, // 2 #if OPTFLOW == ENABLED - { update_optical_flow, 2, 160 }, // 3 + { SCHED_TASK(update_optical_flow), 2, 160 }, // 3 #endif - { update_batt_compass, 40, 120 }, // 4 - { read_aux_switches, 40, 50 }, // 5 - { arm_motors_check, 40, 50 }, // 6 - { auto_trim, 40, 75 }, // 7 - { update_altitude, 40, 140 }, // 8 - { run_nav_updates, 8, 100 }, // 9 - { update_thr_average, 4, 90 }, // 10 - { three_hz_loop, 133, 75 }, // 11 - { compass_accumulate, 8, 100 }, // 12 - { barometer_accumulate, 8, 90 }, // 13 + { SCHED_TASK(update_batt_compass), 40, 120 }, // 4 + { SCHED_TASK(read_aux_switches), 40, 50 }, // 5 + { SCHED_TASK(arm_motors_check), 40, 50 }, // 6 + { SCHED_TASK(auto_trim), 40, 75 }, // 7 + { SCHED_TASK(update_altitude), 40, 140 }, // 8 + { SCHED_TASK(run_nav_updates), 8, 100 }, // 9 + { SCHED_TASK(update_thr_average), 4, 90 }, // 10 + { SCHED_TASK(three_hz_loop), 133, 75 }, // 11 + { SCHED_TASK(compass_accumulate), 8, 100 }, // 12 + { SCHED_TASK(barometer_accumulate), 8, 90 }, // 13 #if FRAME_CONFIG == HELI_FRAME - { check_dynamic_flight, 8, 75 }, + { SCHED_TASK(check_dynamic_flight), 8, 75 }, #endif - { update_notify, 8, 90 }, // 14 - { one_hz_loop, 400, 100 }, // 15 - { ekf_check, 40, 75 }, // 16 - { crash_check, 40, 75 }, // 17 - { landinggear_update, 40, 75 }, // 18 - { lost_vehicle_check, 40, 50 }, // 19 - { gcs_check_input, 1, 180 }, // 20 - { gcs_send_heartbeat, 400, 110 }, // 21 - { gcs_send_deferred, 8, 550 }, // 22 - { gcs_data_stream_send, 8, 550 }, // 23 - { update_mount, 8, 75 }, // 24 - { ten_hz_logging_loop, 40, 350 }, // 25 - { fifty_hz_logging_loop, 8, 110 }, // 26 - { full_rate_logging_loop,1, 100 }, // 27 - { perf_update, 4000, 75 }, // 28 - { read_receiver_rssi, 40, 75 }, // 29 + { SCHED_TASK(update_notify), 8, 90 }, // 14 + { SCHED_TASK(one_hz_loop), 400, 100 }, // 15 + { SCHED_TASK(ekf_check), 40, 75 }, // 16 + { SCHED_TASK(crash_check), 40, 75 }, // 17 + { SCHED_TASK(landinggear_update), 40, 75 }, // 18 + { SCHED_TASK(lost_vehicle_check), 40, 50 }, // 19 + { SCHED_TASK(gcs_check_input), 1, 180 }, // 20 + { SCHED_TASK(gcs_send_heartbeat), 400, 110 }, // 21 + { SCHED_TASK(gcs_send_deferred), 8, 550 }, // 22 + { SCHED_TASK(gcs_data_stream_send), 8, 550 }, // 23 + { SCHED_TASK(update_mount), 8, 75 }, // 24 + { SCHED_TASK(ten_hz_logging_loop), 40, 350 }, // 25 + { SCHED_TASK(fifty_hz_logging_loop), 8, 110 }, // 26 + { SCHED_TASK(full_rate_logging_loop),1, 100 }, // 27 + { SCHED_TASK(perf_update), 4000, 75 }, // 28 + { SCHED_TASK(read_receiver_rssi), 40, 75 }, // 29 #if FRSKY_TELEM_ENABLED == ENABLED - { frsky_telemetry_send, 80, 75 }, // 30 + { SCHED_TASK(frsky_telemetry_send), 80, 75 }, // 30 #endif #if EPM_ENABLED == ENABLED - { epm_update, 40, 75 }, // 31 + { SCHED_TASK(epm_update), 40, 75 }, // 31 #endif #ifdef USERHOOK_FASTLOOP - { userhook_FastLoop, 4, 75 }, + { SCHED_TASK(userhook_FastLoop), 4, 75 }, #endif #ifdef USERHOOK_50HZLOOP - { userhook_50Hz, 8, 75 }, + { SCHED_TASK(userhook_50Hz), 8, 75 }, #endif #ifdef USERHOOK_MEDIUMLOOP - { userhook_MediumLoop, 40, 75 }, + { SCHED_TASK(userhook_MediumLoop), 40, 75 }, #endif #ifdef USERHOOK_SLOWLOOP - { userhook_SlowLoop, 120, 75 }, + { SCHED_TASK(userhook_SlowLoop), 120, 75 }, #endif #ifdef USERHOOK_SUPERSLOWLOOP - { userhook_SuperSlowLoop,400, 75 }, + { SCHED_TASK(userhook_SuperSlowLoop),400, 75 }, #endif }; -void setup() + +void Copter::setup() { cliSerial = hal.console; @@ -804,7 +176,7 @@ void setup() /* if the compass is enabled then try to accumulate a reading */ -static void compass_accumulate(void) +void Copter::compass_accumulate(void) { if (g.compass_enabled) { compass.accumulate(); @@ -814,12 +186,12 @@ static void compass_accumulate(void) /* try to accumulate a baro reading */ -static void barometer_accumulate(void) +void Copter::barometer_accumulate(void) { barometer.accumulate(); } -static void perf_update(void) +void Copter::perf_update(void) { if (should_log(MASK_LOG_PM)) Log_Write_Performance(); @@ -834,7 +206,7 @@ static void perf_update(void) pmTest1 = 0; } -void loop() +void Copter::loop() { // wait for an INS sample ins.wait_for_sample(); @@ -869,7 +241,7 @@ void loop() // Main loop - 400hz -static void fast_loop() +void Copter::fast_loop() { // IMU DCM Algorithm @@ -902,7 +274,7 @@ static void fast_loop() // rc_loops - reads user input from transmitter/receiver // called at 100hz -static void rc_loop() +void Copter::rc_loop() { // Read radio and 3-position switch on radio // ----------------------------------------- @@ -912,7 +284,7 @@ static void rc_loop() // throttle_loop - should be run at 50 hz // --------------------------- -static void throttle_loop() +void Copter::throttle_loop() { // get altitude and climb rate from inertial lib read_inertial_altitude(); @@ -934,7 +306,7 @@ static void throttle_loop() // update_mount - update camera mount position // should be run at 50hz -static void update_mount() +void Copter::update_mount() { #if MOUNT == ENABLED // update camera mount's position @@ -948,7 +320,7 @@ static void update_mount() // update_batt_compass - read battery and compass // should be called at 10hz -static void update_batt_compass(void) +void Copter::update_batt_compass(void) { // read battery before compass because it may be used for motor interference compensation read_battery(); @@ -966,7 +338,7 @@ static void update_batt_compass(void) // ten_hz_logging_loop // should be run at 10hz -static void ten_hz_logging_loop() +void Copter::ten_hz_logging_loop() { // log attitude data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { @@ -995,7 +367,7 @@ static void ten_hz_logging_loop() // fifty_hz_logging_loop // should be run at 50hz -static void fifty_hz_logging_loop() +void Copter::fifty_hz_logging_loop() { #if HIL_MODE != HIL_MODE_DISABLED // HIL for a copter needs very fast update of the servo values @@ -1023,7 +395,7 @@ static void fifty_hz_logging_loop() // full_rate_logging_loop // should be run at the MAIN_LOOP_RATE -static void full_rate_logging_loop() +void Copter::full_rate_logging_loop() { if (should_log(MASK_LOG_IMU_FAST)) { DataFlash.Log_Write_IMU(ins); @@ -1031,7 +403,7 @@ static void full_rate_logging_loop() } // three_hz_loop - 3.3hz loop -static void three_hz_loop() +void Copter::three_hz_loop() { // check if we've lost contact with the ground station failsafe_gcs_check(); @@ -1052,7 +424,7 @@ static void three_hz_loop() } // one_hz_loop - runs at 1Hz -static void one_hz_loop() +void Copter::one_hz_loop() { if (should_log(MASK_LOG_ANY)) { Log_Write_Data(DATA_AP_STATE, ap.value); @@ -1108,7 +480,7 @@ static void one_hz_loop() } // called at 50hz -static void update_GPS(void) +void Copter::update_GPS(void) { static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message bool gps_updated = false; @@ -1145,8 +517,7 @@ static void update_GPS(void) } } -static void -init_simple_bearing() +void Copter::init_simple_bearing() { // capture current cos_yaw and sin_yaw values simple_cos_yaw = ahrs.cos_yaw(); @@ -1164,7 +535,7 @@ init_simple_bearing() } // update_simple_mode - rotates pilot input if we are in simple mode -void update_simple_mode(void) +void Copter::update_simple_mode(void) { float rollx, pitchx; @@ -1193,7 +564,7 @@ void update_simple_mode(void) // update_super_simple_bearing - adjusts simple bearing based on location // should be called after home_bearing has been updated -void update_super_simple_bearing(bool force_update) +void Copter::update_super_simple_bearing(bool force_update) { // check if we are in super simple mode and at least 10m from home if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS)) { @@ -1207,7 +578,7 @@ void update_super_simple_bearing(bool force_update) } } -static void read_AHRS(void) +void Copter::read_AHRS(void) { // Perform IMU calculations and get attitude info //----------------------------------------------- @@ -1220,7 +591,7 @@ static void read_AHRS(void) } // read baro and sonar altitude at 10hz -static void update_altitude() +void Copter::update_altitude() { // read in baro altitude read_barometer(); @@ -1234,5 +605,20 @@ static void update_altitude() } } +/* + compatibility with old pde style build + */ +void setup(void); +void loop(void); + +void setup(void) +{ + copter.setup(); +} +void loop(void) +{ + copter.loop(); +} + AP_HAL_MAIN(); diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index d8914e91b7..897dccb163 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -1,15 +1,17 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // get_smoothing_gain - returns smoothing gain to be passed into attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth // result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp -float get_smoothing_gain() +float Copter::get_smoothing_gain() { return (2.0f + (float)g.rc_feel_rp/10.0f); } // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees -static void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out) +void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out) { float angle_max = constrain_float(aparm.angle_max,1000,8000); float scaler = (float)angle_max/(float)ROLL_PITCH_INPUT_MAX; @@ -37,7 +39,7 @@ static void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float & // get_pilot_desired_heading - transform pilot's yaw input into a desired heading // returns desired angle in centi-degrees // To-Do: return heading as a float? -static float get_pilot_desired_yaw_rate(int16_t stick_angle) +float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) { // convert pilot input to the desired yaw rate return stick_angle * g.acro_yaw_p; @@ -49,7 +51,7 @@ static float get_pilot_desired_yaw_rate(int16_t stick_angle) // get_roi_yaw - returns heading towards location held in roi_WP // should be called at 100hz -static float get_roi_yaw() +float Copter::get_roi_yaw() { static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz @@ -62,7 +64,7 @@ static float get_roi_yaw() return yaw_look_at_WP_bearing; } -static float get_look_ahead_yaw() +float Copter::get_look_ahead_yaw() { const Vector3f& vel = inertial_nav.get_velocity(); float speed = pythagorous2(vel.x,vel.y); @@ -79,7 +81,7 @@ static float get_look_ahead_yaw() // update_thr_average - update estimated throttle required to hover (if necessary) // should be called at 100hz -static void update_thr_average() +void Copter::update_thr_average() { // ensure throttle_average has been initialised if( is_zero(throttle_average) ) { @@ -105,8 +107,7 @@ static void update_thr_average() } // set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared -static void -set_throttle_takeoff() +void Copter::set_throttle_takeoff() { // tell position controller to reset alt target and reset I terms pos_control.init_takeoff(); @@ -118,7 +119,7 @@ set_throttle_takeoff() // get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick // used only for manual throttle modes // returns throttle output 0 to 1000 -static int16_t get_pilot_desired_throttle(int16_t throttle_control) +int16_t Copter::get_pilot_desired_throttle(int16_t throttle_control) { int16_t throttle_out; @@ -146,7 +147,7 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control) // get_pilot_desired_climb_rate - transform pilot's throttle input to // climb rate in cm/s. we use radio_in instead of control_in to get the full range // without any deadzone at the bottom -static float get_pilot_desired_climb_rate(float throttle_control) +float Copter::get_pilot_desired_climb_rate(float throttle_control) { // throttle failsafe check if( failsafe.radio ) { @@ -183,12 +184,12 @@ static float get_pilot_desired_climb_rate(float throttle_control) } // get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff -static float get_non_takeoff_throttle() +float Copter::get_non_takeoff_throttle() { return (g.throttle_mid / 2.0f); } -static float get_takeoff_trigger_throttle() +float Copter::get_takeoff_trigger_throttle() { return channel_throttle->get_control_mid() + g.takeoff_trigger_dz; } @@ -196,7 +197,7 @@ static float get_takeoff_trigger_throttle() // get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off // used only for althold, loiter, hybrid flight modes // returns throttle output 0 to 1000 -static float get_throttle_pre_takeoff(float input_thr) +float Copter::get_throttle_pre_takeoff(float input_thr) { // exit immediately if input_thr is zero if (input_thr <= 0.0f) { @@ -229,7 +230,7 @@ static float get_throttle_pre_takeoff(float input_thr) // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller -static float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) +float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) { static uint32_t last_call_ms = 0; float distance_error; @@ -263,14 +264,14 @@ static float get_surface_tracking_climb_rate(int16_t target_rate, float current_ } // set_accel_throttle_I_from_pilot_throttle - smoothes transition from pilot controlled throttle to autopilot throttle -static void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle) +void Copter::set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle) { // shift difference between pilot's throttle and hover throttle into accelerometer I g.pid_accel_z.set_integrator(pilot_throttle-throttle_average); } // updates position controller's maximum altitude using fence and EKF limits -static void update_poscon_alt_max() +void Copter::update_poscon_alt_max() { float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp new file mode 100644 index 0000000000..345b464504 --- /dev/null +++ b/ArduCopter/Copter.cpp @@ -0,0 +1,96 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + constructor for main Copter class + */ + +const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; + +Copter::Copter(void) : +#if defined(HAL_BOARD_LOG_DIRECTORY) + DataFlash(HAL_BOARD_LOG_DIRECTORY), +#endif + ins_sample_rate(AP_InertialSensor::RATE_400HZ), + flight_modes(&g.flight_mode1), + sonar_enabled(true), + ahrs(ins, barometer, gps, sonar), + mission(ahrs, + FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &), + FUNCTOR_BIND_MEMBER(&Copter::verify_command, bool, const AP_Mission::Mission_Command &), + FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)), + control_mode(STABILIZE), +#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments + motors(g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE), +#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing + motors(MAIN_LOOP_RATE), +#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps + motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE), +#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps + motors(g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE), +#else + motors(MAIN_LOOP_RATE), +#endif + scaleLongDown(1), + simple_cos_yaw(1.0f), + super_simple_cos_yaw(1.0), +#if FRSKY_TELEM_ENABLED == ENABLED + frsky_telemetry(ahrs, battery), +#endif + land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF), + auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), + G_Dt(0.0025f), + inertial_nav(ahrs), +#if FRAME_CONFIG == HELI_FRAME + attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, + g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw), +#else + attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw, + g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw), +#endif + pos_control(ahrs, inertial_nav, motors, attitude_control, + g.p_alt_hold, g.p_vel_z, g.pid_accel_z, + g.p_pos_xy, g.pi_vel_xy), + wp_nav(inertial_nav, ahrs, pos_control, attitude_control), + circle_nav(inertial_nav, ahrs, pos_control), + ServoRelayEvents(relay), +#if CAMERA == ENABLED + camera(&relay), +#endif +#if MOUNT == ENABLED + camera_mount(ahrs, current_loc), +#endif +#if AC_FENCE == ENABLED + fence(inertial_nav), +#endif +#if AC_RALLY == ENABLED + rally(ahrs), +#endif +#if SPRAYER == ENABLED + sprayer(&inertial_nav), +#endif +#if PARACHUTE == ENABLED + parachute(relay), +#endif +#if AP_TERRAIN_AVAILABLE + terrain(ahrs, mission, rally), +#endif + param_loader(var_info) +{ +} + +Copter copter; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h new file mode 100644 index 0000000000..7c16e2cff9 --- /dev/null +++ b/ArduCopter/Copter.h @@ -0,0 +1,961 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define THISFIRMWARE "APM:Copter V3.4-dev" +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + This is the main Copter class + */ + +//////////////////////////////////////////////////////////////////////////////// +// Header includes +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +// Common dependencies +#include +#include +#include +#include +#include +// AP_HAL +#include +#include +#include +#include +#include +#include +#include +#include + +// Application dependencies +#include +#include // MAVLink GCS definitions +#include // Serial manager library +#include // ArduPilot GPS library +#include // ArduPilot Mega Flash Memory Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include +#include +#include // Mission command library +#include // Rally point library +#include // PID library +#include // PID library (2-axis) +#include // Heli specific Rate PID library +#include // P library +#include // Attitude control library +#include // Attitude control library for traditional helicopter +#include // Position control library +#include // RC Channel Library +#include // AP Motors library +#include // Range finder library +#include // Optical Flow library +#include // Filter library +#include // APM FIFO Buffer +#include // APM relay +#include +#include // Photo or video camera +#include // Camera/Antenna mount +#include // needed for AHRS build +#include // needed for AHRS build +#include // ArduPilot Mega inertial navigation library +#include // ArduCopter waypoint navigation library +#include // circle navigation library +#include // ArduPilot Mega Declination Helper Library +#include // Arducopter Fence library +#include // software in the loop support +#include // main loop scheduler +#include // RC input mapping library +#include // Notify library +#include // Battery monitor library +#include // board configuration library +#include +#if SPRAYER == ENABLED +#include // crop sprayer library +#endif +#if EPM_ENABLED == ENABLED +#include // EPM cargo gripper stuff +#endif +#if PARACHUTE == ENABLED +#include // Parachute release library +#endif +#include // Landing Gear library +#include + +// AP_HAL to Arduino compatibility layer +// Configuration +#include "defines.h" +#include "config.h" +#include "config_channels.h" + +// Local modules +#include "Parameters.h" + +// Heli modules +#include "heli.h" + +class Copter { + public: + friend class GCS_MAVLINK; + friend class Parameters; + + Copter(void); + void setup(); + void loop(); + +private: + + // key aircraft parameters passed to multiple libraries + AP_Vehicle::MultiCopter aparm; + + + // cliSerial isn't strictly necessary - it is an alias for hal.console. It may + // be deprecated in favor of hal.console in later releases. + AP_HAL::BetterStream* cliSerial; + + // Global parameters are all contained within the 'g' class. + Parameters g; + + // main loop scheduler + AP_Scheduler scheduler; + + // AP_Notify instance + AP_Notify notify; + + // used to detect MAVLink acks from GCS to stop compassmot + uint8_t command_ack_counter; + + // has a log download started? + bool in_log_download; + + // primary input control channels + RC_Channel *channel_roll; + RC_Channel *channel_pitch; + RC_Channel *channel_throttle; + RC_Channel *channel_yaw; + + // Dataflash +#if defined(HAL_BOARD_LOG_DIRECTORY) + DataFlash_File DataFlash; +#else + DataFlash_Empty DataFlash; +#endif + + // the rate we run the main loop at + const AP_InertialSensor::Sample_rate ins_sample_rate; + + AP_GPS gps; + + // flight modes convenience array + AP_Int8 *flight_modes; + + AP_Baro barometer; + Compass compass; + AP_InertialSensor ins; + +#if CONFIG_SONAR == ENABLED + RangeFinder sonar; + bool sonar_enabled; // enable user switch for sonar +#endif + + // Inertial Navigation EKF + AP_AHRS_NavEKF ahrs; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL sitl; +#endif + + // Mission library + AP_Mission mission; + + // Optical flow sensor +#if OPTFLOW == ENABLED + OpticalFlow optflow; +#endif + + // gnd speed limit required to observe optical flow sensor limits + float ekfGndSpdLimit; + + // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise + float ekfNavVelGainScaler; + + // GCS selection + AP_SerialManager serial_manager; + static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; + + GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; + + // User variables +#ifdef USERHOOK_VARIABLES +# include USERHOOK_VARIABLES +#endif + + // Documentation of GLobals: + union { + struct { + uint8_t unused1 : 1; // 0 + uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE + uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 6 // true if dataflash logging has started + uint8_t land_complete : 1; // 7 // true if we have detected a landing + uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio + uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection + uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test + uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock + uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS + uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only) + enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked) + uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use + uint8_t motor_emergency_stop: 1; // 21 // motor estop switch, shuts off motors when enabled + }; + uint32_t value; + } ap; + + // This is the state of the flight control system + // There are multiple states defined such as STABILIZE, ACRO, + int8_t control_mode; + + // Structure used to detect changes in the flight mode control switch + struct { + int8_t debounced_switch_position; // currently used switch position + int8_t last_switch_position; // switch position in previous iteration + uint32_t last_edge_time_ms; // system time that switch position was last changed + } control_switch_state; + + struct { + bool running; + float speed; + uint32_t start_ms; + uint32_t time_ms; + } takeoff_state; + + RCMapper rcmap; + + // board specific config + AP_BoardConfig BoardConfig; + + // receiver RSSI + uint8_t receiver_rssi; + + // Failsafe + struct { + uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station + uint8_t radio : 1; // 1 // A status flag for the radio failsafe + uint8_t battery : 1; // 2 // A status flag for the battery failsafe + uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe + uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred + + int8_t radio_counter; // number of iterations with throttle below throttle_fs_value + + uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + } failsafe; + + // Motor Output +#if FRAME_CONFIG == QUAD_FRAME + #define MOTOR_CLASS AP_MotorsQuad +#elif FRAME_CONFIG == TRI_FRAME + #define MOTOR_CLASS AP_MotorsTri +#elif FRAME_CONFIG == HEXA_FRAME + #define MOTOR_CLASS AP_MotorsHexa +#elif FRAME_CONFIG == Y6_FRAME + #define MOTOR_CLASS AP_MotorsY6 +#elif FRAME_CONFIG == OCTA_FRAME + #define MOTOR_CLASS AP_MotorsOcta +#elif FRAME_CONFIG == OCTA_QUAD_FRAME + #define MOTOR_CLASS AP_MotorsOctaQuad +#elif FRAME_CONFIG == HELI_FRAME + #define MOTOR_CLASS AP_MotorsHeli +#elif FRAME_CONFIG == SINGLE_FRAME + #define MOTOR_CLASS AP_MotorsSingle +#elif FRAME_CONFIG == COAX_FRAME + #define MOTOR_CLASS AP_MotorsCoax +#else + #error Unrecognised frame type +#endif + + MOTOR_CLASS motors; + + // GPS variables + // Sometimes we need to remove the scaling for distance calcs + float scaleLongDown; + + // Location & Navigation + int32_t wp_bearing; + // The location of home in relation to the copter in centi-degrees + int32_t home_bearing; + // distance between plane and home in cm + int32_t home_distance; + // distance between plane and next waypoint in cm. + uint32_t wp_distance; + uint8_t land_state; // records state of land (flying to location, descending) + + // Auto + AutoMode auto_mode; // controls which auto controller is run + + // Guided + GuidedMode guided_mode; // controls which controller is run (pos or vel) + + // RTL + RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) + bool rtl_state_complete; // set to true if the current state is completed + + // Circle + bool circle_pilot_yaw_override; // true if pilot is overriding yaw + + // 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. + float simple_cos_yaw; + float simple_sin_yaw; + int32_t super_simple_last_bearing; + float super_simple_cos_yaw; + float super_simple_sin_yaw; + + // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable + int32_t initial_armed_bearing; + + // Throttle variables + float throttle_average; // estimated throttle required to hover + int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only + + // ACRO Mode + float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot + + // Loiter control + uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) + uint32_t loiter_time; // How long have we been loitering - The start time in millis + + // Flip + Vector3f flip_orig_attitude; // original copter attitude before flip + + // Battery Sensors + AP_BattMonitor battery; + + // FrSky telemetry support +#if FRSKY_TELEM_ENABLED == ENABLED + AP_Frsky_Telem frsky_telemetry; +#endif + + // Altitude + // The cm/s we are moving up or down based on filtered data - Positive = UP + int16_t climb_rate; + // The altitude as reported by Sonar in cm - Values are 20 to 700 generally. + int16_t sonar_alt; + uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar + float target_sonar_alt; // desired altitude in cm above the ground + int32_t baro_alt; // barometer altitude in cm above home + float baro_climbrate; // barometer climbrate in cm/s + LowPassFilterVector3f land_accel_ef_filter; // accelerations for land detector test + + // 3D Location vectors + // Current location of the copter (altitude is relative to home) + struct Location current_loc; + + // Navigation Yaw control + // auto flight mode's yaw mode + uint8_t auto_yaw_mode; + + // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI + Vector3f roi_WP; + + // bearing from current location to the yaw_look_at_WP + float yaw_look_at_WP_bearing; + + // yaw used for YAW_LOOK_AT_HEADING yaw_mode + int32_t yaw_look_at_heading; + + // Deg/s we should turn + int16_t yaw_look_at_heading_slew; + + // heading when in yaw_look_ahead_bearing + float yaw_look_ahead_bearing; + + // Delay Mission Scripting Command + int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) + uint32_t condition_start; + + // IMU variables + // Integration time (in seconds) for the gyros (DCM algorithm) + // Updated with the fast loop + float G_Dt; + + // Inertial Navigation + AP_InertialNav_NavEKF inertial_nav; + + // Attitude, Position and Waypoint navigation objects + // To-Do: move inertial nav up or other navigation variables down here +#if FRAME_CONFIG == HELI_FRAME + AC_AttitudeControl_Heli attitude_control; +#else + AC_AttitudeControl attitude_control; +#endif + AC_PosControl pos_control; + AC_WPNav wp_nav; + AC_Circle circle_nav; + + // Performance monitoring + int16_t pmTest1; + + // System Timers + // -------------- + // Time in microseconds of main control loop + uint32_t fast_loopTimer; + // Counter of main loop executions. Used for performance monitoring and failsafe processing + uint16_t mainLoop_count; + // Loiter timer - Records how long we have been in loiter + uint32_t rtl_loiter_start_time; + + // Used to exit the roll and pitch auto trim function + uint8_t auto_trim_counter; + + // Reference to the relay object + AP_Relay relay; + + // handle repeated servo and relay events + AP_ServoRelayEvents ServoRelayEvents; + + // Reference to the camera object (it uses the relay object inside it) +#if CAMERA == ENABLED + AP_Camera camera; +#endif + + // a pin for reading the receiver RSSI voltage. + AP_HAL::AnalogSource* rssi_analog_source; + + // Camera/Antenna mount tracking and stabilisation stuff +#if MOUNT == ENABLED + // current_loc uses the baro/gps soloution for altitude rather than gps only. + AP_Mount camera_mount; +#endif + + // AC_Fence library to reduce fly-aways +#if AC_FENCE == ENABLED + AC_Fence fence; +#endif + + // Rally library +#if AC_RALLY == ENABLED + AP_Rally rally; +#endif + + // Crop Sprayer +#if SPRAYER == ENABLED + AC_Sprayer sprayer; +#endif + + // EPM Cargo Griper +#if EPM_ENABLED == ENABLED + AP_EPM epm; +#endif + + // Parachute release +#if PARACHUTE == ENABLED + AP_Parachute parachute; +#endif + + // Landing Gear Controller + AP_LandingGear landinggear; + + // terrain handling +#if AP_TERRAIN_AVAILABLE + AP_Terrain terrain; +#endif + + // use this to prevent recursion during sensor init + bool in_mavlink_delay; + + // true if we are out of time in our event timeslice + bool gcs_out_of_time; + + // Top-level logic + // setup the var_info table + AP_Param param_loader; + + static const AP_Scheduler::Task scheduler_tasks[]; + static const AP_Param::Info var_info[]; + static const struct LogStructure log_structure[]; + + void compass_accumulate(void); + void barometer_accumulate(void); + void perf_update(void); + void fast_loop(); + void rc_loop(); + void throttle_loop(); + void update_mount(); + void update_batt_compass(void); + void ten_hz_logging_loop(); + void fifty_hz_logging_loop(); + void full_rate_logging_loop(); + void three_hz_loop(); + void one_hz_loop(); + void update_GPS(void); + void init_simple_bearing(); + void update_simple_mode(void); + void update_super_simple_bearing(bool force_update); + void read_AHRS(void); + void update_altitude(); + void set_home_state(enum HomeState new_home_state); + bool home_is_set(); + void set_auto_armed(bool b); + void set_simple_mode(uint8_t b); + void set_failsafe_radio(bool b); + void set_failsafe_battery(bool b); + void set_failsafe_gcs(bool b); + void set_land_complete(bool b); + void set_land_complete_maybe(bool b); + void set_pre_arm_check(bool b); + void set_pre_arm_rc_check(bool b); + void set_using_interlock(bool b); + void set_motor_emergency_stop(bool b); + float get_smoothing_gain(); + void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out); + float get_pilot_desired_yaw_rate(int16_t stick_angle); + float get_roi_yaw(); + float get_look_ahead_yaw(); + void update_thr_average(); + void set_throttle_takeoff(); + int16_t get_pilot_desired_throttle(int16_t throttle_control); + float get_pilot_desired_climb_rate(float throttle_control); + float get_non_takeoff_throttle(); + float get_takeoff_trigger_throttle(); + float get_throttle_pre_takeoff(float input_thr); + float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); + void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle); + void update_poscon_alt_max(); + void gcs_send_heartbeat(void); + void gcs_send_deferred(void); + void send_heartbeat(mavlink_channel_t chan); + void send_attitude(mavlink_channel_t chan); + void send_limits_status(mavlink_channel_t chan); + void send_extended_status1(mavlink_channel_t chan); + void send_location(mavlink_channel_t chan); + void send_nav_controller_output(mavlink_channel_t chan); + void send_simstate(mavlink_channel_t chan); + void send_hwstatus(mavlink_channel_t chan); + void send_servo_out(mavlink_channel_t chan); + void send_radio_out(mavlink_channel_t chan); + void send_vfr_hud(mavlink_channel_t chan); + void send_current_waypoint(mavlink_channel_t chan); + void send_rangefinder(mavlink_channel_t chan); + void send_pid_tuning(mavlink_channel_t chan); + void send_statustext(mavlink_channel_t chan); + bool telemetry_delayed(mavlink_channel_t chan); + void gcs_send_message(enum ap_message id); + void gcs_data_stream_send(void); + void gcs_check_input(void); + void gcs_send_text_P(gcs_severity severity, const prog_char_t *str); + void do_erase_logs(void); + void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_target, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp); + void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); + void Log_Write_Current(); + void Log_Write_Optflow(); + void Log_Write_Nav_Tuning(); + void Log_Write_Control_Tuning(); + void Log_Write_Performance(); + void Log_Write_Attitude(); + void Log_Write_Rate(); + void Log_Write_MotBatt(); + void Log_Write_Startup(); + void Log_Write_EntireMission(); + void Log_Write_Event(uint8_t id); + void Log_Write_Data(uint8_t id, int32_t value); + void Log_Write_Data(uint8_t id, uint32_t value); + void Log_Write_Data(uint8_t id, int16_t value); + void Log_Write_Data(uint8_t id, uint16_t value); + void Log_Write_Data(uint8_t id, float value); + void Log_Write_Error(uint8_t sub_system, uint8_t error_code); + void Log_Write_Baro(void); + void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high); + void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); + void start_logging() ; + void load_parameters(void); + void userhook_init(); + void userhook_FastLoop(); + void userhook_50Hz(); + void userhook_MediumLoop(); + void userhook_SlowLoop(); + void userhook_SuperSlowLoop(); + void update_home_from_EKF(); + void set_home_to_current_location_inflight(); + bool set_home_to_current_location(); + bool set_home_to_current_location_and_lock(); + bool set_home_and_lock(const Location& loc); + bool set_home(const Location& loc); + bool far_from_EKF_origin(const Location& loc); + void set_system_time_from_GPS(); + void exit_mission(); + void do_RTL(void); + bool verify_takeoff(); + bool verify_land(); + bool verify_loiter_unlimited(); + bool verify_loiter_time(); + bool verify_RTL(); + bool verify_wait_delay(); + bool verify_change_alt(); + bool verify_within_distance(); + bool verify_yaw(); + void do_take_picture(); + void log_picture(); + uint8_t mavlink_compassmot(mavlink_channel_t chan); + void delay(uint32_t ms); + uint32_t millis(); + uint32_t micros(); + bool acro_init(bool ignore_checks); + void acro_run(); + void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); + bool althold_init(bool ignore_checks); + void althold_run(); + bool auto_init(bool ignore_checks); + void auto_run(); + void auto_takeoff_start(float final_alt_above_home); + void auto_takeoff_run(); + void auto_wp_start(const Vector3f& destination); + void auto_wp_run(); + void auto_spline_run(); + void auto_land_start(); + void auto_land_start(const Vector3f& destination); + void auto_land_run(); + void auto_rtl_start(); + void auto_rtl_run(); + void auto_circle_movetoedge_start(); + void auto_circle_start(); + void auto_circle_run(); + void auto_nav_guided_start(); + void auto_nav_guided_run(); + bool auto_loiter_start(); + void auto_loiter_run(); + uint8_t get_default_auto_yaw_mode(bool rtl); + void set_auto_yaw_mode(uint8_t yaw_mode); + void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); + void set_auto_yaw_roi(const Location &roi_location); + float get_auto_heading(void); + bool autotune_init(bool ignore_checks); + void autotune_stop(); + bool autotune_start(bool ignore_checks); + void autotune_run(); + void autotune_attitude_control(); + void autotune_backup_gains_and_initialise(); + void autotune_load_orig_gains(); + void autotune_load_tuned_gains(); + void autotune_load_intra_test_gains(); + void autotune_load_twitch_gains(); + void autotune_save_tuning_gains(); + void autotune_update_gcs(uint8_t message_id); + bool autotune_roll_enabled(); + bool autotune_pitch_enabled(); + bool autotune_yaw_enabled(); + void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max); + void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max); + void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max); + void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max); + void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); + bool brake_init(bool ignore_checks); + void brake_run(); + bool circle_init(bool ignore_checks); + void circle_run(); + bool drift_init(bool ignore_checks); + void drift_run(); + int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled); + bool flip_init(bool ignore_checks); + void flip_run(); + bool guided_init(bool ignore_checks); + void guided_takeoff_start(float final_alt_above_home); + void guided_pos_control_start(); + void guided_vel_control_start(); + void guided_posvel_control_start(); + void guided_set_destination(const Vector3f& destination); + void guided_set_velocity(const Vector3f& velocity); + void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + void guided_run(); + void guided_takeoff_run(); + void guided_pos_control_run(); + void guided_vel_control_run(); + void guided_posvel_control_run(); + void guided_limit_clear(); + void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); + void guided_limit_init_time_and_pos(); + bool guided_limit_check(); + bool land_init(bool ignore_checks); + void land_run(); + void land_gps_run(); + void land_nogps_run(); + float get_land_descent_speed(); + void land_do_not_use_GPS(); + void set_mode_land_with_pause(); + bool landing_with_GPS(); + bool loiter_init(bool ignore_checks); + void loiter_run(); + bool poshold_init(bool ignore_checks); + void poshold_run(); + void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); + int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); + void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); + void poshold_update_wind_comp_estimate(); + void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void poshold_roll_controller_to_pilot_override(); + void poshold_pitch_controller_to_pilot_override(); + + bool rtl_init(bool ignore_checks); + void rtl_run(); + void rtl_climb_start(); + void rtl_return_start(); + void rtl_climb_return_run(); + void rtl_loiterathome_start(); + void rtl_loiterathome_run(); + void rtl_descent_start(); + void rtl_descent_run(); + void rtl_land_start(); + void rtl_land_run(); + float get_RTL_alt(); + bool sport_init(bool ignore_checks); + void sport_run(); + bool stabilize_init(bool ignore_checks); + void stabilize_run(); + void crash_check(); + void parachute_check(); + void parachute_release(); + void parachute_manual_release(); + void ekf_check(); + bool ekf_over_threshold(); + void failsafe_ekf_event(); + void failsafe_ekf_off_event(void); + void esc_calibration_startup_check(); + void esc_calibration_passthrough(); + void esc_calibration_auto(); + void failsafe_radio_on_event(); + void failsafe_radio_off_event(); + void failsafe_battery_event(void); + void failsafe_gcs_check(); + void failsafe_gcs_off_event(void); + void set_mode_RTL_or_land_with_pause(); + void update_events(); + void failsafe_enable(); + void failsafe_disable(); + void fence_check(); + void fence_send_mavlink_status(mavlink_channel_t chan); + bool set_mode(uint8_t mode); + void update_flight_mode(); + void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode); + bool mode_requires_GPS(uint8_t mode); + bool mode_has_manual_throttle(uint8_t mode); + bool mode_allows_arming(uint8_t mode, bool arming_from_gcs); + void notify_flight_mode(uint8_t mode); + void heli_init(); + int16_t get_pilot_desired_collective(int16_t control_in); + void check_dynamic_flight(void); + void update_heli_control_dynamics(void); + void heli_update_landing_swash(); + void heli_update_rotor_speed_targets(); + void heli_radio_passthrough(); + bool heli_acro_init(bool ignore_checks); + void heli_acro_run(); + void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out); + bool heli_stabilize_init(bool ignore_checks); + void heli_stabilize_run(); + void read_inertia(); + void read_inertial_altitude(); + bool land_complete_maybe(); + void update_land_detector(); + void update_throttle_thr_mix(); + void landinggear_update(); + void update_notify(); + void motor_test_output(); + bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc); + uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec); + void motor_test_stop(); + void arm_motors_check(); + void auto_disarm_check(); + bool init_arm_motors(bool arming_from_gcs); + bool pre_arm_checks(bool display_failure); + void pre_arm_rc_checks(); + bool pre_arm_gps_checks(bool display_failure); + bool arm_checks(bool display_failure, bool arming_from_gcs); + void init_disarm_motors(); + void motors_output(); + void lost_vehicle_check(); + void run_nav_updates(void); + void calc_position(); + void calc_distance_and_bearing(); + void calc_wp_distance(); + void calc_wp_bearing(); + void calc_home_distance_and_bearing(); + void run_autopilot(); + void perf_info_reset(); + void perf_ignore_this_loop(); + void perf_info_check_loop_time(uint32_t time_in_micros); + uint16_t perf_info_get_num_loops(); + uint32_t perf_info_get_max_time(); + uint32_t perf_info_get_min_time(); + uint16_t perf_info_get_num_long_running(); + Vector3f pv_location_to_vector(const Location& loc); + Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec); + float pv_alt_above_origin(float alt_above_home_cm); + float pv_alt_above_home(float alt_above_origin_cm); + float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination); + float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination); + void default_dead_zones(); + void init_rc_in(); + void init_rc_out(); + void enable_motor_output(); + void read_radio(); + void set_throttle_and_failsafe(uint16_t throttle_pwm); + void set_throttle_zero_flag(int16_t throttle_control); + void init_barometer(bool full_calibration); + void read_barometer(void); + void init_sonar(void); + int16_t read_sonar(void); + void init_compass(); + void init_optflow(); + void update_optical_flow(void); + void read_battery(void); + void read_receiver_rssi(void); + void epm_update(); + void report_batt_monitor(); + void report_frame(); + void report_radio(); + void report_ins(); + void report_flight_modes(); + void report_optflow(); + void print_radio_values(); + void print_switch(uint8_t p, uint8_t m, bool b); + void print_accel_offsets_and_scaling(void); + void print_gyro_offsets(void); + void report_compass(); + void print_blanks(int16_t num); + void print_divider(void); + void print_enabled(bool b); + void report_version(); + void read_control_switch(); + bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check); + bool check_duplicate_auxsw(void); + void reset_control_switch(); + uint8_t read_3pos_switch(int16_t radio_in); + void read_aux_switches(); + void init_aux_switches(); + void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag); + void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag); + void save_trim(); + void auto_trim(); + void init_ardupilot(); + void startup_ground(bool force_gyro_cal); + bool position_ok(); + bool optflow_position_ok(); + void update_auto_armed(); + void check_usb_mux(void); + void frsky_telemetry_send(void); + bool should_log(uint32_t mask); + bool current_mode_has_user_takeoff(bool must_navigate); + bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate); + void takeoff_timer_start(float alt); + void takeoff_stop(); + void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); + void print_hit_enter(); + void tuning(); + void gcs_send_text_fmt(const prog_char_t *fmt, ...); + void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd); + bool start_command(const AP_Mission::Mission_Command& cmd); + bool verify_command(const AP_Mission::Mission_Command& cmd); + + bool do_guided(const AP_Mission::Mission_Command& cmd); + void do_takeoff(const AP_Mission::Mission_Command& cmd); + void do_nav_wp(const AP_Mission::Mission_Command& cmd); + void do_land(const AP_Mission::Mission_Command& cmd); + void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); + void do_circle(const AP_Mission::Mission_Command& cmd); + void do_loiter_time(const AP_Mission::Mission_Command& cmd); + void do_spline_wp(const AP_Mission::Mission_Command& cmd); +#if NAV_GUIDED == ENABLED + void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); + void do_guided_limits(const AP_Mission::Mission_Command& cmd); +#endif + void do_wait_delay(const AP_Mission::Mission_Command& cmd); + void do_within_distance(const AP_Mission::Mission_Command& cmd); + void do_change_alt(const AP_Mission::Mission_Command& cmd); + void do_yaw(const AP_Mission::Mission_Command& cmd); + void do_change_speed(const AP_Mission::Mission_Command& cmd); + void do_set_home(const AP_Mission::Mission_Command& cmd); + void do_roi(const AP_Mission::Mission_Command& cmd); + void do_mount_control(const AP_Mission::Mission_Command& cmd); +#if CAMERA == ENABLED + void do_digicam_configure(const AP_Mission::Mission_Command& cmd); + void do_digicam_control(const AP_Mission::Mission_Command& cmd); +#endif +#if PARACHUTE == ENABLED + void do_parachute(const AP_Mission::Mission_Command& cmd); +#endif +#if EPM_ENABLED == ENABLED + void do_gripper(const AP_Mission::Mission_Command& cmd); +#endif + bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); + bool verify_circle(const AP_Mission::Mission_Command& cmd); + bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); +#if NAV_GUIDED == ENABLED + bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); +#endif + void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); + + void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); + void log_init(void); + void run_cli(AP_HAL::UARTDriver *port); + +public: + void mavlink_delay_cb(); + void failsafe_check(); + int8_t dump_log(uint8_t argc, const Menu::arg *argv); + int8_t erase_logs(uint8_t argc, const Menu::arg *argv); + int8_t select_logs(uint8_t argc, const Menu::arg *argv); + bool print_log_menu(void); + + int8_t process_logs(uint8_t argc, const Menu::arg *argv); + int8_t main_menu_help(uint8_t, const Menu::arg*); + int8_t setup_mode(uint8_t argc, const Menu::arg *argv); + int8_t setup_factory(uint8_t argc, const Menu::arg *argv); + int8_t setup_set(uint8_t argc, const Menu::arg *argv); + int8_t setup_show(uint8_t argc, const Menu::arg *argv); + int8_t esc_calib(uint8_t argc, const Menu::arg *argv); + + int8_t test_mode(uint8_t argc, const Menu::arg *argv); + int8_t test_baro(uint8_t argc, const Menu::arg *argv); + int8_t test_compass(uint8_t argc, const Menu::arg *argv); + int8_t test_ins(uint8_t argc, const Menu::arg *argv); + int8_t test_optflow(uint8_t argc, const Menu::arg *argv); + int8_t test_relay(uint8_t argc, const Menu::arg *argv); + int8_t test_shell(uint8_t argc, const Menu::arg *argv); + int8_t test_sonar(uint8_t argc, const Menu::arg *argv); + + int8_t reboot_board(uint8_t argc, const Menu::arg *argv); +}; + +#define MENU_FUNC(func) FUNCTOR_BIND(&copter, &Copter::func, int8_t, uint8_t, const Menu::arg *) + +extern const AP_HAL::HAL& hal; +extern Copter copter; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 24b1681790..9a975ea8ef 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1,28 +1,20 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) -// forward declarations to make compiler happy -static bool do_guided(const AP_Mission::Mission_Command& cmd); - -// use this to prevent recursion during sensor init -static bool in_mavlink_delay; - -// true if we are out of time in our event timeslice -static bool gcs_out_of_time; - - // check if a message will fit in the payload space available #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) #define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false -static void gcs_send_heartbeat(void) +void Copter::gcs_send_heartbeat(void) { gcs_send_message(MSG_HEARTBEAT); } -static void gcs_send_deferred(void) +void Copter::gcs_send_deferred(void) { gcs_send_message(MSG_RETRY_DEFERRED); } @@ -37,7 +29,7 @@ static void gcs_send_deferred(void) * pattern below when adding any new messages */ -static NOINLINE void send_heartbeat(mavlink_channel_t chan) +NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE; @@ -113,7 +105,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) system_status); } -static NOINLINE void send_attitude(mavlink_channel_t chan) +NOINLINE void Copter::send_attitude(mavlink_channel_t chan) { const Vector3f &gyro = ins.get_gyro(); mavlink_msg_attitude_send( @@ -128,14 +120,14 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) } #if AC_FENCE == ENABLED -static NOINLINE void send_limits_status(mavlink_channel_t chan) +NOINLINE void Copter::send_limits_status(mavlink_channel_t chan) { fence_send_mavlink_status(chan); } #endif -static NOINLINE void send_extended_status1(mavlink_channel_t chan) +NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) { uint32_t control_sensors_present; uint32_t control_sensors_enabled; @@ -279,7 +271,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) } -static void NOINLINE send_location(mavlink_channel_t chan) +void NOINLINE Copter::send_location(mavlink_channel_t chan) { uint32_t fix_time; // if we have a GPS fix, take the time as the last fix time. That @@ -306,7 +298,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) ahrs.yaw_sensor); // compass heading in 1/100 degree } -static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) { const Vector3f &targets = attitude_control.angle_ef_targets(); mavlink_msg_nav_controller_output_send( @@ -322,14 +314,14 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) } // report simulator state -static void NOINLINE send_simstate(mavlink_channel_t chan) +void NOINLINE Copter::send_simstate(mavlink_channel_t chan) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL sitl.simstate_send(chan); #endif } -static void NOINLINE send_hwstatus(mavlink_channel_t chan) +void NOINLINE Copter::send_hwstatus(mavlink_channel_t chan) { mavlink_msg_hwstatus_send( chan, @@ -337,7 +329,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) hal.i2c->lockup_count()); } -static void NOINLINE send_servo_out(mavlink_channel_t chan) +void NOINLINE Copter::send_servo_out(mavlink_channel_t chan) { #if HIL_MODE != HIL_MODE_DISABLED // normalized values scaled to -10000 to 10000 @@ -375,7 +367,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) #endif // HIL_MODE } -static void NOINLINE send_radio_out(mavlink_channel_t chan) +void NOINLINE Copter::send_radio_out(mavlink_channel_t chan) { mavlink_msg_servo_output_raw_send( chan, @@ -391,7 +383,7 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) hal.rcout->read(7)); } -static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan) { mavlink_msg_vfr_hud_send( chan, @@ -403,13 +395,13 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) climb_rate / 100.0f); } -static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } #if CONFIG_SONAR == ENABLED -static void NOINLINE send_rangefinder(mavlink_channel_t chan) +void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan) { // exit immediately if sonar is disabled if (!sonar.has_data()) { @@ -426,7 +418,7 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan) /* send PID tuning message */ -static void send_pid_tuning(mavlink_channel_t chan) +void Copter::send_pid_tuning(mavlink_channel_t chan) { const Vector3f &gyro = ahrs.get_gyro(); if (g.gcs_pid_mask & 1) { @@ -484,7 +476,7 @@ static void send_pid_tuning(mavlink_channel_t chan) } -static void NOINLINE send_statustext(mavlink_channel_t chan) +void NOINLINE Copter::send_statustext(mavlink_channel_t chan) { mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; mavlink_msg_statustext_send( @@ -494,7 +486,7 @@ static void NOINLINE send_statustext(mavlink_channel_t chan) } // are we still delaying telemetry to try to avoid Xbee bricking? -static bool telemetry_delayed(mavlink_channel_t chan) +bool Copter::telemetry_delayed(mavlink_channel_t chan) { uint32_t tnow = millis() >> 10; if (tnow > (uint32_t)g.telem_delay) { @@ -515,7 +507,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) { uint16_t txspace = comm_get_txspace(chan); - if (telemetry_delayed(chan)) { + if (copter.telemetry_delayed(chan)) { return false; } @@ -523,8 +515,8 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) // if we don't have at least 250 micros remaining before the main loop // wants to fire then don't send a mavlink message. We want to // prioritise the main flight control loop over communications - if (scheduler.time_available_usec() < 250 && motors.armed()) { - gcs_out_of_time = true; + if (copter.scheduler.time_available_usec() < 250 && copter.motors.armed()) { + copter.gcs_out_of_time = true; return false; } #endif @@ -532,188 +524,188 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) switch(id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); - gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis(); - send_heartbeat(chan); + copter.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis(); + copter.send_heartbeat(chan); break; case MSG_EXTENDED_STATUS1: // send extended status only once vehicle has been initialised // to avoid unnecessary errors being reported to user - if (ap.initialised) { + if (copter.ap.initialised) { CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan); + copter.send_extended_status1(chan); CHECK_PAYLOAD_SIZE(POWER_STATUS); - gcs[chan-MAVLINK_COMM_0].send_power_status(); + copter.gcs[chan-MAVLINK_COMM_0].send_power_status(); } break; case MSG_EXTENDED_STATUS2: CHECK_PAYLOAD_SIZE(MEMINFO); - gcs[chan-MAVLINK_COMM_0].send_meminfo(); + copter.gcs[chan-MAVLINK_COMM_0].send_meminfo(); break; case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); - send_attitude(chan); + copter.send_attitude(chan); break; case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - send_location(chan); + copter.send_location(chan); break; case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); - send_local_position(ahrs); + send_local_position(copter.ahrs); break; case MSG_NAV_CONTROLLER_OUTPUT: CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - send_nav_controller_output(chan); + copter.send_nav_controller_output(chan); break; case MSG_GPS_RAW: - return gcs[chan-MAVLINK_COMM_0].send_gps_raw(gps); + return copter.gcs[chan-MAVLINK_COMM_0].send_gps_raw(copter.gps); case MSG_SYSTEM_TIME: CHECK_PAYLOAD_SIZE(SYSTEM_TIME); - gcs[chan-MAVLINK_COMM_0].send_system_time(gps); + copter.gcs[chan-MAVLINK_COMM_0].send_system_time(copter.gps); break; case MSG_SERVO_OUT: CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - send_servo_out(chan); + copter.send_servo_out(chan); break; case MSG_RADIO_IN: CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - gcs[chan-MAVLINK_COMM_0].send_radio_in(receiver_rssi); + copter.gcs[chan-MAVLINK_COMM_0].send_radio_in(copter.receiver_rssi); break; case MSG_RADIO_OUT: CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - send_radio_out(chan); + copter.send_radio_out(chan); break; case MSG_VFR_HUD: CHECK_PAYLOAD_SIZE(VFR_HUD); - send_vfr_hud(chan); + copter.send_vfr_hud(chan); break; case MSG_RAW_IMU1: CHECK_PAYLOAD_SIZE(RAW_IMU); - gcs[chan-MAVLINK_COMM_0].send_raw_imu(ins, compass); + copter.gcs[chan-MAVLINK_COMM_0].send_raw_imu(copter.ins, copter.compass); break; case MSG_RAW_IMU2: CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(barometer); + copter.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(copter.barometer); break; case MSG_RAW_IMU3: CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(ins, compass, barometer); + copter.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(copter.ins, copter.compass, copter.barometer); break; case MSG_CURRENT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_CURRENT); - send_current_waypoint(chan); + copter.send_current_waypoint(chan); break; case MSG_NEXT_PARAM: CHECK_PAYLOAD_SIZE(PARAM_VALUE); - gcs[chan-MAVLINK_COMM_0].queued_param_send(); + copter.gcs[chan-MAVLINK_COMM_0].queued_param_send(); break; case MSG_NEXT_WAYPOINT: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); - gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); + copter.gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); break; case MSG_RANGEFINDER: #if CONFIG_SONAR == ENABLED CHECK_PAYLOAD_SIZE(RANGEFINDER); - send_rangefinder(chan); + copter.send_rangefinder(chan); #endif break; case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); - terrain.send_request(chan); + copter.terrain.send_request(chan); #endif break; case MSG_CAMERA_FEEDBACK: #if CAMERA == ENABLED CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); - camera.send_feedback(chan, gps, ahrs, current_loc); + copter.camera.send_feedback(chan, copter.gps, copter.ahrs, copter.current_loc); #endif break; case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); - send_statustext(chan); + copter.send_statustext(chan); break; case MSG_LIMITS_STATUS: #if AC_FENCE == ENABLED CHECK_PAYLOAD_SIZE(LIMITS_STATUS); - send_limits_status(chan); + copter.send_limits_status(chan); #endif break; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); - gcs[chan-MAVLINK_COMM_0].send_ahrs(ahrs); + copter.gcs[chan-MAVLINK_COMM_0].send_ahrs(copter.ahrs); break; case MSG_SIMSTATE: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL CHECK_PAYLOAD_SIZE(SIMSTATE); - send_simstate(chan); + copter.send_simstate(chan); #endif #if AP_AHRS_NAVEKF_AVAILABLE CHECK_PAYLOAD_SIZE(AHRS2); - gcs[chan-MAVLINK_COMM_0].send_ahrs2(ahrs); + copter.gcs[chan-MAVLINK_COMM_0].send_ahrs2(copter.ahrs); #endif break; case MSG_HWSTATUS: CHECK_PAYLOAD_SIZE(HWSTATUS); - send_hwstatus(chan); + copter.send_hwstatus(chan); break; case MSG_MOUNT_STATUS: #if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(MOUNT_STATUS); - camera_mount.status_msg(chan); + copter.camera_mount.status_msg(chan); #endif // MOUNT == ENABLED break; case MSG_BATTERY2: CHECK_PAYLOAD_SIZE(BATTERY2); - gcs[chan-MAVLINK_COMM_0].send_battery2(battery); + copter.gcs[chan-MAVLINK_COMM_0].send_battery2(copter.battery); break; case MSG_OPTICAL_FLOW: #if OPTFLOW == ENABLED CHECK_PAYLOAD_SIZE(OPTICAL_FLOW); - gcs[chan-MAVLINK_COMM_0].send_opticalflow(ahrs, optflow); + copter.gcs[chan-MAVLINK_COMM_0].send_opticalflow(copter.ahrs, copter.optflow); #endif break; case MSG_GIMBAL_REPORT: #if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(GIMBAL_REPORT); - camera_mount.send_gimbal_report(chan); + copter.camera_mount.send_gimbal_report(chan); #endif break; case MSG_EKF_STATUS_REPORT: #if AP_AHRS_NAVEKF_AVAILABLE CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); - ahrs.get_NavEKF().send_status_report(chan); + copter.ahrs.get_NavEKF().send_status_report(chan); #endif break; @@ -724,7 +716,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); - send_pid_tuning(chan); + copter.send_pid_tuning(chan); break; case MSG_RETRY_DEFERRED: @@ -861,11 +853,11 @@ GCS_MAVLINK::data_stream_send(void) return; } - if (!in_mavlink_delay && !motors.armed()) { - handle_log_send(DataFlash); + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_send(copter.DataFlash); } - gcs_out_of_time = false; + copter.gcs_out_of_time = false; if (_queued_parameter != NULL) { if (streamRates[STREAM_PARAMS].get() <= 0) { @@ -878,9 +870,9 @@ GCS_MAVLINK::data_stream_send(void) return; } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; - if (in_mavlink_delay) { + if (copter.in_mavlink_delay) { // don't send any other stream types while in the delay callback return; } @@ -891,7 +883,7 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_RAW_IMU3); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTENDED_STATUS)) { send_message(MSG_EXTENDED_STATUS1); @@ -902,27 +894,27 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_LIMITS_STATUS); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_POSITION)) { send_message(MSG_LOCATION); send_message(MSG_LOCAL_POSITION); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_RAW_CONTROLLER)) { send_message(MSG_SERVO_OUT); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_RC_CHANNELS)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); @@ -930,13 +922,13 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_PID_TUNING); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA2)) { send_message(MSG_VFR_HUD); } - if (gcs_out_of_time) return; + if (copter.gcs_out_of_time) return; if (stream_trigger(STREAM_EXTRA3)) { send_message(MSG_AHRS); @@ -957,14 +949,14 @@ GCS_MAVLINK::data_stream_send(void) void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) { - do_guided(cmd); + copter.do_guided(cmd); } void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) { // add home alt if needed if (cmd.content.location.flags.relative_alt) { - cmd.content.location.alt += ahrs.get_home().alt; + cmd.content.location.alt += copter.ahrs.get_home().alt; } // To-Do: update target altitude for loiter or waypoint controller depending upon nav mode @@ -979,15 +971,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0 { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if(msg->sysid != g.sysid_my_gcs) break; - failsafe.last_heartbeat_ms = millis(); - pmTest1++; + if(msg->sysid != copter.g.sysid_my_gcs) break; + copter.failsafe.last_heartbeat_ms = hal.scheduler->millis(); + copter.pmTest1++; break; } case MAVLINK_MSG_ID_SET_MODE: // MAV ID: 11 { - handle_set_mode(msg, set_mode); + handle_set_mode(msg, FUNCTOR_BIND(&copter, &Copter::set_mode, bool, uint8_t)); break; } @@ -1012,21 +1004,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_SET: // 23 { - handle_param_set(msg, &DataFlash); + handle_param_set(msg, &copter.DataFlash); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38 { - handle_mission_write_partial_list(mission, msg); + handle_mission_write_partial_list(copter.mission, msg); break; } // GCS has sent us a command from GCS, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39 { - if (handle_mission_item(msg, mission)) { - Log_Write_EntireMission(); + if (handle_mission_item(msg, copter.mission)) { + copter.Log_Write_EntireMission(); } break; } @@ -1034,20 +1026,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // read an individual command from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40 { - handle_mission_request(mission, msg); + handle_mission_request(copter.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // MAV ID: 41 { - handle_mission_set_current(mission, msg); + handle_mission_set_current(copter.mission, msg); break; } // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: // MAV ID: 43 { - handle_mission_request_list(mission, msg); + handle_mission_request_list(copter.mission, msg); break; } @@ -1055,13 +1047,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message case MAVLINK_MSG_ID_MISSION_COUNT: // MAV ID: 44 { - handle_mission_count(mission, msg); + handle_mission_count(copter.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // MAV ID: 45 { - handle_mission_clear_all(mission, msg); + handle_mission_clear_all(copter.mission, msg); break; } @@ -1074,7 +1066,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_GIMBAL_REPORT: { #if MOUNT == ENABLED - handle_gimbal_report(camera_mount, msg); + handle_gimbal_report(copter.camera_mount, msg); #endif break; } @@ -1084,7 +1076,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); @@ -1100,9 +1092,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) hal.rcin->set_overrides(v, 8); // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation - failsafe.rc_override_active = true; + copter.failsafe.rc_override_active = true; // a RC override message is consiered to be a 'heartbeat' from the ground station for failsafe purposes - failsafe.last_heartbeat_ms = millis(); + copter.failsafe.last_heartbeat_ms = hal.scheduler->millis(); break; } @@ -1124,7 +1116,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float takeoff_alt = packet.param7 * 100; // Convert m to cm - if(do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { + if(copter.do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1134,19 +1126,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_NAV_LOITER_UNLIM: - if (set_mode(LOITER)) { + if (copter.set_mode(LOITER)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - if (set_mode(RTL)) { + if (copter.set_mode(RTL)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_NAV_LAND: - if (set_mode(LAND)) { + if (copter.set_mode(LAND)) { result = MAV_RESULT_ACCEPTED; } break; @@ -1159,7 +1151,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); + copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1172,7 +1164,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param3 : unused // param4 : unused if (packet.param2 > 0.0f) { - wp_nav.set_speed_xy(packet.param2 * 100.0f); + copter.wp_nav.set_speed_xy(packet.param2 * 100.0f); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1186,7 +1178,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param7 : altitude (absolute) result = MAV_RESULT_FAILED; // assume failure if(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) { - if (set_home_to_current_location_and_lock()) { + if (copter.set_home_to_current_location_and_lock()) { result = MAV_RESULT_ACCEPTED; } } else { @@ -1194,8 +1186,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); - if (!far_from_EKF_origin(new_home_loc)) { - if (set_home_and_lock(new_home_loc)) { + if (!copter.far_from_EKF_origin(new_home_loc)) { + if (copter.set_home_and_lock(new_home_loc)) { result = MAV_RESULT_ACCEPTED; } } @@ -1213,36 +1205,36 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); - set_auto_yaw_roi(roi_loc); + copter.set_auto_yaw_roi(roi_loc); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_MISSION_START: - if (motors.armed() && set_mode(AUTO)) { - set_auto_armed(true); + if (copter.motors.armed() && copter.set_mode(AUTO)) { + copter.set_auto_armed(true); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_PREFLIGHT_CALIBRATION: // exit immediately if armed - if (motors.armed()) { + if (copter.motors.armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1,1.0f)) { // gyro offset calibration - ins.init_gyro(); + copter.ins.init_gyro(); // reset ahrs gyro bias - if (ins.gyro_calibrated_ok_all()) { - ahrs.reset_gyro_drift(); + if (copter.ins.gyro_calibrated_ok_all()) { + copter.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3,1.0f)) { // fast barometer calibration - init_barometer(false); + copter.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4,1.0f)) { result = MAV_RESULT_UNSUPPORTED; @@ -1251,9 +1243,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float trim_roll, trim_pitch; // this blocks AP_InertialSensor_UserInteract_MAVLink interact(this); - if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { + if(copter.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine - ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + copter.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1261,28 +1253,28 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } else if (is_equal(packet.param5,2.0f)) { // accel trim float trim_roll, trim_pitch; - if(ins.calibrate_trim(trim_roll, trim_pitch)) { + if(copter.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine - ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); + copter.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param6,1.0f)) { // compassmot calibration - result = mavlink_compassmot(chan); + result = copter.mavlink_compassmot(chan); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: if (is_equal(packet.param1,2.0f)) { // save first compass's offsets - compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); + copter.compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } if (is_equal(packet.param1,5.0f)) { // save secondary compass's offsets - compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); + copter.compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; @@ -1290,11 +1282,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1,1.0f)) { // attempt to arm and return success or failure - if (init_arm_motors(true)) { + if (copter.init_arm_motors(true)) { result = MAV_RESULT_ACCEPTED; } - } else if (is_zero(packet.param1) && (mode_has_manual_throttle(control_mode) || ap.land_complete || is_equal(packet.param2,21196.0f))) { - init_disarm_motors(); + } else if (is_zero(packet.param1) && (copter.mode_has_manual_throttle(copter.control_mode) || copter.ap.land_complete || is_equal(packet.param2,21196.0f))) { + copter.init_disarm_motors(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_UNSUPPORTED; @@ -1302,25 +1294,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_SET_SERVO: - if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { + if (copter.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: - if (ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { + if (copter.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: - if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { + if (copter.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: - if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { + if (copter.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { result = MAV_RESULT_ACCEPTED; } break; @@ -1328,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { AP_Notify::events.firmware_update = 1; - update_notify(); + copter.update_notify(); hal.scheduler->delay(50); // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); @@ -1341,10 +1333,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; switch ((uint16_t)packet.param1) { case 0: - fence.enable(false); + copter.fence.enable(false); break; case 1: - fence.enable(true); + copter.fence.enable(true); break; default: result = MAV_RESULT_FAILED; @@ -1362,16 +1354,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; switch ((uint16_t)packet.param1) { case PARACHUTE_DISABLE: - parachute.enabled(false); - Log_Write_Event(DATA_PARACHUTE_DISABLED); + copter.parachute.enabled(false); + copter.Log_Write_Event(DATA_PARACHUTE_DISABLED); break; case PARACHUTE_ENABLE: - parachute.enabled(true); - Log_Write_Event(DATA_PARACHUTE_ENABLED); + copter.parachute.enabled(true); + copter.Log_Write_Event(DATA_PARACHUTE_ENABLED); break; case PARACHUTE_RELEASE: // treat as a manual release which performs some additional check of altitude - parachute_manual_release(); + copter.parachute_manual_release(); break; default: result = MAV_RESULT_FAILED; @@ -1384,23 +1376,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) // param4 : timeout (in seconds) - result = mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); + result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); break; #if EPM_ENABLED == ENABLED case MAV_CMD_DO_GRIPPER: // param1 : gripper number (ignored) // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum. - if(!epm.enabled()) { + if(!copter.epm.enabled()) { result = MAV_RESULT_FAILED; } else { result = MAV_RESULT_ACCEPTED; switch ((uint8_t)packet.param2) { case GRIPPER_ACTION_RELEASE: - epm.release(); + copter.epm.release(); break; case GRIPPER_ACTION_GRAB: - epm.grab(); + copter.epm.grab(); break; default: result = MAV_RESULT_FAILED; @@ -1412,7 +1404,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { - gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } break; @@ -1431,7 +1423,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_COMMAND_ACK: // MAV ID: 77 { - command_ack_counter++; + copter.command_ack_counter++; break; } @@ -1442,7 +1434,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((control_mode != GUIDED) && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { break; } @@ -1459,14 +1451,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (!pos_ignore && !vel_ignore && acc_ignore) { Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); - pos_ned.z = pv_alt_above_origin(pos_ned.z); - guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + pos_ned.z = copter.pv_alt_above_origin(pos_ned.z); + copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (pos_ignore && !vel_ignore && acc_ignore) { - guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { Vector3f pos_ned = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f); - pos_ned.z = pv_alt_above_origin(pos_ned.z); - guided_set_destination(pos_ned); + pos_ned.z = copter.pv_alt_above_origin(pos_ned.z); + copter.guided_set_destination(pos_ned); } else { result = MAV_RESULT_FAILED; } @@ -1481,7 +1473,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((control_mode != GUIDED) && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { break; } @@ -1521,15 +1513,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) loc.flags.terrain_alt = false; break; } - pos_ned = pv_location_to_vector(loc); + pos_ned = copter.pv_location_to_vector(loc); } if (!pos_ignore && !vel_ignore && acc_ignore) { - guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (pos_ignore && !vel_ignore && acc_ignore) { - guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { - guided_set_destination(pos_ned); + copter.guided_set_destination(pos_ned); } else { result = MAV_RESULT_FAILED; } @@ -1571,9 +1563,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ins.set_accel(0, accels); - barometer.setHIL(packet.alt*0.001f); - compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); - compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); + copter.barometer.setHIL(packet.alt*0.001f); + copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); + copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } @@ -1582,33 +1574,33 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 { - handle_radio_status(msg, DataFlash, should_log(MASK_LOG_PM)); + handle_radio_status(msg, copter.DataFlash, copter.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_ERASE: - in_log_download = true; + copter.in_log_download = true; // fallthru case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - if (!in_mavlink_delay && !motors.armed()) { - handle_log_message(msg, DataFlash); + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_message(msg, copter.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: - in_log_download = false; - if (!in_mavlink_delay && !motors.armed()) { - handle_log_message(msg, DataFlash); + copter.in_log_download = false; + if (!copter.in_mavlink_delay && !copter.motors.armed()) { + handle_log_message(msg, copter.DataFlash); } break; #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 case MAVLINK_MSG_ID_SERIAL_CONTROL: - handle_serial_control(msg, gps); + handle_serial_control(msg, copter.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: - handle_gps_inject(msg, gps); + handle_gps_inject(msg, copter.gps); result = MAV_RESULT_ACCEPTED; break; @@ -1619,25 +1611,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAVLINK_MSG_ID_DIGICAM_CONTROL: - camera.control_msg(msg); - log_picture(); + copter.camera.control_msg(msg); + copter.log_picture(); break; #endif // CAMERA == ENABLED #if MOUNT == ENABLED case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204 - camera_mount.configure_msg(msg); + copter.camera_mount.configure_msg(msg); break; case MAVLINK_MSG_ID_MOUNT_CONTROL: - camera_mount.control_msg(msg); + copter.camera_mount.control_msg(msg); break; #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE - terrain.handle_data(chan, msg); + copter.terrain.handle_data(chan, msg); #endif break; @@ -1647,13 +1639,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_rally_point_t packet; mavlink_msg_rally_point_decode(msg, &packet); - if (packet.idx >= rally.get_rally_total() || - packet.idx >= rally.get_rally_max()) { + if (packet.idx >= copter.rally.get_rally_total() || + packet.idx >= copter.rally.get_rally_max()) { send_text_P(SEVERITY_LOW,PSTR("bad rally point message ID")); break; } - if (packet.count != rally.get_rally_total()) { + if (packet.count != copter.rally.get_rally_total()) { send_text_P(SEVERITY_LOW,PSTR("bad rally point message count")); break; } @@ -1666,7 +1658,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rally_point.land_dir = packet.land_dir; rally_point.flags = packet.flags; - if (!rally.set_rally_point_with_index(packet.idx, rally_point)) { + if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) { send_text_P(SEVERITY_HIGH, PSTR("error setting rally point")); } @@ -1682,7 +1674,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.pde 2")); // #### TEMP - if (packet.idx > rally.get_rally_total()) { + if (packet.idx > copter.rally.get_rally_total()) { send_text_P(SEVERITY_LOW, PSTR("bad rally point index")); break; } @@ -1690,7 +1682,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //send_text_P(SEVERITY_HIGH, PSTR("## getting rally point in GCS_Mavlink.pde 3")); // #### TEMP RallyLocation rally_point; - if (!rally.get_rally_point_with_index(packet.idx, rally_point)) { + if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) { send_text_P(SEVERITY_LOW, PSTR("failed to set rally point")); break; } @@ -1699,7 +1691,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_rally_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, - rally.get_rally_total(), rally_point.lat, rally_point.lng, + copter.rally.get_rally_total(), rally_point.lat, rally_point.lng, rally_point.alt, rally_point.break_alt, rally_point.land_dir, rally_point.flags); @@ -1710,7 +1702,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // AC_RALLY == ENABLED case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break; case MAVLINK_MSG_ID_LED_CONTROL: @@ -1728,7 +1720,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) * MAVLink to process packets while waiting for the initialisation to * complete */ -static void mavlink_delay_cb() +void Copter::mavlink_delay_cb() { static uint32_t last_1hz, last_50hz, last_5s; if (!gcs[0].initialised || in_mavlink_delay) return; @@ -1760,7 +1752,7 @@ static void mavlink_delay_cb() /* * send a message on both GCS links */ -static void gcs_send_message(enum ap_message id) +void Copter::gcs_send_message(enum ap_message id) { for (uint8_t i=0; iprintf_P(PSTR("logs enabled: ")); @@ -60,38 +55,36 @@ print_log_menu(void) } #if CLI_ENABLED == ENABLED -static int8_t -dump_log(uint8_t argc, const Menu::arg *argv) +int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv) { - int16_t dump_log; + int16_t dump_log_num; uint16_t dump_log_start; uint16_t dump_log_end; uint16_t last_log_num; // check that the requested log number can be read - dump_log = argv[1].i; + dump_log_num = argv[1].i; last_log_num = DataFlash.find_last_log(); - if (dump_log == -2) { + if (dump_log_num == -2) { DataFlash.DumpPageInfo(cliSerial); return(-1); - } else if (dump_log <= 0) { + } else if (dump_log_num <= 0) { cliSerial->printf_P(PSTR("dumping all\n")); Log_Read(0, 1, 0); return(-1); - } else if ((argc != 2) || ((uint16_t)dump_log <= (last_log_num - DataFlash.get_num_logs())) || (static_cast(dump_log) > last_log_num)) { + } else if ((argc != 2) || ((uint16_t)dump_log_num <= (last_log_num - DataFlash.get_num_logs())) || (static_cast(dump_log_num) > last_log_num)) { cliSerial->printf_P(PSTR("bad log number\n")); return(-1); } - DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end); - Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end); + DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end); + Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end); return (0); } #endif -static int8_t -erase_logs(uint8_t argc, const Menu::arg *argv) +int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv) { in_mavlink_delay = true; do_erase_logs(); @@ -99,8 +92,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv) return 0; } -static int8_t -select_logs(uint8_t argc, const Menu::arg *argv) +int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv) { uint16_t bits; @@ -148,15 +140,14 @@ select_logs(uint8_t argc, const Menu::arg *argv) return(0); } -static int8_t -process_logs(uint8_t argc, const Menu::arg *argv) +int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv) { log_menu.run(); return 0; } #endif // CLI_ENABLED -static void do_erase_logs(void) +void Copter::do_erase_logs(void) { gcs_send_text_P(SEVERITY_HIGH, PSTR("Erasing logs\n")); DataFlash.EraseAll(); @@ -178,7 +169,7 @@ struct PACKED log_AutoTune { }; // Write an Autotune data packet -static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_target, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) +void Copter::Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_target, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) { struct log_AutoTune pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), @@ -203,7 +194,7 @@ struct PACKED log_AutoTuneDetails { }; // Write an Autotune data packet -static void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) +void Copter::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) { struct log_AutoTuneDetails pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG), @@ -216,7 +207,7 @@ static void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) #endif // Write a Current data packet -static void Log_Write_Current() +void Copter::Log_Write_Current() { DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle())); @@ -235,7 +226,7 @@ struct PACKED log_Optflow { }; // Write an optical flow packet -static void Log_Write_Optflow() +void Copter::Log_Write_Optflow() { #if OPTFLOW == ENABLED // exit immediately if not enabled @@ -273,7 +264,7 @@ struct PACKED log_Nav_Tuning { }; // Write an Nav Tuning packet -static void Log_Write_Nav_Tuning() +void Copter::Log_Write_Nav_Tuning() { const Vector3f &pos_target = pos_control.get_pos_target(); const Vector3f &vel_target = pos_control.get_vel_target(); @@ -314,7 +305,7 @@ struct PACKED log_Control_Tuning { }; // Write a control tuning packet -static void Log_Write_Control_Tuning() +void Copter::Log_Write_Control_Tuning() { struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), @@ -345,7 +336,7 @@ struct PACKED log_Performance { }; // Write a performance monitoring packet -static void Log_Write_Performance() +void Copter::Log_Write_Performance() { struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), @@ -361,7 +352,7 @@ static void Log_Write_Performance() } // Write a mission command. Total length : 36 bytes -static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) +void Copter::Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) { mavlink_mission_item_t mav_cmd = {}; AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd); @@ -369,7 +360,7 @@ static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) } // Write an attitude packet -static void Log_Write_Attitude() +void Copter::Log_Write_Attitude() { Vector3f targets = attitude_control.angle_ef_targets(); DataFlash.Log_Write_Attitude(ahrs, targets); @@ -406,7 +397,7 @@ struct PACKED log_Rate { }; // Write an rate packet -static void Log_Write_Rate() +void Copter::Log_Write_Rate() { const Vector3f &rate_targets = attitude_control.rate_bf_targets(); const Vector3f &accel_target = pos_control.get_accel_target(); @@ -439,7 +430,7 @@ struct PACKED log_MotBatt { }; // Write an rate packet -static void Log_Write_MotBatt() +void Copter::Log_Write_MotBatt() { struct log_MotBatt pkt_mot = { LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG), @@ -458,7 +449,7 @@ struct PACKED log_Startup { }; // Write Startup packet -static void Log_Write_Startup() +void Copter::Log_Write_Startup() { struct log_Startup pkt = { LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), @@ -469,7 +460,7 @@ static void Log_Write_Startup() Log_Write_EntireMission(); } -static void Log_Write_EntireMission() +void Copter::Log_Write_EntireMission() { DataFlash.Log_Write_Message_P(PSTR("New mission")); @@ -488,7 +479,7 @@ struct PACKED log_Event { }; // Wrote an event packet -static void Log_Write_Event(uint8_t id) +void Copter::Log_Write_Event(uint8_t id) { if (should_log(MASK_LOG_ANY)) { struct log_Event pkt = { @@ -509,7 +500,7 @@ struct PACKED log_Data_Int16t { // Write an int16_t data packet UNUSED_FUNCTION -static void Log_Write_Data(uint8_t id, int16_t value) +void Copter::Log_Write_Data(uint8_t id, int16_t value) { if (should_log(MASK_LOG_ANY)) { struct log_Data_Int16t pkt = { @@ -531,7 +522,7 @@ struct PACKED log_Data_UInt16t { // Write an uint16_t data packet UNUSED_FUNCTION -static void Log_Write_Data(uint8_t id, uint16_t value) +void Copter::Log_Write_Data(uint8_t id, uint16_t value) { if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt16t pkt = { @@ -552,7 +543,7 @@ struct PACKED log_Data_Int32t { }; // Write an int32_t data packet -static void Log_Write_Data(uint8_t id, int32_t value) +void Copter::Log_Write_Data(uint8_t id, int32_t value) { if (should_log(MASK_LOG_ANY)) { struct log_Data_Int32t pkt = { @@ -573,7 +564,7 @@ struct PACKED log_Data_UInt32t { }; // Write a uint32_t data packet -static void Log_Write_Data(uint8_t id, uint32_t value) +void Copter::Log_Write_Data(uint8_t id, uint32_t value) { if (should_log(MASK_LOG_ANY)) { struct log_Data_UInt32t pkt = { @@ -595,7 +586,7 @@ struct PACKED log_Data_Float { // Write a float data packet UNUSED_FUNCTION -static void Log_Write_Data(uint8_t id, float value) +void Copter::Log_Write_Data(uint8_t id, float value) { if (should_log(MASK_LOG_ANY)) { struct log_Data_Float pkt = { @@ -616,7 +607,7 @@ struct PACKED log_Error { }; // Write an error packet -static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) +void Copter::Log_Write_Error(uint8_t sub_system, uint8_t error_code) { struct log_Error pkt = { LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG), @@ -627,7 +618,7 @@ static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -static void Log_Write_Baro(void) +void Copter::Log_Write_Baro(void) { DataFlash.Log_Write_Baro(barometer); } @@ -642,7 +633,7 @@ struct PACKED log_ParameterTuning { int16_t tuning_high; // tuning high end value }; -static void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) +void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) { struct log_ParameterTuning pkt_tune = { LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), @@ -657,7 +648,7 @@ static void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune)); } -static const struct LogStructure log_structure[] PROGMEM = { +const struct LogStructure Copter::log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES, #if AUTOTUNE_ENABLED == ENABLED { LOG_AUTOTUNE_MSG, sizeof(log_AutoTune), @@ -699,7 +690,7 @@ static const struct LogStructure log_structure[] PROGMEM = { #if CLI_ENABLED == ENABLED // Read the DataFlash log memory -static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) +void Copter::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) { cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING "\nFree RAM: %u\n" @@ -709,13 +700,13 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) cliSerial->println_P(PSTR(HAL_BOARD_NAME)); DataFlash.LogReadProcess(log_num, start_page, end_page, - print_flight_mode, + FUNCTOR_BIND_MEMBER(&Copter::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t), cliSerial); } #endif // CLI_ENABLED // start a new log -static void start_logging() +void Copter::start_logging() { if (g.log_bitmask != 0) { if (!ap.logging_started) { @@ -744,33 +735,17 @@ static void start_logging() } } -#else // LOGGING_ENABLED - -static void Log_Write_Startup() {} -static void Log_Write_EntireMission() {} -#if AUTOTUNE_ENABLED == ENABLED -static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_target, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) {} -static void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {} -#endif -static void Log_Write_Current() {} -static void Log_Write_Attitude() {} -static void Log_Write_Rate() {} -static void Log_Write_MotBatt() {} -static void Log_Write_Data(uint8_t id, int16_t value){} -static void Log_Write_Data(uint8_t id, uint16_t value){} -static void Log_Write_Data(uint8_t id, int32_t value){} -static void Log_Write_Data(uint8_t id, uint32_t value){} -static void Log_Write_Data(uint8_t id, float value){} -static void Log_Write_Event(uint8_t id){} -static void Log_Write_Optflow() {} -static void Log_Write_Nav_Tuning() {} -static void Log_Write_Control_Tuning() {} -static void Log_Write_Performance() {} -static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {} -static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} -static void Log_Write_Baro(void) {} -static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { - return 0; +void Copter::log_init(void) +{ + DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0])); + if (!DataFlash.CardInserted()) { + gcs_send_text_P(SEVERITY_HIGH, PSTR("No dataflash inserted")); + g.log_bitmask.set(0); + } else if (DataFlash.NeedErase()) { + gcs_send_text_P(SEVERITY_HIGH, PSTR("ERASING LOGS")); + do_erase_logs(); + gcs[0].reset_cli_timeout(); + } } #endif // LOGGING_DISABLED diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 764068f204..a1b79f276d 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1,4 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,13 +22,13 @@ * */ -#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} } -#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} } -#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} } -#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} } -#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} } +#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} } +#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} } +#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &copter.g.v, {group_info : class::var_info} } +#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} } +#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} } -const AP_Param::Info var_info[] PROGMEM = { +const AP_Param::Info Copter::var_info[] PROGMEM = { // @Param: SYSID_SW_MREV // @DisplayName: Eeprom format version number // @Description: This value is incremented when changes are made to the eeprom format @@ -1069,7 +1072,7 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = { { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, }; -static void load_parameters(void) +void Copter::load_parameters(void) { if (!AP_Param::check_var_info()) { cliSerial->printf_P(PSTR("Bad var table\n")); diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index d754b8a38e..e7d2ef9d84 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -1,7 +1,9 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #ifdef USERHOOK_INIT -void userhook_init() +void Copter::userhook_init() { // put your initialisation code here // this will be called once at start-up @@ -9,36 +11,36 @@ void userhook_init() #endif #ifdef USERHOOK_FASTLOOP -void userhook_FastLoop() +void Copter::userhook_FastLoop() { // put your 100Hz code here } #endif #ifdef USERHOOK_50HZLOOP -void userhook_50Hz() +void Copter::userhook_50Hz() { // put your 50Hz code here } #endif #ifdef USERHOOK_MEDIUMLOOP -void userhook_MediumLoop() +void Copter::userhook_MediumLoop() { // put your 10Hz code here } #endif #ifdef USERHOOK_SLOWLOOP -void userhook_SlowLoop() +void Copter::userhook_SlowLoop() { // put your 3.3Hz code here } #endif #ifdef USERHOOK_SUPERSLOWLOOP -void userhook_SuperSlowLoop() +void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here } -#endif \ No newline at end of file +#endif diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index c265851700..5915d86d8f 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * the home_state has a number of possible values (see enum HomeState in defines.h's) * HOME_UNSET = home is not set, no GPS positions yet received @@ -8,7 +10,7 @@ */ // checks if we should update ahrs/RTL home position from the EKF -static void update_home_from_EKF() +void Copter::update_home_from_EKF() { // exit immediately if home already set if (ap.home_state != HOME_UNSET) { @@ -25,7 +27,7 @@ static void update_home_from_EKF() } // set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically -static void set_home_to_current_location_inflight() { +void Copter::set_home_to_current_location_inflight() { // get current location from EKF Location temp_loc; if (inertial_nav.get_location(temp_loc)) { @@ -36,7 +38,7 @@ static void set_home_to_current_location_inflight() { } // set_home_to_current_location - set home to current GPS location -static bool set_home_to_current_location() { +bool Copter::set_home_to_current_location() { // get current location from EKF Location temp_loc; if (inertial_nav.get_location(temp_loc)) { @@ -46,7 +48,7 @@ static bool set_home_to_current_location() { } // set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved -static bool set_home_to_current_location_and_lock() +bool Copter::set_home_to_current_location_and_lock() { if (set_home_to_current_location()) { set_home_state(HOME_SET_AND_LOCKED); @@ -57,7 +59,7 @@ static bool set_home_to_current_location_and_lock() // set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved // unless this function is called again -static bool set_home_and_lock(const Location& loc) +bool Copter::set_home_and_lock(const Location& loc) { if (set_home(loc)) { set_home_state(HOME_SET_AND_LOCKED); @@ -69,7 +71,7 @@ static bool set_home_and_lock(const Location& loc) // set_home - sets ahrs home (used for RTL) to specified location // initialises inertial nav and compass on first call // returns true if home location set successfully -static bool set_home(const Location& loc) +bool Copter::set_home(const Location& loc) { // check location is valid if (loc.lat == 0 && loc.lng == 0) { @@ -87,7 +89,6 @@ static bool set_home(const Location& loc) } // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); - scaleLongUp = 1.0f/scaleLongDown; // record home is set set_home_state(HOME_SET_NOT_LOCKED); @@ -106,7 +107,7 @@ static bool set_home(const Location& loc) // far_from_EKF_origin - checks if a location is too far from the EKF origin // returns true if too far -static bool far_from_EKF_origin(const Location& loc) +bool Copter::far_from_EKF_origin(const Location& loc) { // check distance to EKF origin const struct Location &ekf_origin = inertial_nav.get_origin(); @@ -119,7 +120,7 @@ static bool far_from_EKF_origin(const Location& loc) } // checks if we should update ahrs/RTL home position from GPS -static void set_system_time_from_GPS() +void Copter::set_system_time_from_GPS() { // exit immediately if system time already set if (ap.system_time_set) { diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 60e3019374..6063b84432 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -1,45 +1,9 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// forward declarations to make compiler happy -static void do_takeoff(const AP_Mission::Mission_Command& cmd); -static void do_nav_wp(const AP_Mission::Mission_Command& cmd); -static void do_land(const AP_Mission::Mission_Command& cmd); -static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); -static void do_circle(const AP_Mission::Mission_Command& cmd); -static void do_loiter_time(const AP_Mission::Mission_Command& cmd); -static void do_spline_wp(const AP_Mission::Mission_Command& cmd); -#if NAV_GUIDED == ENABLED -static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); -static void do_guided_limits(const AP_Mission::Mission_Command& cmd); -#endif -static void do_wait_delay(const AP_Mission::Mission_Command& cmd); -static void do_within_distance(const AP_Mission::Mission_Command& cmd); -static void do_change_alt(const AP_Mission::Mission_Command& cmd); -static void do_yaw(const AP_Mission::Mission_Command& cmd); -static void do_change_speed(const AP_Mission::Mission_Command& cmd); -static void do_set_home(const AP_Mission::Mission_Command& cmd); -static void do_roi(const AP_Mission::Mission_Command& cmd); -static void do_mount_control(const AP_Mission::Mission_Command& cmd); -#if CAMERA == ENABLED -static void do_digicam_configure(const AP_Mission::Mission_Command& cmd); -static void do_digicam_control(const AP_Mission::Mission_Command& cmd); -#endif -#if PARACHUTE == ENABLED -static void do_parachute(const AP_Mission::Mission_Command& cmd); -#endif -#if EPM_ENABLED == ENABLED -static void do_gripper(const AP_Mission::Mission_Command& cmd); -#endif -static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); -static bool verify_circle(const AP_Mission::Mission_Command& cmd); -static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); -#if NAV_GUIDED == ENABLED -static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); -#endif -static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); +#include "Copter.h" // start_command - this function will be called when the ap_mission lib wishes to start a new command -static bool start_command(const AP_Mission::Mission_Command& cmd) +bool Copter::start_command(const AP_Mission::Mission_Command& cmd) { // To-Do: logging when new commands start/end if (should_log(MASK_LOG_CMD)) { @@ -198,7 +162,7 @@ static bool start_command(const AP_Mission::Mission_Command& cmd) // verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing // should return true once the active navigation command completes successfully // called at 10hz or higher -static bool verify_command(const AP_Mission::Mission_Command& cmd) +bool Copter::verify_command(const AP_Mission::Mission_Command& cmd) { switch(cmd.id) { @@ -277,7 +241,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) } // exit_mission - function that is called once the mission completes -static void exit_mission() +void Copter::exit_mission() { // play a tone AP_Notify::events.mission_complete = 1; @@ -305,7 +269,7 @@ static void exit_mission() /********************************************************************************/ // do_RTL - start Return-to-Launch -static void do_RTL(void) +void Copter::do_RTL(void) { // start rtl in auto flight mode auto_rtl_start(); @@ -316,7 +280,7 @@ static void do_RTL(void) /********************************************************************************/ // do_takeoff - initiate takeoff navigation command -static void do_takeoff(const AP_Mission::Mission_Command& cmd) +void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position float takeoff_alt = cmd.content.location.alt; @@ -326,7 +290,7 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd) } // do_nav_wp - initiate move to next waypoint -static void do_nav_wp(const AP_Mission::Mission_Command& cmd) +void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd) { const Vector3f &curr_pos = inertial_nav.get_position(); const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos); @@ -345,7 +309,7 @@ static void do_nav_wp(const AP_Mission::Mission_Command& cmd) } // do_land - initiate landing procedure -static void do_land(const AP_Mission::Mission_Command& cmd) +void Copter::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed @@ -369,7 +333,7 @@ static void do_land(const AP_Mission::Mission_Command& cmd) // do_loiter_unlimited - start loitering with no end conditions // note: caller should set yaw_mode -static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { Vector3f target_pos; @@ -395,7 +359,7 @@ static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) } // do_circle - initiate moving in a circle -static void do_circle(const AP_Mission::Mission_Command& cmd) +void Copter::do_circle(const AP_Mission::Mission_Command& cmd) { Vector3f curr_pos = inertial_nav.get_position(); Vector3f circle_center = pv_location_to_vector(cmd.content.location); @@ -438,7 +402,7 @@ static void do_circle(const AP_Mission::Mission_Command& cmd) // do_loiter_time - initiate loitering at a point for a given time period // note: caller should set yaw_mode -static void do_loiter_time(const AP_Mission::Mission_Command& cmd) +void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd) { Vector3f target_pos; @@ -467,7 +431,7 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd) } // do_spline_wp - initiate move to next waypoint -static void do_spline_wp(const AP_Mission::Mission_Command& cmd) +void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd) { const Vector3f& curr_pos = inertial_nav.get_position(); Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos); @@ -512,7 +476,7 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd) #if NAV_GUIDED == ENABLED // do_nav_guided_enable - initiate accepting commands from external nav computer -static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // initialise guided limits @@ -527,7 +491,7 @@ static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) #if PARACHUTE == ENABLED // do_parachute - configure or release parachute -static void do_parachute(const AP_Mission::Mission_Command& cmd) +void Copter::do_parachute(const AP_Mission::Mission_Command& cmd) { switch (cmd.p1) { case PARACHUTE_DISABLE: @@ -550,7 +514,7 @@ static void do_parachute(const AP_Mission::Mission_Command& cmd) #if EPM_ENABLED == ENABLED // do_gripper - control EPM gripper -static void do_gripper(const AP_Mission::Mission_Command& cmd) +void Copter::do_gripper(const AP_Mission::Mission_Command& cmd) { // Note: we ignore the gripper num parameter because we only support one gripper switch (cmd.content.gripper.action) { @@ -571,7 +535,7 @@ static void do_gripper(const AP_Mission::Mission_Command& cmd) #if NAV_GUIDED == ENABLED // do_guided_limits - pass guided limits to guided controller -static void do_guided_limits(const AP_Mission::Mission_Command& cmd) +void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd) { guided_limit_set(cmd.p1 * 1000, // convert seconds to ms cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm @@ -585,14 +549,14 @@ static void do_guided_limits(const AP_Mission::Mission_Command& cmd) /********************************************************************************/ // verify_takeoff - check if we have completed the takeoff -static bool verify_takeoff() +bool Copter::verify_takeoff() { // have we reached our target altitude? return wp_nav.reached_wp_destination(); } // verify_land - returns true if landing has been completed -static bool verify_land() +bool Copter::verify_land() { bool retval = false; @@ -628,7 +592,7 @@ static bool verify_land() } // verify_nav_wp - check if we have reached the next way point -static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd) +bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { // check if we have reached the waypoint if( !wp_nav.reached_wp_destination() ) { @@ -652,13 +616,13 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd) } } -static bool verify_loiter_unlimited() +bool Copter::verify_loiter_unlimited() { return false; } // verify_loiter_time - check if we have loitered long enough -static bool verify_loiter_time() +bool Copter::verify_loiter_time() { // return immediately if we haven't reached our destination if (!wp_nav.reached_wp_destination()) { @@ -675,7 +639,7 @@ static bool verify_loiter_time() } // verify_circle - check if we have circled the point enough -static bool verify_circle(const AP_Mission::Mission_Command& cmd) +bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) { // check if we've reached the edge if (auto_mode == Auto_CircleMoveToEdge) { @@ -704,19 +668,16 @@ static bool verify_circle(const AP_Mission::Mission_Command& cmd) return fabsf(circle_nav.get_angle_total()/M_2PI_F) >= LOWBYTE(cmd.p1); } -// externs to remove compiler warning -extern bool rtl_state_complete; - // verify_RTL - handles any state changes required to implement RTL // do_RTL should have been called once first to initialise all variables // returns true with RTL has completed successfully -static bool verify_RTL() +bool Copter::verify_RTL() { return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land)); } // verify_spline_wp - check if we have reached the next way point using spline -static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd) +bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd) { // check if we have reached the waypoint if( !wp_nav.reached_wp_destination() ) { @@ -739,7 +700,7 @@ static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd) #if NAV_GUIDED == ENABLED // verify_nav_guided - check if we have breached any limits -static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { // if disabling guided mode then immediately return true so we move to next command if (cmd.p1 == 0) { @@ -756,23 +717,23 @@ static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // Condition (May) commands /********************************************************************************/ -static void do_wait_delay(const AP_Mission::Mission_Command& cmd) +void Copter::do_wait_delay(const AP_Mission::Mission_Command& cmd) { condition_start = millis(); condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -static void do_change_alt(const AP_Mission::Mission_Command& cmd) +void Copter::do_change_alt(const AP_Mission::Mission_Command& cmd) { // To-Do: store desired altitude in a variable so that it can be verified later } -static void do_within_distance(const AP_Mission::Mission_Command& cmd) +void Copter::do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters * 100; } -static void do_yaw(const AP_Mission::Mission_Command& cmd) +void Copter::do_yaw(const AP_Mission::Mission_Command& cmd) { set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, @@ -786,7 +747,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd) // Verify Condition (May) commands /********************************************************************************/ -static bool verify_wait_delay() +bool Copter::verify_wait_delay() { if (millis() - condition_start > (uint32_t)max(condition_value,0)) { condition_value = 0; @@ -795,13 +756,13 @@ static bool verify_wait_delay() return false; } -static bool verify_change_alt() +bool Copter::verify_change_alt() { // To-Do: use recorded target altitude to verify we have reached the target return true; } -static bool verify_within_distance() +bool Copter::verify_within_distance() { // update distance calculation calc_wp_distance(); @@ -813,7 +774,7 @@ static bool verify_within_distance() } // verify_yaw - return true if we have reached the desired heading -static bool verify_yaw() +bool Copter::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { @@ -833,7 +794,7 @@ static bool verify_yaw() /********************************************************************************/ // do_guided - start guided mode -static bool do_guided(const AP_Mission::Mission_Command& cmd) +bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { Vector3f pos_or_vel; // target location or velocity @@ -866,14 +827,14 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd) return true; } -static void do_change_speed(const AP_Mission::Mission_Command& cmd) +void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { wp_nav.set_speed_xy(cmd.content.speed.target_ms * 100.0f); } } -static void do_set_home(const AP_Mission::Mission_Command& cmd) +void Copter::do_set_home(const AP_Mission::Mission_Command& cmd) { if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { set_home_to_current_location(); @@ -888,13 +849,13 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd) // this involves either moving the camera to point at the ROI (region of interest) // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint -static void do_roi(const AP_Mission::Mission_Command& cmd) +void Copter::do_roi(const AP_Mission::Mission_Command& cmd) { set_auto_yaw_roi(cmd.content.location); } // do_digicam_configure Send Digicam Configure message with the camera library -static void do_digicam_configure(const AP_Mission::Mission_Command& cmd) +void Copter::do_digicam_configure(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED camera.configure_cmd(cmd); @@ -902,7 +863,7 @@ static void do_digicam_configure(const AP_Mission::Mission_Command& cmd) } // do_digicam_control Send Digicam Control message with the camera library -static void do_digicam_control(const AP_Mission::Mission_Command& cmd) +void Copter::do_digicam_control(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED camera.control_cmd(cmd); @@ -911,7 +872,7 @@ static void do_digicam_control(const AP_Mission::Mission_Command& cmd) } // do_take_picture - take a picture with the camera library -static void do_take_picture() +void Copter::do_take_picture() { #if CAMERA == ENABLED camera.trigger_pic(true); @@ -920,7 +881,7 @@ static void do_take_picture() } // log_picture - log picture taken and send feedback to GCS -static void log_picture() +void Copter::log_picture() { gcs_send_message(MSG_CAMERA_FEEDBACK); if (should_log(MASK_LOG_CAMERA)) { @@ -929,7 +890,7 @@ static void log_picture() } // point the camera to a specified angle -static void do_mount_control(const AP_Mission::Mission_Command& cmd) +void Copter::do_mount_control(const AP_Mission::Mission_Command& cmd) { #if MOUNT == ENABLED camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index ddb3fd5a5c..a1c93135ef 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -1,11 +1,13 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* compass/motor interference calibration */ // setup_compassmot - sets compass's motor interference parameters -static uint8_t mavlink_compassmot(mavlink_channel_t chan) +uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) { int8_t comp_type; // throttle or current based compensation Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero diff --git a/ArduCopter/compat.cpp b/ArduCopter/compat.cpp index b5b2926e17..1ac9ae3c12 100644 --- a/ArduCopter/compat.cpp +++ b/ArduCopter/compat.cpp @@ -1,16 +1,16 @@ +#include "Copter.h" - -static void delay(uint32_t ms) +void Copter::delay(uint32_t ms) { hal.scheduler->delay(ms); } -static uint32_t millis() +uint32_t Copter::millis() { return hal.scheduler->millis(); } -static uint32_t micros() +uint32_t Copter::micros() { return hal.scheduler->micros(); } diff --git a/ArduCopter/compat.h b/ArduCopter/compat.h deleted file mode 100644 index 89abbc282b..0000000000 --- a/ArduCopter/compat.h +++ /dev/null @@ -1,9 +0,0 @@ - -#ifndef __COMPAT_H__ -#define __COMPAT_H__ - -/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */ -static void run_cli(AP_HAL::UARTDriver *port); - -#endif // __COMPAT_H__ - diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 593dcf1d06..ec20ccd236 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_acro.pde - init and run calls for acro flight mode */ // acro_init - initialise acro controller -static bool acro_init(bool ignore_checks) +bool Copter::acro_init(bool ignore_checks) { // always successfully enter acro return true; @@ -13,7 +15,7 @@ static bool acro_init(bool ignore_checks) // acro_run - runs the acro controller // should be called at 100hz or more -static void acro_run() +void Copter::acro_run() { float target_roll, target_pitch, target_yaw; int16_t pilot_throttle_scaled; @@ -40,7 +42,7 @@ static void acro_run() // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // returns desired angle rates in centi-degrees-per-second -static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) +void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) { float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 6ad11a8712..7b438bac55 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_althold.pde - init and run calls for althold, flight mode */ // althold_init - initialise althold controller -static bool althold_init(bool ignore_checks) +bool Copter::althold_init(bool ignore_checks) { // initialize vertical speeds and leash lengths pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); @@ -19,7 +21,7 @@ static bool althold_init(bool ignore_checks) // althold_run - runs the althold controller // should be called at 100hz or more -static void althold_run() +void Copter::althold_run() { float target_roll, target_pitch; float target_yaw_rate; diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index c36250d85a..34dbe52a85 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_auto.pde - init and run calls for auto flight mode * @@ -18,7 +20,7 @@ */ // auto_init - initialise auto controller -static bool auto_init(bool ignore_checks) +bool Copter::auto_init(bool ignore_checks) { if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; @@ -46,7 +48,7 @@ static bool auto_init(bool ignore_checks) // auto_run - runs the auto controller // should be called at 100hz or more // relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands -static void auto_run() +void Copter::auto_run() { // call the correct auto controller switch (auto_mode) { @@ -89,7 +91,7 @@ static void auto_run() } // auto_takeoff_start - initialises waypoint controller to implement take-off -static void auto_takeoff_start(float final_alt_above_home) +void Copter::auto_takeoff_start(float final_alt_above_home) { auto_mode = Auto_TakeOff; @@ -107,7 +109,7 @@ static void auto_takeoff_start(float final_alt_above_home) // auto_takeoff_run - takeoff in auto mode // called by auto_run at 100hz or more -static void auto_takeoff_run() +void Copter::auto_takeoff_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -138,7 +140,7 @@ static void auto_takeoff_run() } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -static void auto_wp_start(const Vector3f& destination) +void Copter::auto_wp_start(const Vector3f& destination) { auto_mode = Auto_WP; @@ -154,7 +156,7 @@ static void auto_wp_start(const Vector3f& destination) // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -static void auto_wp_run() +void Copter::auto_wp_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -194,7 +196,9 @@ static void auto_wp_run() // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller // seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided -static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_destination) +void Copter::auto_spline_start(const Vector3f& destination, bool stopped_at_start, + AC_WPNav::spline_segment_end_type seg_end_type, + const Vector3f& next_destination) { auto_mode = Auto_Spline; @@ -210,7 +214,7 @@ static void auto_spline_start(const Vector3f& destination, bool stopped_at_start // auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more -static void auto_spline_run() +void Copter::auto_spline_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -249,7 +253,7 @@ static void auto_spline_run() } // auto_land_start - initialises controller to implement a landing -static void auto_land_start() +void Copter::auto_land_start() { // set target to stopping point Vector3f stopping_point; @@ -260,7 +264,7 @@ static void auto_land_start() } // auto_land_start - initialises controller to implement a landing -static void auto_land_start(const Vector3f& destination) +void Copter::auto_land_start(const Vector3f& destination) { auto_mode = Auto_Land; @@ -276,7 +280,7 @@ static void auto_land_start(const Vector3f& destination) // auto_land_run - lands in auto mode // called by auto_run at 100hz or more -static void auto_land_run() +void Copter::auto_land_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; @@ -328,7 +332,7 @@ static void auto_land_run() } // auto_rtl_start - initialises RTL in AUTO flight mode -static void auto_rtl_start() +void Copter::auto_rtl_start() { auto_mode = Auto_RTL; @@ -338,7 +342,7 @@ static void auto_rtl_start() // auto_rtl_run - rtl in AUTO flight mode // called by auto_run at 100hz or more -void auto_rtl_run() +void Copter::auto_rtl_run() { // call regular rtl flight mode run function rtl_run(); @@ -347,7 +351,7 @@ void auto_rtl_run() // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks -static void auto_circle_movetoedge_start() +void Copter::auto_circle_movetoedge_start() { // check our distance from edge of circle Vector3f circle_edge; @@ -372,7 +376,7 @@ static void auto_circle_movetoedge_start() } // auto_circle_start - initialises controller to fly a circle in AUTO flight mode -static void auto_circle_start() +void Copter::auto_circle_start() { auto_mode = Auto_Circle; @@ -383,7 +387,7 @@ static void auto_circle_start() // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void auto_circle_run() +void Copter::auto_circle_run() { // call circle controller circle_nav.update(); @@ -397,7 +401,7 @@ void auto_circle_run() #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void auto_nav_guided_start() +void Copter::auto_nav_guided_start() { auto_mode = Auto_NavGuided; @@ -410,7 +414,7 @@ void auto_nav_guided_start() // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void auto_nav_guided_run() +void Copter::auto_nav_guided_run() { // call regular guided flight mode run function guided_run(); @@ -419,7 +423,7 @@ void auto_nav_guided_run() // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool auto_loiter_start() +bool Copter::auto_loiter_start() { // return failure if GPS is bad if (!position_ok()) { @@ -445,7 +449,7 @@ bool auto_loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void auto_loiter_run() +void Copter::auto_loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { @@ -467,7 +471,7 @@ void auto_loiter_run() // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL -uint8_t get_default_auto_yaw_mode(bool rtl) +uint8_t Copter::get_default_auto_yaw_mode(bool rtl) { switch (g.wp_yaw_behavior) { @@ -495,7 +499,7 @@ uint8_t get_default_auto_yaw_mode(bool rtl) } // set_auto_yaw_mode - sets the yaw mode for auto -void set_auto_yaw_mode(uint8_t yaw_mode) +void Copter::set_auto_yaw_mode(uint8_t yaw_mode) { // return immediately if no change if (auto_yaw_mode == yaw_mode) { @@ -532,7 +536,7 @@ void set_auto_yaw_mode(uint8_t yaw_mode) } // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) +void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target int32_t curr_yaw_target = attitude_control.angle_ef_targets().z; @@ -565,7 +569,7 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, i } // set_auto_yaw_roi - sets the yaw to look at roi for auto mode -static void set_auto_yaw_roi(const Location &roi_location) +void Copter::set_auto_yaw_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { @@ -603,7 +607,7 @@ static void set_auto_yaw_roi(const Location &roi_location) // get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate -float get_auto_heading(void) +float Copter::get_auto_heading(void) { switch(auto_yaw_mode) { diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 96116a6735..fb4848288b 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #if AUTOTUNE_ENABLED == ENABLED /* @@ -160,7 +162,7 @@ static float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; // autotune_init - should be called when autotune mode is selected -static bool autotune_init(bool ignore_checks) +bool Copter::autotune_init(bool ignore_checks) { bool success = true; @@ -207,7 +209,7 @@ static bool autotune_init(bool ignore_checks) } // autotune_stop - should be called when the ch7/ch8 switch is switched OFF -static void autotune_stop() +void Copter::autotune_stop() { // set gains to their original values autotune_load_orig_gains(); @@ -224,7 +226,7 @@ static void autotune_stop() } // autotune_start - Initialize autotune flight mode -static bool autotune_start(bool ignore_checks) +bool Copter::autotune_start(bool ignore_checks) { // only allow flip from Stabilize or AltHold flight modes if (control_mode != STABILIZE && control_mode != ALT_HOLD) { @@ -253,7 +255,7 @@ static bool autotune_start(bool ignore_checks) // autotune_run - runs the autotune flight mode // should be called at 100hz or more -static void autotune_run() +void Copter::autotune_run() { float target_roll, target_pitch; float target_yaw_rate; @@ -329,7 +331,7 @@ static void autotune_run() } // autotune_attitude_controller - sets attitude control targets during tuning -static void autotune_attitude_control() +void Copter::autotune_attitude_control() { float rotation_rate = 0.0f; // rotation rate in radians/second float lean_angle = 0.0f; @@ -729,7 +731,7 @@ static void autotune_attitude_control() // autotune_backup_gains_and_initialise - store current gains as originals // called before tuning starts to backup original gains -static void autotune_backup_gains_and_initialise() +void Copter::autotune_backup_gains_and_initialise() { // initialise state because this is our first time if (autotune_roll_enabled()) { @@ -783,7 +785,7 @@ static void autotune_backup_gains_and_initialise() // autotune_load_orig_gains - set gains to their original values // called by autotune_stop and autotune_failed functions -static void autotune_load_orig_gains() +void Copter::autotune_load_orig_gains() { // sanity check the gains bool failed = false; @@ -825,7 +827,7 @@ static void autotune_load_orig_gains() } // autotune_load_tuned_gains - load tuned gains -static void autotune_load_tuned_gains() +void Copter::autotune_load_tuned_gains() { // sanity check the gains bool failed = true; @@ -865,7 +867,7 @@ static void autotune_load_tuned_gains() // autotune_load_intra_test_gains - gains used between tests // called during testing mode's update-gains step to set gains ahead of return-to-level step -static void autotune_load_intra_test_gains() +void Copter::autotune_load_intra_test_gains() { // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) // sanity check the gains @@ -900,7 +902,7 @@ static void autotune_load_intra_test_gains() // autotune_load_twitch_gains - load the to-be-tested gains for a single axis // called by autotune_attitude_control() just before it beings testing a gain (i.e. just before it twitches) -static void autotune_load_twitch_gains() +void Copter::autotune_load_twitch_gains() { bool failed = true; switch (autotune_state.axis) { @@ -941,7 +943,7 @@ static void autotune_load_twitch_gains() // autotune_save_tuning_gains - save the final tuned gains for each axis // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) -static void autotune_save_tuning_gains() +void Copter::autotune_save_tuning_gains() { // if we successfully completed tuning if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) { @@ -1033,7 +1035,7 @@ static void autotune_save_tuning_gains() } // autotune_update_gcs - send message to ground station -void autotune_update_gcs(uint8_t message_id) +void Copter::autotune_update_gcs(uint8_t message_id) { switch (message_id) { case AUTOTUNE_MESSAGE_STARTED: @@ -1055,21 +1057,21 @@ void autotune_update_gcs(uint8_t message_id) } // axis helper functions -inline bool autotune_roll_enabled() { +inline bool Copter::autotune_roll_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; } -inline bool autotune_pitch_enabled() { +inline bool Copter::autotune_pitch_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; } -inline bool autotune_yaw_enabled() { +inline bool Copter::autotune_yaw_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; } // autotune_twitching_test - twitching tests // update min and max and test for end conditions -void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) +void Copter::autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) { // capture maximum measurement if (measurement > measurement_max) { @@ -1109,7 +1111,7 @@ void autotune_twitching_test(float measurement, float target, float &measurement // autotune_updating_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1164,7 +1166,7 @@ void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, f // autotune_updating_d_down - decrease D and adjust P to optimize the D term for no bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1219,7 +1221,7 @@ void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step // autotune_updating_p_down - decrease P until we don't reach the target before time out // P is decreased to ensure we are not overshooting the target -void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) +void Copter::autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max < target) { if (autotune_state.ignore_next == 0){ @@ -1248,7 +1250,7 @@ void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step // autotune_updating_p_up - increase P to ensure the target is reached // P is increased until we achieve our target within a reasonable time -void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) +void Copter::autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max > target) { // ignore the next result unless it is the same as this one @@ -1277,7 +1279,7 @@ void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_r // autotune_updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing // P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold -void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // ignore the next result unless it is the same as this one @@ -1326,7 +1328,7 @@ void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d } // autotune_twitching_measure_acceleration - measure rate of change of measurement -void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +void Copter::autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) { if (rate_measurement_max < rate_measurement) { rate_measurement_max = rate_measurement; diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index 0291a04998..1898f219b9 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_brake.pde - init and run calls for brake flight mode */ // brake_init - initialise brake controller -static bool brake_init(bool ignore_checks) +bool Copter::brake_init(bool ignore_checks) { if (position_ok() || optflow_position_ok() || ignore_checks) { @@ -30,7 +32,7 @@ static bool brake_init(bool ignore_checks) // brake_run - runs the brake controller // should be called at 100hz or more -static void brake_run() +void Copter::brake_run() { float target_yaw_rate = 0; float target_climb_rate = 0; diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 612ad08c32..e28ad52342 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_circle.pde - init and run calls for circle flight mode */ // circle_init - initialise circle controller flight mode -static bool circle_init(bool ignore_checks) +bool Copter::circle_init(bool ignore_checks) { if (position_ok() || ignore_checks) { circle_pilot_yaw_override = false; @@ -27,7 +29,7 @@ static bool circle_init(bool ignore_checks) // circle_run - runs the circle flight mode // should be called at 100hz or more -static void circle_run() +void Copter::circle_run() { float target_yaw_rate = 0; float target_climb_rate = 0; diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index f9898b8810..369cbc1f86 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_drift.pde - init and run calls for drift flight mode */ @@ -27,7 +29,7 @@ #endif // drift_init - initialise drift controller -static bool drift_init(bool ignore_checks) +bool Copter::drift_init(bool ignore_checks) { if (position_ok() || ignore_checks) { return true; @@ -38,7 +40,7 @@ static bool drift_init(bool ignore_checks) // drift_run - runs the drift controller // should be called at 100hz or more -static void drift_run() +void Copter::drift_run() { static float breaker = 0.0f; static float roll_input = 0.0f; @@ -99,7 +101,7 @@ static void drift_run() } // get_throttle_assist - return throttle output (range 0 ~ 1000) based on pilot input and z-axis velocity -int16_t get_throttle_assist(float velz, int16_t pilot_throttle_scaled) +int16_t Copter::get_throttle_assist(float velz, int16_t pilot_throttle_scaled) { // throttle assist - adjusts throttle to slow the vehicle's vertical velocity // Only active when pilot's throttle is between 213 ~ 787 diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 5e5b9e85f9..877392d8b5 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_flip.pde - init and run calls for flip flight mode * original implementation in 2010 by Jose Julio @@ -37,7 +39,7 @@ int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back) // flip_init - initialise flip controller -static bool flip_init(bool ignore_checks) +bool Copter::flip_init(bool ignore_checks) { // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) { @@ -92,7 +94,7 @@ static bool flip_init(bool ignore_checks) // flip_run - runs the flip controller // should be called at 100hz or more -static void flip_run() +void Copter::flip_run() { int16_t throttle_out; float recovery_angle; diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index ccc1c7b67c..a8b28d5937 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_guided.pde - init and run calls for guided flight mode */ @@ -24,7 +26,7 @@ struct Guided_Limit { } guided_limit; // guided_init - initialise guided controller -static bool guided_init(bool ignore_checks) +bool Copter::guided_init(bool ignore_checks) { if (position_ok() || ignore_checks) { // initialise yaw @@ -39,7 +41,7 @@ static bool guided_init(bool ignore_checks) // guided_takeoff_start - initialises waypoint controller to implement take-off -static void guided_takeoff_start(float final_alt_above_home) +void Copter::guided_takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; @@ -56,7 +58,7 @@ static void guided_takeoff_start(float final_alt_above_home) } // initialise guided mode's position controller -static void guided_pos_control_start() +void Copter::guided_pos_control_start() { // set to position control mode guided_mode = Guided_WP; @@ -77,7 +79,7 @@ static void guided_pos_control_start() } // initialise guided mode's velocity controller -static void guided_vel_control_start() +void Copter::guided_vel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Velocity; @@ -91,7 +93,7 @@ static void guided_vel_control_start() } // initialise guided mode's posvel controller -static void guided_posvel_control_start() +void Copter::guided_posvel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_PosVel; @@ -118,7 +120,7 @@ static void guided_posvel_control_start() } // guided_set_destination - sets guided mode's target destination -static void guided_set_destination(const Vector3f& destination) +void Copter::guided_set_destination(const Vector3f& destination) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -129,7 +131,7 @@ static void guided_set_destination(const Vector3f& destination) } // guided_set_velocity - sets guided mode's target velocity -static void guided_set_velocity(const Vector3f& velocity) +void Copter::guided_set_velocity(const Vector3f& velocity) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { @@ -141,7 +143,7 @@ static void guided_set_velocity(const Vector3f& velocity) } // set guided mode posvel target -static void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { +void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { // check we are in velocity control mode if (guided_mode != Guided_PosVel) { guided_posvel_control_start(); @@ -156,7 +158,7 @@ static void guided_set_destination_posvel(const Vector3f& destination, const Vec // guided_run - runs the guided controller // should be called at 100hz or more -static void guided_run() +void Copter::guided_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -193,7 +195,7 @@ static void guided_run() // guided_takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more -static void guided_takeoff_run() +void Copter::guided_takeoff_run() { // if not auto armed or motors interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -225,7 +227,7 @@ static void guided_takeoff_run() // guided_pos_control_run - runs the guided position controller // called from guided_run -static void guided_pos_control_run() +void Copter::guided_pos_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -255,7 +257,7 @@ static void guided_pos_control_run() // guided_vel_control_run - runs the guided velocity controller // called from guided_run -static void guided_vel_control_run() +void Copter::guided_vel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -293,7 +295,7 @@ static void guided_vel_control_run() // guided_posvel_control_run - runs the guided spline controller // called from guided_run -static void guided_posvel_control_run() +void Copter::guided_posvel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -348,7 +350,7 @@ static void guided_posvel_control_run() // Guided Limit code // guided_limit_clear - clear/turn off guided limits -static void guided_limit_clear() +void Copter::guided_limit_clear() { guided_limit.timeout_ms = 0; guided_limit.alt_min_cm = 0.0f; @@ -357,7 +359,7 @@ static void guided_limit_clear() } // guided_limit_set - set guided timeout and movement limits -static void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { guided_limit.timeout_ms = timeout_ms; guided_limit.alt_min_cm = alt_min_cm; @@ -367,7 +369,7 @@ static void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_ma // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking // only called from AUTO mode's auto_nav_guided_start function -static void guided_limit_init_time_and_pos() +void Copter::guided_limit_init_time_and_pos() { // initialise start time guided_limit.start_time = hal.scheduler->millis(); @@ -378,7 +380,7 @@ static void guided_limit_init_time_and_pos() // guided_limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command -static bool guided_limit_check() +bool Copter::guided_limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index c6c3d15e04..b43aa518da 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -1,12 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + static bool land_with_gps; static uint32_t land_start_time; static bool land_pause; // land_init - initialise land controller -static bool land_init(bool ignore_checks) +bool Copter::land_init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do land_with_gps = position_ok(); @@ -33,7 +35,7 @@ static bool land_init(bool ignore_checks) // land_run - runs the land controller // should be called at 100hz or more -static void land_run() +void Copter::land_run() { if (land_with_gps) { land_gps_run(); @@ -45,7 +47,7 @@ static void land_run() // land_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more -static void land_gps_run() +void Copter::land_gps_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; @@ -118,7 +120,7 @@ static void land_gps_run() // land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more -static void land_nogps_run() +void Copter::land_nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; @@ -177,7 +179,7 @@ static void land_nogps_run() // get_land_descent_speed - high level landing logic // returns climb rate (in cm/s) which should be passed to the position controller // should be called at 100hz or higher -static float get_land_descent_speed() +float Copter::get_land_descent_speed() { #if CONFIG_SONAR == ENABLED bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good); @@ -195,14 +197,14 @@ static float get_land_descent_speed() // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch // called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS // has no effect if we are not already in LAND mode -static void land_do_not_use_GPS() +void Copter::land_do_not_use_GPS() { land_with_gps = false; } // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts // this is always called from a failsafe so we trigger notification to pilot -static void set_mode_land_with_pause() +void Copter::set_mode_land_with_pause() { set_mode(LAND); land_pause = true; @@ -212,6 +214,6 @@ static void set_mode_land_with_pause() } // landing_with_GPS - returns true if vehicle is landing using GPS -static bool landing_with_GPS() { +bool Copter::landing_with_GPS() { return (control_mode == LAND && land_with_gps); } diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index d8d18230aa..d0dd5a4ace 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_loiter.pde - init and run calls for loiter flight mode */ // loiter_init - initialise loiter controller -static bool loiter_init(bool ignore_checks) +bool Copter::loiter_init(bool ignore_checks) { if (position_ok() || optflow_position_ok() || ignore_checks) { @@ -27,7 +29,7 @@ static bool loiter_init(bool ignore_checks) // loiter_run - runs the loiter controller // should be called at 100hz or more -static void loiter_run() +void Copter::loiter_run() { float target_yaw_rate = 0; float target_climb_rate = 0; diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index aef4e02ba8..54bd35609c 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #if POSHOLD_ENABLED == ENABLED /* @@ -23,15 +25,6 @@ #define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s -// declare some function to keep compiler happy -static void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); -static int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); -static void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); -static void poshold_update_wind_comp_estimate(); -static void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); -static void poshold_roll_controller_to_pilot_override(); -static void poshold_pitch_controller_to_pilot_override(); - // mission state enumeration enum poshold_rp_mode { POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch) @@ -82,7 +75,7 @@ static struct { } poshold; // poshold_init - initialise PosHold controller -static bool poshold_init(bool ignore_checks) +bool Copter::poshold_init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock if (!position_ok() && !ignore_checks) { @@ -130,7 +123,7 @@ static bool poshold_init(bool ignore_checks) // poshold_run - runs the PosHold controller // should be called at 100hz or more -static void poshold_run() +void Copter::poshold_run() { float target_roll, target_pitch; // pilot's roll and pitch angle inputs float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec @@ -524,7 +517,7 @@ static void poshold_run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -static void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { @@ -546,7 +539,7 @@ static void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &l // poshold_mix_controls - mixes two controls based on the mix_ratio // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -static int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) { mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); @@ -555,7 +548,7 @@ static int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int1 // poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain // brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max // velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -static void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void Copter::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -581,7 +574,7 @@ static void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float // poshold_update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -static void poshold_update_wind_comp_estimate() +void Copter::poshold_update_wind_comp_estimate() { // check wind estimate start has not been delayed if (poshold.wind_comp_start_timer > 0) { @@ -617,7 +610,7 @@ static void poshold_update_wind_comp_estimate() // poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles // should be called at the maximum loop rate -static void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz poshold.wind_comp_timer++; @@ -632,7 +625,7 @@ static void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitc } // poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -static void poshold_roll_controller_to_pilot_override() +void Copter::poshold_roll_controller_to_pilot_override() { poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; @@ -643,7 +636,7 @@ static void poshold_roll_controller_to_pilot_override() } // poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -static void poshold_pitch_controller_to_pilot_override() +void Copter::poshold_pitch_controller_to_pilot_override() { poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index cdff50461b..2a336d7c0c 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_rtl.pde - init and run calls for RTL flight mode * @@ -8,7 +10,7 @@ */ // rtl_init - initialise rtl controller -static bool rtl_init(bool ignore_checks) +bool Copter::rtl_init(bool ignore_checks) { if (position_ok() || ignore_checks) { rtl_climb_start(); @@ -20,7 +22,7 @@ static bool rtl_init(bool ignore_checks) // rtl_run - runs the return-to-launch controller // should be called at 100hz or more -static void rtl_run() +void Copter::rtl_run() { // check if we need to move to next state if (rtl_state_complete) { @@ -73,7 +75,7 @@ static void rtl_run() } // rtl_climb_start - initialise climb to RTL altitude -static void rtl_climb_start() +void Copter::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; @@ -104,7 +106,7 @@ static void rtl_climb_start() } // rtl_return_start - initialise return to home -static void rtl_return_start() +void Copter::rtl_return_start() { rtl_state = RTL_ReturnHome; rtl_state_complete = false; @@ -129,7 +131,7 @@ static void rtl_return_start() // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -static void rtl_climb_return_run() +void Copter::rtl_climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -169,7 +171,7 @@ static void rtl_climb_return_run() } // rtl_return_start - initialise return to home -static void rtl_loiterathome_start() +void Copter::rtl_loiterathome_start() { rtl_state = RTL_LoiterAtHome; rtl_state_complete = false; @@ -185,7 +187,7 @@ static void rtl_loiterathome_start() // rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -static void rtl_loiterathome_run() +void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { @@ -235,7 +237,7 @@ static void rtl_loiterathome_run() } // rtl_descent_start - initialise descent to final alt -static void rtl_descent_start() +void Copter::rtl_descent_start() { rtl_state = RTL_FinalDescent; rtl_state_complete = false; @@ -252,7 +254,7 @@ static void rtl_descent_start() // rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more -static void rtl_descent_run() +void Copter::rtl_descent_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; @@ -298,7 +300,7 @@ static void rtl_descent_run() } // rtl_loiterathome_start - initialise controllers to loiter over home -static void rtl_land_start() +void Copter::rtl_land_start() { rtl_state = RTL_Land; rtl_state_complete = false; @@ -315,7 +317,7 @@ static void rtl_land_start() // rtl_returnhome_run - return home // called by rtl_run at 100hz or more -static void rtl_land_run() +void Copter::rtl_land_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; @@ -385,7 +387,7 @@ static void rtl_land_run() // get_RTL_alt - return altitude which vehicle should return home at // altitude is in cm above home -static float get_RTL_alt() +float Copter::get_RTL_alt() { // maximum of current altitude and rtl altitude float rtl_alt = max(current_loc.alt, g.rtl_altitude); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index ee333c3dbc..fba8ae8642 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_sport.pde - init and run calls for sport flight mode */ // sport_init - initialise sport controller -static bool sport_init(bool ignore_checks) +bool Copter::sport_init(bool ignore_checks) { // initialize vertical speed and accelerationj pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); @@ -19,7 +21,7 @@ static bool sport_init(bool ignore_checks) // sport_run - runs the sport controller // should be called at 100hz or more -static void sport_run() +void Copter::sport_run() { float target_roll_rate, target_pitch_rate, target_yaw_rate; float target_climb_rate = 0; diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index 743def3463..4a10e06192 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -1,11 +1,13 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * control_stabilize.pde - init and run calls for stabilize flight mode */ // stabilize_init - initialise stabilize controller -static bool stabilize_init(bool ignore_checks) +bool Copter::stabilize_init(bool ignore_checks) { // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? @@ -17,7 +19,7 @@ static bool stabilize_init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -static void stabilize_run() +void Copter::stabilize_run() { float target_roll, target_pitch; float target_yaw_rate; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 45b3389526..d2422a01ba 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // Code to detect a crash main ArduCopter code #ifndef CRASH_CHECK_ITERATIONS_MAX # define CRASH_CHECK_ITERATIONS_MAX 20 // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash @@ -14,7 +16,7 @@ // crash_check - disarms motors if a crash has been detected // crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second // should be called at 10hz -void crash_check() +void Copter::crash_check() { static uint8_t inverted_count; // number of iterations we have been inverted static int32_t baro_alt_prev; @@ -78,7 +80,7 @@ void crash_check() // parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected // vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second // should be called at 10hz -void parachute_check() +void Copter::parachute_check() { static uint8_t control_loss_count; // number of iterations we have been out of control static int32_t baro_alt_start; @@ -154,7 +156,7 @@ void parachute_check() } // parachute_release - trigger the release of the parachute, disarm the motors and notify the user -static void parachute_release() +void Copter::parachute_release() { // send message to gcs and dataflash gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Released!")); @@ -169,7 +171,7 @@ static void parachute_release() // parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error // checks if the vehicle is landed -static void parachute_manual_release() +void Copter::parachute_manual_release() { // exit immediately if parachute is not enabled if (!parachute.enabled()) { diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index fa3161a907..ad191fd44c 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /** * * ekf_check.pde - detects failures of the ekf or inertial nav system @@ -26,7 +28,7 @@ static struct { // ekf_check - detects if ekf variance are out of tolerance and triggers failsafe // should be called at 10hz -void ekf_check() +void Copter::ekf_check() { // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected if (!motors.armed() || ap.usb_connected || (g.ekfcheck_thresh <= 0.0f)) { @@ -81,7 +83,7 @@ void ekf_check() } // ekf_over_threshold - returns true if the ekf's variance are over the tolerance -static bool ekf_over_threshold() +bool Copter::ekf_over_threshold() { #if AP_AHRS_NAVEKF_AVAILABLE // return false immediately if disabled @@ -112,7 +114,7 @@ static bool ekf_over_threshold() // failsafe_ekf_event - perform ekf failsafe -static void failsafe_ekf_event() +void Copter::failsafe_ekf_event() { // return immediately if ekf failsafe already triggered if (failsafe.ekf) { @@ -140,7 +142,7 @@ static void failsafe_ekf_event() } // failsafe_ekf_off_event - actions to take when EKF failsafe is cleared -static void failsafe_ekf_off_event(void) +void Copter::failsafe_ekf_off_event(void) { // return immediately if not in ekf failsafe if (!failsafe.ekf) { diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 306df01f97..e9fa650465 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /***************************************************************************** * esc_calibration.pde : functions to check and perform ESC calibration *****************************************************************************/ @@ -15,7 +17,7 @@ enum ESCCalibrationModes { }; // check if we should enter esc calibration mode -static void esc_calibration_startup_check() +void Copter::esc_calibration_startup_check() { // exit immediately if pre-arm rc checks fail pre_arm_rc_checks(); @@ -66,7 +68,7 @@ static void esc_calibration_startup_check() } // esc_calibration_passthrough - pass through pilot throttle to escs -static void esc_calibration_passthrough() +void Copter::esc_calibration_passthrough() { // clear esc flag for next time g.esc_calibrate.set_and_save(ESCCAL_NONE); @@ -95,7 +97,7 @@ static void esc_calibration_passthrough() } // esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input -static void esc_calibration_auto() +void Copter::esc_calibration_auto() { bool printed_msg = false; diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 3e836b4b28..428795f5d3 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -1,10 +1,12 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * This event will be called when the failsafe changes * boolean failsafe reflects the current state */ -static void failsafe_radio_on_event() +void Copter::failsafe_radio_on_event() { // if motors are not armed there is nothing to do if( !motors.armed() ) { @@ -91,14 +93,14 @@ static void failsafe_radio_on_event() // failsafe_off_event - respond to radio contact being regained // we must be in AUTO, LAND or RTL modes // or Stabilize or ACRO mode but with motors disarmed -static void failsafe_radio_off_event() +void Copter::failsafe_radio_off_event() { // no need to do anything except log the error as resolved // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED); } -static void failsafe_battery_event(void) +void Copter::failsafe_battery_event(void) { // return immediately if low battery event has already been triggered if (failsafe.battery) { @@ -163,7 +165,7 @@ static void failsafe_battery_event(void) } // failsafe_gcs_check - check for ground station failsafe -static void failsafe_gcs_check() +void Copter::failsafe_gcs_check() { uint32_t last_gcs_update_ms; @@ -251,7 +253,7 @@ static void failsafe_gcs_check() } // failsafe_gcs_off_event - actions to take when GCS contact is restored -static void failsafe_gcs_off_event(void) +void Copter::failsafe_gcs_off_event(void) { // log recovery of GCS in logs? Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); @@ -259,7 +261,7 @@ static void failsafe_gcs_off_event(void) // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts // this is always called from a failsafe so we trigger notification to pilot -static void set_mode_RTL_or_land_with_pause() +void Copter::set_mode_RTL_or_land_with_pause() { // attempt to switch to RTL, if this fails then switch to Land if (!set_mode(RTL)) { @@ -271,7 +273,7 @@ static void set_mode_RTL_or_land_with_pause() } } -static void update_events() +void Copter::update_events() { ServoRelayEvents.update_events(); } diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 8e6d17b3b4..7b16d5b954 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -1,4 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + // // failsafe support // Andrew Tridgell, December 2011 @@ -14,7 +17,7 @@ static bool in_failsafe; // // failsafe_enable - enable failsafe // -void failsafe_enable() +void Copter::failsafe_enable() { failsafe_enabled = true; failsafe_last_timestamp = micros(); @@ -23,7 +26,7 @@ void failsafe_enable() // // failsafe_disable - used when we know we are going to delay the mainloop significantly // -void failsafe_disable() +void Copter::failsafe_disable() { failsafe_enabled = false; } @@ -31,7 +34,7 @@ void failsafe_disable() // // failsafe_check - this function is called from the core timer interrupt at 1kHz. // -void failsafe_check() +void Copter::failsafe_check() { uint32_t tnow = hal.scheduler->micros(); diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index edb7927aae..5d306f9aa5 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -1,12 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // Code to integrate AC_Fence library with main ArduCopter code #if AC_FENCE == ENABLED // fence_check - ask fence library to check for breaches and initiate the response // called at 1hz -void fence_check() +void Copter::fence_check() { uint8_t new_breaches; // the type of fence that has been breached uint8_t orig_breaches = fence.get_breaches(); @@ -56,7 +58,7 @@ void fence_check() } // fence_send_mavlink_status - send fence status to ground station -static void fence_send_mavlink_status(mavlink_channel_t chan) +void Copter::fence_send_mavlink_status(mavlink_channel_t chan) { if (fence.enabled()) { // traslate fence library breach types to mavlink breach types diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 79af1534e4..c3baf57951 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * flight.pde - high level calls to set and update flight modes * logic for individual flight modes is in control_acro.pde, control_stabilize.pde, etc @@ -9,7 +11,7 @@ // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was succesfully set // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately -static bool set_mode(uint8_t mode) +bool Copter::set_mode(uint8_t mode) { // boolean to record if flight mode could be set bool success = false; @@ -127,7 +129,7 @@ static bool set_mode(uint8_t mode) // update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more -static void update_flight_mode() +void Copter::update_flight_mode() { #if AP_AHRS_NAVEKF_AVAILABLE // Update EKF speed limit - used to limit speed when we are using optical flow @@ -209,7 +211,7 @@ static void update_flight_mode() } // exit_mode - high level call to organise cleanup as a flight mode is exited -static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) +void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) { #if AUTOTUNE_ENABLED == ENABLED if (old_control_mode == AUTOTUNE) { @@ -248,7 +250,7 @@ static void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) } // returns true or false whether mode requires GPS -static bool mode_requires_GPS(uint8_t mode) { +bool Copter::mode_requires_GPS(uint8_t mode) { switch(mode) { case AUTO: case GUIDED: @@ -267,7 +269,7 @@ static bool mode_requires_GPS(uint8_t mode) { } // mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle) -static bool mode_has_manual_throttle(uint8_t mode) { +bool Copter::mode_has_manual_throttle(uint8_t mode) { switch(mode) { case ACRO: case STABILIZE: @@ -281,7 +283,7 @@ static bool mode_has_manual_throttle(uint8_t mode) { // mode_allows_arming - returns true if vehicle can be armed in the specified mode // arming_from_gcs should be set to true if the arming request comes from the ground station -static bool mode_allows_arming(uint8_t mode, bool arming_from_gcs) { +bool Copter::mode_allows_arming(uint8_t mode, bool arming_from_gcs) { if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) { return true; } @@ -289,7 +291,7 @@ static bool mode_allows_arming(uint8_t mode, bool arming_from_gcs) { } // notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device -static void notify_flight_mode(uint8_t mode) { +void Copter::notify_flight_mode(uint8_t mode) { switch(mode) { case AUTO: case GUIDED: @@ -309,8 +311,7 @@ static void notify_flight_mode(uint8_t mode) { // // print_flight_mode - prints flight mode to serial port. // -static void -print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) +void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) { switch (mode) { case STABILIZE: diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 0f48d09482..00d1f44ab6 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // Traditional helicopter variables and functions #include "heli.h" @@ -14,13 +16,13 @@ static int8_t heli_dynamic_flight_counter; // heli_init - perform any special initialisation required for the tradheli -static void heli_init() +void Copter::heli_init() { // Nothing in here for now. To-Do: Eliminate this function completely? } // get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the channel_throttle->servo_out function -static int16_t get_pilot_desired_collective(int16_t control_in) +int16_t Copter::get_pilot_desired_collective(int16_t control_in) { // return immediately if reduce collective range for manual flight has not been configured if (g.heli_stab_col_min == 0 && g.heli_stab_col_max == 1000) { @@ -36,7 +38,7 @@ static int16_t get_pilot_desired_collective(int16_t control_in) // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity // should be called at 50hz -static void check_dynamic_flight(void) +void Copter::check_dynamic_flight(void) { if (!motors.armed() || !motors.rotor_runup_complete() || control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) { @@ -80,7 +82,7 @@ static void check_dynamic_flight(void) // update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli. // should be run between the rate controller and the servo updates. -static void update_heli_control_dynamics(void) +void Copter::update_heli_control_dynamics(void) { // Use Leaky_I if we are not moving fast attitude_control.use_leaky_i(!heli_flags.dynamic_flight); @@ -90,7 +92,7 @@ static void update_heli_control_dynamics(void) // heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing // should be called soon after update_land_detector in main code -static void heli_update_landing_swash() +void Copter::heli_update_landing_swash() { switch(control_mode) { case ACRO: @@ -130,7 +132,7 @@ static void heli_update_landing_swash() } // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object -static void heli_update_rotor_speed_targets() +void Copter::heli_update_rotor_speed_targets() { // get rotor control method uint8_t rsc_control_mode = motors.get_rsc_mode(); @@ -156,7 +158,7 @@ static void heli_update_rotor_speed_targets() } // heli_radio_passthrough send RC inputs direct into motors library for use during manual passthrough for helicopter setup -static void heli_radio_passthrough() +void Copter::heli_radio_passthrough() { motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in); } diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index 27f15411ee..4f9277dd2f 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -1,11 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + #if FRAME_CONFIG == HELI_FRAME /* * heli_control_acro.pde - init and run calls for acro flight mode for trad heli */ // heli_acro_init - initialise acro controller -static bool heli_acro_init(bool ignore_checks) +bool Copter::heli_acro_init(bool ignore_checks) { // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos attitude_control.use_flybar_passthrough(motors.has_flybar()); @@ -16,7 +19,7 @@ static bool heli_acro_init(bool ignore_checks) // heli_acro_run - runs the acro controller // should be called at 100hz or more -static void heli_acro_run() +void Copter::heli_acro_run() { float target_roll, target_pitch, target_yaw; @@ -57,7 +60,7 @@ static void heli_acro_run() // get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate // returns desired yaw rate in centi-degrees-per-second -static void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out) +void Copter::get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out) { // calculate rate request float rate_bf_yaw_request = yaw_in * g.acro_yaw_p; diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index bee3428b59..f18ee88d28 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -1,11 +1,14 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + #if FRAME_CONFIG == HELI_FRAME /* * heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli */ // stabilize_init - initialise stabilize controller -static bool heli_stabilize_init(bool ignore_checks) +bool Copter::heli_stabilize_init(bool ignore_checks) { // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? @@ -15,7 +18,7 @@ static bool heli_stabilize_init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -static void heli_stabilize_run() +void Copter::heli_stabilize_run() { float target_roll, target_pitch; float target_yaw_rate; diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index 0c0c07e223..ebcc5b80cf 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -1,14 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // read_inertia - read inertia in from accelerometers -static void read_inertia() +void Copter::read_inertia() { // inertial altitude estimates inertial_nav.update(G_Dt); } // read_inertial_altitude - pull altitude and climb rate from inertial nav library -static void read_inertial_altitude() +void Copter::read_inertial_altitude() { // exit immediately if we do not have an altitude estimate if (!inertial_nav.get_filter_status().flags.vert_pos) { diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 1bedbb21a3..c2dabea85d 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -1,17 +1,20 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + + // counter to verify landings static uint32_t land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE; // we assume we are landed // land_complete_maybe - return true if we may have landed (used to reset loiter targets during landing) -static bool land_complete_maybe() +bool Copter::land_complete_maybe() { return (ap.land_complete || ap.land_complete_maybe); } // update_land_detector - checks if we have landed and updates the ap.land_complete flag // called at MAIN_LOOP_RATE -static void update_land_detector() +void Copter::update_land_detector() { // land detector can not use the following sensors because they are unreliable during landing // barometer altitude : ground effect can cause errors larger than 4m @@ -64,7 +67,7 @@ static void update_land_detector() // update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle -static void update_throttle_thr_mix() +void Copter::update_throttle_thr_mix() { if (mode_has_manual_throttle(control_mode)) { // manual throttle diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 54674e0aeb..c31ac83ae4 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -1,7 +1,10 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + + // Run landing gear controller at 10Hz -static void landinggear_update(){ +void Copter::landinggear_update(){ // If landing gear control is active, run update function. if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ diff --git a/ArduCopter/leds.cpp b/ArduCopter/leds.cpp index a1dac561fd..f36b0bbebd 100644 --- a/ArduCopter/leds.cpp +++ b/ArduCopter/leds.cpp @@ -1,8 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + + // updates the status of notify // should be called at 50hz -static void update_notify() +void Copter::update_notify() { notify.update(); } diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc new file mode 100644 index 0000000000..834f7ea0ca --- /dev/null +++ b/ArduCopter/make.inc @@ -0,0 +1,65 @@ +LIBRARIES += AP_Common +LIBRARIES += AP_Progmem +LIBRARIES += AP_Menu +LIBRARIES += AP_Param +LIBRARIES += StorageManager +LIBRARIES += AP_HAL +LIBRARIES += AP_HAL_AVR +LIBRARIES += AP_HAL_SITL +LIBRARIES += AP_HAL_PX4 +LIBRARIES += AP_HAL_VRBRAIN +LIBRARIES += AP_HAL_FLYMAPLE +LIBRARIES += AP_HAL_Linux +LIBRARIES += AP_HAL_Empty +LIBRARIES += GCS +LIBRARIES += GCS_MAVLink +LIBRARIES += AP_SerialManager +LIBRARIES += AP_GPS +LIBRARIES += DataFlash +LIBRARIES += AP_ADC +LIBRARIES += AP_ADC_AnalogSource +LIBRARIES += AP_Baro +LIBRARIES += AP_Compass +LIBRARIES += AP_Math +LIBRARIES += AP_Curve +LIBRARIES += AP_InertialSensor +LIBRARIES += AP_AHRS +LIBRARIES += AP_NavEKF +LIBRARIES += AP_Mission +LIBRARIES += AP_Rally +LIBRARIES += AC_PID +LIBRARIES += AC_PI_2D +LIBRARIES += AC_HELI_PID +LIBRARIES += AC_P +LIBRARIES += AC_AttitudeControl +LIBRARIES += AC_AttitudeControl_Heli +LIBRARIES += AC_PosControl +LIBRARIES += RC_Channel +LIBRARIES += AP_Motors +LIBRARIES += AP_RangeFinder +LIBRARIES += AP_OpticalFlow +LIBRARIES += Filter +LIBRARIES += AP_Buffer +LIBRARIES += AP_Relay +LIBRARIES += AP_ServoRelayEvents +LIBRARIES += AP_Camera +LIBRARIES += AP_Mount +LIBRARIES += AP_Airspeed +LIBRARIES += AP_Vehicle +LIBRARIES += AP_InertialNav +LIBRARIES += AC_WPNav +LIBRARIES += AC_Circle +LIBRARIES += AP_Declination +LIBRARIES += AC_Fence +LIBRARIES += SITL +LIBRARIES += AP_Scheduler +LIBRARIES += AP_RCMapper +LIBRARIES += AP_Notify +LIBRARIES += AP_BattMonitor +LIBRARIES += AP_BoardConfig +LIBRARIES += AP_Frsky_Telem +LIBRARIES += AC_Sprayer +LIBRARIES += AP_EPM +LIBRARIES += AP_Parachute +LIBRARIES += AP_LandingGear +LIBRARIES += AP_Terrain diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 3d111bb063..ea7728fe53 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps to ensure proper wiring, rotation. @@ -17,7 +19,7 @@ static uint8_t motor_test_throttle_type = 0; // motor throttle type (0=thrott static uint16_t motor_test_throttle_value = 0; // throttle to be sent to motor, value depends upon it's type // motor_test_output - checks for timeout and sends updates to motors objects -static void motor_test_output() +void Copter::motor_test_output() { // exit immediately if the motor test is not running if (!ap.motor_test) { @@ -67,7 +69,7 @@ static void motor_test_output() // mavlink_motor_test_check - perform checks before motor tests can begin // return true if tests can continue, false if not -static bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) +bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) { // check rc has been calibrated pre_arm_rc_checks(); @@ -94,7 +96,7 @@ static bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) // mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm // returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure -static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec) +uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec) { // if test has not started try to start it if (!ap.motor_test) { @@ -139,7 +141,7 @@ static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_se } // motor_test_stop - stops the motor test -static void motor_test_stop() +void Copter::motor_test_stop() { // exit immediately if the test is not running if (!ap.motor_test) { diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 11d67bcc8e..f87943b62e 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #define ARM_DELAY 20 // called at 10hz so 2 seconds #define DISARM_DELAY 20 // called at 10hz so 2 seconds #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds @@ -11,7 +13,7 @@ static uint8_t auto_disarming_counter; // arm_motors_check - checks for pilot input to arm or disarm the copter // called at 10hz -static void arm_motors_check() +void Copter::arm_motors_check() { static int16_t arming_counter; @@ -71,10 +73,10 @@ static void arm_motors_check() // auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds // called at 1hz -static void auto_disarm_check() +void Copter::auto_disarm_check() { - uint8_t delay; + uint8_t disarm_delay; // exit immediately if we are already disarmed or throttle output is not zero, if (!motors.armed() || !ap.throttle_zero) { @@ -90,12 +92,12 @@ static void auto_disarm_check() // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less // obvious the copter is armed as the motors will not be spinning if (ap.using_interlock || ap.motor_emergency_stop){ - delay = AUTO_DISARMING_DELAY_SHORT; + disarm_delay = AUTO_DISARMING_DELAY_SHORT; } else { - delay = AUTO_DISARMING_DELAY_LONG; + disarm_delay = AUTO_DISARMING_DELAY_LONG; } - if(auto_disarming_counter >= delay) { + if(auto_disarming_counter >= disarm_delay) { init_disarm_motors(); auto_disarming_counter = 0; } @@ -106,7 +108,7 @@ static void auto_disarm_check() // init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure -static bool init_arm_motors(bool arming_from_gcs) +bool Copter::init_arm_motors(bool arming_from_gcs) { // arming marker // Flag used to track if we have armed the motors the first time. @@ -233,7 +235,7 @@ static bool init_arm_motors(bool arming_from_gcs) // perform pre-arm checks and set ap.pre_arm_check flag // return true if the checks pass successfully -static bool pre_arm_checks(bool display_failure) +bool Copter::pre_arm_checks(bool display_failure) { // exit immediately if already armed if (motors.armed()) { @@ -528,7 +530,7 @@ static bool pre_arm_checks(bool display_failure) } // perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag -static void pre_arm_rc_checks() +void Copter::pre_arm_rc_checks() { // exit immediately if we've already successfully performed the pre-arm rc check if( ap.pre_arm_rc_check ) { @@ -571,7 +573,7 @@ static void pre_arm_rc_checks() } // performs pre_arm gps related checks and returns true if passed -static bool pre_arm_gps_checks(bool display_failure) +bool Copter::pre_arm_gps_checks(bool display_failure) { // always check if inertial nav has started and is ready if(!ahrs.get_NavEKF().healthy()) { @@ -638,7 +640,7 @@ static bool pre_arm_gps_checks(bool display_failure) // arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm // has side-effect that logging is started -static bool arm_checks(bool display_failure, bool arming_from_gcs) +bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) { #if LOGGING_ENABLED == ENABLED // start dataflash @@ -760,7 +762,7 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs) } // init_disarm_motors - disarm motors -static void init_disarm_motors() +void Copter::init_disarm_motors() { // return immediately if we are already disarmed if (!motors.armed()) { @@ -806,7 +808,7 @@ static void init_disarm_motors() } // motors_output - send output to motors library which will adjust and send to ESCs and servos -static void motors_output() +void Copter::motors_output() { // check if we are performing the motor test if (ap.motor_test) { @@ -824,7 +826,7 @@ static void motors_output() } // check for pilot stick input to trigger lost vehicle alarm -static void lost_vehicle_check() +void Copter::lost_vehicle_check() { static uint8_t soundalarm_counter; diff --git a/ArduCopter/navigation.cpp b/ArduCopter/navigation.cpp index aa9c513d0a..5ea68206e3 100644 --- a/ArduCopter/navigation.cpp +++ b/ArduCopter/navigation.cpp @@ -1,9 +1,11 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // run_nav_updates - top level call for the autopilot // ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions // To-Do - rename and move this function to make it's purpose more clear -static void run_nav_updates(void) +void Copter::run_nav_updates(void) { // fetch position from inertial navigation calc_position(); @@ -16,7 +18,7 @@ static void run_nav_updates(void) } // calc_position - get lat and lon positions from inertial nav library -static void calc_position() +void Copter::calc_position() { // pull position from interial nav library current_loc.lng = inertial_nav.get_longitude(); @@ -24,7 +26,7 @@ static void calc_position() } // calc_distance_and_bearing - calculate distance and bearing to next waypoint and home -static void calc_distance_and_bearing() +void Copter::calc_distance_and_bearing() { calc_wp_distance(); calc_wp_bearing(); @@ -32,7 +34,7 @@ static void calc_distance_and_bearing() } // calc_wp_distance - calculate distance to next waypoint for reporting and autopilot decisions -static void calc_wp_distance() +void Copter::calc_wp_distance() { // get target from loiter or wpinav controller if (control_mode == LOITER || control_mode == CIRCLE) { @@ -45,7 +47,7 @@ static void calc_wp_distance() } // calc_wp_bearing - calculate bearing to next waypoint for reporting and autopilot decisions -static void calc_wp_bearing() +void Copter::calc_wp_bearing() { // get target from loiter or wpinav controller if (control_mode == LOITER || control_mode == CIRCLE) { @@ -58,7 +60,7 @@ static void calc_wp_bearing() } // calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions -static void calc_home_distance_and_bearing() +void Copter::calc_home_distance_and_bearing() { Vector3f curr = inertial_nav.get_position(); @@ -74,7 +76,7 @@ static void calc_home_distance_and_bearing() } // run_autopilot - highest level call to process mission commands -static void run_autopilot() +void Copter::run_autopilot() { if (control_mode == AUTO) { // update state of mission diff --git a/ArduCopter/perf_info.cpp b/ArduCopter/perf_info.cpp index faeac38de9..7da62c69f3 100644 --- a/ArduCopter/perf_info.cpp +++ b/ArduCopter/perf_info.cpp @@ -1,4 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + // // high level performance monitoring // @@ -15,7 +18,7 @@ static uint16_t perf_info_long_running; static bool perf_ignore_loop = false; // perf_info_reset - reset all records of loop time to zero -static void perf_info_reset() +void Copter::perf_info_reset() { perf_info_loop_count = 0; perf_info_max_time = 0; @@ -24,13 +27,13 @@ static void perf_info_reset() } // perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming) -static void perf_ignore_this_loop() +void Copter::perf_ignore_this_loop() { perf_ignore_loop = true; } // perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold -static void perf_info_check_loop_time(uint32_t time_in_micros) +void Copter::perf_info_check_loop_time(uint32_t time_in_micros) { perf_info_loop_count++; @@ -52,25 +55,25 @@ static void perf_info_check_loop_time(uint32_t time_in_micros) } // perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops -static uint16_t perf_info_get_num_loops() +uint16_t Copter::perf_info_get_num_loops() { return perf_info_loop_count; } // perf_info_get_max_time - return maximum loop time (in microseconds) -static uint32_t perf_info_get_max_time() +uint32_t Copter::perf_info_get_max_time() { return perf_info_max_time; } // perf_info_get_max_time - return maximum loop time (in microseconds) -static uint32_t perf_info_get_min_time() +uint32_t Copter::perf_info_get_min_time() { return perf_info_min_time; } // perf_info_get_num_long_running - get number of long running loops -static uint16_t perf_info_get_num_long_running() +uint16_t Copter::perf_info_get_num_long_running() { return perf_info_long_running; } diff --git a/ArduCopter/position_vector.cpp b/ArduCopter/position_vector.cpp index 0c3d030e9a..4d81b8ea20 100644 --- a/ArduCopter/position_vector.cpp +++ b/ArduCopter/position_vector.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // position_vector.pde related utility functions // position vectors are Vector3f @@ -8,7 +10,7 @@ // .z = altitude above home in cm // pv_location_to_vector - convert lat/lon coordinates to a position vector -Vector3f pv_location_to_vector(const Location& loc) +Vector3f Copter::pv_location_to_vector(const Location& loc) { const struct Location &origin = inertial_nav.get_origin(); float alt_above_origin = pv_alt_above_origin(loc.alt); // convert alt-relative-to-home to alt-relative-to-origin @@ -17,7 +19,7 @@ Vector3f pv_location_to_vector(const Location& loc) // pv_location_to_vector_with_default - convert lat/lon coordinates to a position vector, // defaults to the default_posvec's lat/lon and alt if the provided lat/lon or alt are zero -Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec) +Vector3f Copter::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec) { Vector3f pos = pv_location_to_vector(loc); @@ -36,21 +38,21 @@ Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& } // pv_alt_above_origin - convert altitude above home to altitude above EKF origin -float pv_alt_above_origin(float alt_above_home_cm) +float Copter::pv_alt_above_origin(float alt_above_home_cm) { const struct Location &origin = inertial_nav.get_origin(); return alt_above_home_cm + (ahrs.get_home().alt - origin.alt); } // pv_alt_above_home - convert altitude above EKF origin to altitude above home -float pv_alt_above_home(float alt_above_origin_cm) +float Copter::pv_alt_above_home(float alt_above_origin_cm) { const struct Location &origin = inertial_nav.get_origin(); return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt); } // pv_get_bearing_cd - return bearing in centi-degrees between two positions -float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination) +float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination) { float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100; if (bearing < 0) { @@ -60,7 +62,7 @@ float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination) } // pv_get_horizontal_distance_cm - return distance between two positions in cm -float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination) +float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination) { return pythagorous2(destination.x-origin.x,destination.y-origin.y); } diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index de6bab0030..8f1c2f921a 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -1,9 +1,12 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + + // Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static void default_dead_zones() +void Copter::default_dead_zones() { channel_roll->set_default_dead_zone(30); channel_pitch->set_default_dead_zone(30); @@ -18,7 +21,7 @@ static void default_dead_zones() g.rc_6.set_default_dead_zone(0); } -static void init_rc_in() +void Copter::init_rc_in() { channel_roll = RC_Channel::rc_channel(rcmap.roll()-1); channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1); @@ -49,7 +52,7 @@ static void init_rc_in() } // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration -static void init_rc_out() +void Copter::init_rc_out() { motors.set_update_rate(g.rc_speed); motors.set_frame_orientation(g.frame_orientation); @@ -79,14 +82,14 @@ static void init_rc_out() } // enable_motor_output() - enable and output lowest possible value to motors -void enable_motor_output() +void Copter::enable_motor_output() { // enable motors motors.enable(); motors.output_min(); } -static void read_radio() +void Copter::read_radio() { static uint32_t last_update_ms = 0; uint32_t tnow_ms = millis(); @@ -118,7 +121,7 @@ static void read_radio() } #define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value -static void set_throttle_and_failsafe(uint16_t throttle_pwm) +void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm) { // if failsafe not enabled pass through throttle and exit if(g.failsafe_throttle == FS_THR_DISABLED) { @@ -164,7 +167,7 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm) // throttle_zero is used to determine if the pilot intends to shut down the motors // Basically, this signals when we are not flying. We are either on the ground // or the pilot has shut down the copter in the air and it is free-falling -static void set_throttle_zero_flag(int16_t throttle_control) +void Copter::set_throttle_zero_flag(int16_t throttle_control) { static uint32_t last_nonzero_throttle_ms = 0; uint32_t tnow_ms = millis(); diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 618d97ddd3..4713518814 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -1,6 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void init_barometer(bool full_calibration) +#include "Copter.h" + +void Copter::init_barometer(bool full_calibration) { gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); if (full_calibration) { @@ -12,7 +14,7 @@ static void init_barometer(bool full_calibration) } // return barometric altitude in centimeters -static void read_barometer(void) +void Copter::read_barometer(void) { barometer.update(); if (should_log(MASK_LOG_IMU)) { @@ -25,14 +27,14 @@ static void read_barometer(void) } #if CONFIG_SONAR == ENABLED -static void init_sonar(void) +void Copter::init_sonar(void) { sonar.init(); } #endif // return sonar altitude in centimeters -static int16_t read_sonar(void) +int16_t Copter::read_sonar(void) { #if CONFIG_SONAR == ENABLED sonar.update(); @@ -68,7 +70,7 @@ static int16_t read_sonar(void) } // initialise compass -static void init_compass() +void Copter::init_compass() { if (!compass.init() || !compass.read()) { // make sure we don't pass a broken compass to DCM @@ -80,7 +82,7 @@ static void init_compass() } // initialise optical flow sensor -static void init_optflow() +void Copter::init_optflow() { #if OPTFLOW == ENABLED // exit immediately if not enabled @@ -95,7 +97,7 @@ static void init_optflow() // called at 200hz #if OPTFLOW == ENABLED -static void update_optical_flow(void) +void Copter::update_optical_flow(void) { static uint32_t last_of_update = 0; @@ -123,7 +125,7 @@ static void update_optical_flow(void) // read_battery - check battery voltage and current and invoke failsafe if necessary // called at 10hz -static void read_battery(void) +void Copter::read_battery(void) { battery.read(); @@ -154,7 +156,7 @@ static void read_battery(void) // read the receiver RSSI as an 8 bit number for MAVLink // RC_CHANNELS_SCALED message -void read_receiver_rssi(void) +void Copter::read_receiver_rssi(void) { // avoid divide by zero if (g.rssi_range <= 0) { @@ -168,7 +170,7 @@ void read_receiver_rssi(void) #if EPM_ENABLED == ENABLED // epm update - moves epm pwm output back to neutral after grab or release is completed -void epm_update() +void Copter::epm_update() { epm.update(); } diff --git a/ArduCopter/setup.cpp b/ArduCopter/setup.cpp index b4857b9f09..a003d13a03 100644 --- a/ArduCopter/setup.cpp +++ b/ArduCopter/setup.cpp @@ -1,28 +1,20 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #if CLI_ENABLED == ENABLED #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #define WITH_ESC_CALIB #endif -// Functions called from the setup menu -static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); -static int8_t setup_show (uint8_t argc, const Menu::arg *argv); -static int8_t setup_set (uint8_t argc, const Menu::arg *argv); -#ifdef WITH_ESC_CALIB -static int8_t esc_calib (uint8_t argc, const Menu::arg *argv); -#endif - // Command/function table for the setup menu -const struct Menu::command setup_menu_commands[] PROGMEM = { - // command function called - // ======= =============== - {"reset", setup_factory}, - {"show", setup_show}, - {"set", setup_set}, +static const struct Menu::command setup_menu_commands[] PROGMEM = { + {"reset", MENU_FUNC(setup_factory)}, + {"show", MENU_FUNC(setup_show)}, + {"set", MENU_FUNC(setup_set)}, #ifdef WITH_ESC_CALIB - {"esc_calib", esc_calib}, + {"esc_calib", MENU_FUNC(esc_calib)}, #endif }; @@ -30,8 +22,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. -static int8_t -setup_mode(uint8_t argc, const Menu::arg *argv) +int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance cliSerial->printf_P(PSTR("Setup Mode\n\n\n")); @@ -43,8 +34,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv) // Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). // Called by the setup menu 'factoryreset' command. -static int8_t -setup_factory(uint8_t argc, const Menu::arg *argv) +int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv) { int16_t c; @@ -70,7 +60,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) //Set a parameter to a specified value. It will cast the value to the current type of the //parameter and make sure it fits in case of INT8 and INT16 -static int8_t setup_set(uint8_t argc, const Menu::arg *argv) +int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv) { int8_t value_int8; int16_t value_int16; @@ -129,8 +119,7 @@ static int8_t setup_set(uint8_t argc, const Menu::arg *argv) // Print the current configuration. // Called by the setup menu 'show' command. -static int8_t -setup_show(uint8_t argc, const Menu::arg *argv) +int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv) { AP_Param *param; ap_var_type type; @@ -175,8 +164,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) #define PWM_HIGHEST_MIN 1800 #define PWM_LOWEST_MIN 800 -static int8_t -esc_calib(uint8_t argc,const Menu::arg *argv) +int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) { @@ -318,7 +306,7 @@ esc_calib(uint8_t argc,const Menu::arg *argv) // CLI reports /***************************************************************************/ -static void report_batt_monitor() +void Copter::report_batt_monitor() { cliSerial->printf_P(PSTR("\nBatt Mon:\n")); print_divider(); @@ -332,7 +320,7 @@ static void report_batt_monitor() print_blanks(2); } -static void report_frame() +void Copter::report_frame() { cliSerial->printf_P(PSTR("Frame\n")); print_divider(); @@ -354,7 +342,7 @@ static void report_frame() print_blanks(2); } -static void report_radio() +void Copter::report_radio() { cliSerial->printf_P(PSTR("Radio\n")); print_divider(); @@ -363,7 +351,7 @@ static void report_radio() print_blanks(2); } -static void report_ins() +void Copter::report_ins() { cliSerial->printf_P(PSTR("INS\n")); print_divider(); @@ -373,7 +361,7 @@ static void report_ins() print_blanks(2); } -static void report_flight_modes() +void Copter::report_flight_modes() { cliSerial->printf_P(PSTR("Flight modes\n")); print_divider(); @@ -384,7 +372,7 @@ static void report_flight_modes() print_blanks(2); } -void report_optflow() +void Copter::report_optflow() { #if OPTFLOW == ENABLED cliSerial->printf_P(PSTR("OptFlow\n")); @@ -400,8 +388,7 @@ void report_optflow() // CLI utilities /***************************************************************************/ -static void -print_radio_values() +void Copter::print_radio_values() { cliSerial->printf_P(PSTR("CH1: %d | %d\n"), (int)channel_roll->radio_min, (int)channel_roll->radio_max); cliSerial->printf_P(PSTR("CH2: %d | %d\n"), (int)channel_pitch->radio_min, (int)channel_pitch->radio_max); @@ -413,8 +400,7 @@ print_radio_values() cliSerial->printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); } -static void -print_switch(uint8_t p, uint8_t m, bool b) +void Copter::print_switch(uint8_t p, uint8_t m, bool b) { cliSerial->printf_P(PSTR("Pos %d:\t"),p); print_flight_mode(cliSerial, m); @@ -425,8 +411,7 @@ print_switch(uint8_t p, uint8_t m, bool b) cliSerial->printf_P(PSTR("OFF\n")); } -static void -print_accel_offsets_and_scaling(void) +void Copter::print_accel_offsets_and_scaling(void) { const Vector3f &accel_offsets = ins.get_accel_offsets(); const Vector3f &accel_scale = ins.get_accel_scale(); @@ -439,8 +424,7 @@ print_accel_offsets_and_scaling(void) (double)accel_scale.z); // YAW } -static void -print_gyro_offsets(void) +void Copter::print_gyro_offsets(void) { const Vector3f &gyro_offsets = ins.get_gyro_offsets(); cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"), @@ -452,7 +436,7 @@ print_gyro_offsets(void) #endif // CLI_ENABLED // report_compass - displays compass information. Also called by compassmot.pde -static void report_compass() +void Copter::report_compass() { cliSerial->printf_P(PSTR("Compass\n")); print_divider(); @@ -499,8 +483,7 @@ static void report_compass() print_blanks(1); } -static void -print_blanks(int16_t num) +void Copter::print_blanks(int16_t num) { while(num > 0) { num--; @@ -508,8 +491,7 @@ print_blanks(int16_t num) } } -static void -print_divider(void) +void Copter::print_divider(void) { for (int i = 0; i < 40; i++) { cliSerial->print_P(PSTR("-")); @@ -517,7 +499,7 @@ print_divider(void) cliSerial->println(); } -static void print_enabled(bool b) +void Copter::print_enabled(bool b) { if(b) cliSerial->print_P(PSTR("en")); @@ -526,7 +508,7 @@ static void print_enabled(bool b) cliSerial->print_P(PSTR("abled\n")); } -static void report_version() +void Copter::report_version() { cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version); print_divider(); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 8d29942170..22cf660d96 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200 //Documentation of Aux Switch Flags: @@ -16,7 +18,7 @@ static union { uint32_t value; } aux_con; -static void read_control_switch() +void Copter::read_control_switch() { uint32_t tnow_ms = millis(); @@ -73,7 +75,7 @@ static void read_control_switch() } // check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. -static bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check) +bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check) { bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check || g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check; @@ -82,7 +84,7 @@ static bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check) } // check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated -static bool check_duplicate_auxsw(void) +bool Copter::check_duplicate_auxsw(void) { bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option || g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option || @@ -103,14 +105,14 @@ static bool check_duplicate_auxsw(void) return ret; } -static void reset_control_switch() +void Copter::reset_control_switch() { control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1; read_control_switch(); } // read_3pos_switch -static uint8_t read_3pos_switch(int16_t radio_in) +uint8_t Copter::read_3pos_switch(int16_t radio_in) { if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position @@ -118,7 +120,7 @@ static uint8_t read_3pos_switch(int16_t radio_in) } // read_aux_switches - checks aux switch positions and invokes configured actions -static void read_aux_switches() +void Copter::read_aux_switches() { uint8_t switch_position; @@ -193,7 +195,7 @@ static void read_aux_switches() } // init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so -static void init_aux_switches() +void Copter::init_aux_switches() { // set the CH7 ~ CH12 flags aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in); @@ -221,7 +223,7 @@ static void init_aux_switches() } // init_aux_switch_function - initialize aux functions -static void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) +void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) { // init channel options switch(ch_option) { @@ -254,7 +256,7 @@ static void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) } // do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch -static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) +void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) { switch(ch_function) { @@ -588,7 +590,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } // save_trim - adds roll and pitch trims from the radio to ahrs -static void save_trim() +void Copter::save_trim() { // save roll and pitch trim float roll_trim = ToRad((float)channel_roll->control_in/100.0f); @@ -600,7 +602,7 @@ static void save_trim() // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions // meant to be called continuously while the pilot attempts to keep the copter level -static void auto_trim() +void Copter::auto_trim() { if(auto_trim_counter > 0) { auto_trim_counter--; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 916549a513..b1f5c15ad8 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -1,4 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + /***************************************************************************** * The init_ardupilot function processes everything we need for an in - air restart * We will determine later if we are actually on the ground and process a @@ -14,7 +17,7 @@ static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in te static int8_t reboot_board(uint8_t argc, const Menu::arg *argv); // This is the help function -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) +int8_t Copter::main_menu_help(uint8_t argc, const Menu::arg *argv) { cliSerial->printf_P(PSTR("Commands:\n" " logs\n" @@ -29,24 +32,24 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) const struct Menu::command main_menu_commands[] PROGMEM = { // command function called // ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"reboot", reboot_board}, - {"help", main_menu_help}, + {"logs", MENU_FUNC(process_logs)}, + {"setup", MENU_FUNC(setup_mode)}, + {"test", MENU_FUNC(test_mode)}, + {"reboot", MENU_FUNC(reboot_board)}, + {"help", MENU_FUNC(main_menu_help)}, }; // Create the top-level menu object. MENU(main_menu, THISFIRMWARE, main_menu_commands); -static int8_t reboot_board(uint8_t argc, const Menu::arg *argv) +int8_t Copter::reboot_board(uint8_t argc, const Menu::arg *argv) { hal.scheduler->reboot(false); return 0; } // the user wants the CLI. It never exits -static void run_cli(AP_HAL::UARTDriver *port) +void Copter::run_cli(AP_HAL::UARTDriver *port) { cliSerial = port; Menu::set_port(port); @@ -71,7 +74,18 @@ static void run_cli(AP_HAL::UARTDriver *port) #endif // CLI_ENABLED -static void init_ardupilot() +static void mavlink_delay_cb_static() +{ + copter.mavlink_delay_cb(); +} + + +static void failsafe_check_static() +{ + copter.failsafe_check(); +} + +void Copter::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with @@ -121,7 +135,7 @@ static void init_ardupilot() // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. - hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); + hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. @@ -153,15 +167,7 @@ static void init_ardupilot() mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED - DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0])); - if (!DataFlash.CardInserted()) { - gcs_send_text_P(SEVERITY_HIGH, PSTR("No dataflash inserted")); - g.log_bitmask.set(0); - } else if (DataFlash.NeedErase()) { - gcs_send_text_P(SEVERITY_HIGH, PSTR("ERASING LOGS")); - do_erase_logs(); - gcs[0].reset_cli_timeout(); - } + log_init(); #endif init_rc_in(); // sets up rc channels from radio @@ -176,7 +182,7 @@ static void init_ardupilot() * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ - hal.scheduler->register_timer_failsafe(failsafe_check, 1000); + hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // Do GPS init gps.init(&DataFlash, serial_manager); @@ -279,7 +285,7 @@ static void init_ardupilot() //****************************************************************************** //This function does all the calibrations, etc. that we need during a ground start //****************************************************************************** -static void startup_ground(bool force_gyro_cal) +void Copter::startup_ground(bool force_gyro_cal) { gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); @@ -306,7 +312,7 @@ static void startup_ground(bool force_gyro_cal) } // position_ok - returns true if the horizontal absolute position is ok and home position is set -static bool position_ok() +bool Copter::position_ok() { if (!ahrs.have_inertial_nav()) { // do not allow navigation with dcm position @@ -331,7 +337,7 @@ static bool position_ok() } // optflow_position_ok - returns true if optical flow based position estimate is ok -static bool optflow_position_ok() +bool Copter::optflow_position_ok() { #if OPTFLOW != ENABLED return false; @@ -348,7 +354,7 @@ static bool optflow_position_ok() } // update_auto_armed - update status of auto_armed flag -static void update_auto_armed() +void Copter::update_auto_armed() { // disarm checks if(ap.auto_armed){ @@ -378,7 +384,7 @@ static void update_auto_armed() } } -static void check_usb_mux(void) +void Copter::check_usb_mux(void) { bool usb_check = hal.gpio->usb_connected(); if (usb_check == ap.usb_connected) { @@ -392,7 +398,7 @@ static void check_usb_mux(void) // frsky_telemetry_send - sends telemetry data using frsky telemetry // should be called at 5Hz by scheduler #if FRSKY_TELEM_ENABLED == ENABLED -static void frsky_telemetry_send(void) +void Copter::frsky_telemetry_send(void) { frsky_telemetry.send_frames((uint8_t)control_mode); } @@ -401,7 +407,7 @@ static void frsky_telemetry_send(void) /* should we log a message type now? */ -static bool should_log(uint32_t mask) +bool Copter::should_log(uint32_t mask) { #if LOGGING_ENABLED == ENABLED if (!(mask & g.log_bitmask) || in_mavlink_delay) { diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 223ece7c33..7a54ac8a7e 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude // A safe takeoff speed is calculated and used to calculate a time_ms @@ -7,7 +9,7 @@ // return true if this flight mode supports user takeoff // must_nagivate is true if mode must also control horizontal position -bool current_mode_has_user_takeoff(bool must_navigate) +bool Copter::current_mode_has_user_takeoff(bool must_navigate) { switch (control_mode) { case GUIDED: @@ -23,7 +25,7 @@ bool current_mode_has_user_takeoff(bool must_navigate) } // initiate user takeoff - called when MAVLink TAKEOFF command is received -static bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate) +bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) { if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) { switch(control_mode) { @@ -44,7 +46,7 @@ static bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // start takeoff to specified altitude above home -static void takeoff_timer_start(float alt) +void Copter::takeoff_timer_start(float alt) { // calculate climb rate float speed = min(wp_nav.get_speed_up(), max(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); @@ -62,7 +64,7 @@ static void takeoff_timer_start(float alt) } // stop takeoff -static void takeoff_stop() +void Copter::takeoff_stop() { takeoff_state.running = false; takeoff_state.start_ms = 0; @@ -72,7 +74,7 @@ static void takeoff_stop() // pilot_climb_rate is both an input and an output // takeoff_climb_rate is only an output // has side-effect of turning takeoff off when timeout as expired -static void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) +void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) { // return pilot_climb_rate if take-off inactive if (!takeoff_state.running) { diff --git a/ArduCopter/test.cpp b/ArduCopter/test.cpp index 7daf561b0f..9eb8c997ac 100644 --- a/ArduCopter/test.cpp +++ b/ArduCopter/test.cpp @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + #if CLI_ENABLED == ENABLED // These are function definitions so the Menu can be constructed before the functions @@ -22,35 +24,33 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); // and stores them in Flash memory, not RAM. // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details -const struct Menu::command test_menu_commands[] PROGMEM = { +static const struct Menu::command test_menu_commands[] PROGMEM = { #if HIL_MODE == HIL_MODE_DISABLED - {"baro", test_baro}, + {"baro", MENU_FUNC(test_baro)}, #endif - {"compass", test_compass}, - {"ins", test_ins}, - {"optflow", test_optflow}, - {"relay", test_relay}, + {"compass", MENU_FUNC(test_compass)}, + {"ins", MENU_FUNC(test_ins)}, + {"optflow", MENU_FUNC(test_optflow)}, + {"relay", MENU_FUNC(test_relay)}, #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - {"shell", test_shell}, + {"shell", MENU_FUNC(test_shell)}, #endif #if HIL_MODE == HIL_MODE_DISABLED - {"rangefinder", test_sonar}, + {"rangefinder", MENU_FUNC(test_sonar)}, #endif }; // A Macro to create the Menu MENU(test_menu, "test", test_menu_commands); -static int8_t -test_mode(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_mode(uint8_t argc, const Menu::arg *argv) { test_menu.run(); return 0; } #if HIL_MODE == HIL_MODE_DISABLED -static int8_t -test_baro(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); init_barometer(true); @@ -75,8 +75,7 @@ test_baro(uint8_t argc, const Menu::arg *argv) } #endif -static int8_t -test_compass(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv) { uint8_t delta_ms_fast_loop; uint8_t medium_loopCounter = 0; @@ -162,8 +161,7 @@ test_compass(uint8_t argc, const Menu::arg *argv) return (0); } -static int8_t -test_ins(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv) { Vector3f gyro, accel; print_hit_enter(); @@ -196,8 +194,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) } } -static int8_t -test_optflow(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv) { #if OPTFLOW == ENABLED if(optflow.enabled()) { @@ -227,7 +224,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv) #endif // OPTFLOW == ENABLED } -static int8_t test_relay(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_relay(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); delay(1000); @@ -253,8 +250,7 @@ static int8_t test_relay(uint8_t argc, const Menu::arg *argv) /* * run a debug shell */ -static int8_t -test_shell(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv) { hal.util->run_debug_shell(cliSerial); return 0; @@ -265,8 +261,7 @@ test_shell(uint8_t argc, const Menu::arg *argv) /* * test the rangefinders */ -static int8_t -test_sonar(uint8_t argc, const Menu::arg *argv) +int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv) { #if CONFIG_SONAR == ENABLED sonar.init(); @@ -291,7 +286,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) } #endif -static void print_hit_enter() +void Copter::print_hit_enter() { cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n")); } diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 7b1e316a55..c873ef6107 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include "Copter.h" + /* * tuning.pde - function to update various parameters in flight using the ch6 tuning knob * This should not be confused with the AutoTune feature which can bve found in control_autotune.pde @@ -7,7 +9,7 @@ // tuning - updates parameters based on the ch6 tuning knob's position // should be called at 3.3hz -static void tuning() { +void Copter::tuning() { // exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {