Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: finished conversion to .cpp files 

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2015-05-30 12:12:49 +10:00 committed by Randy Mackay
parent 356ece3402
commit 278883c521
57 changed files with 1950 additions and 1451 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/AP_State.cpp
View File

@ -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);
}
}
}

770
ArduCopter/ArduCopter.cpp
View File

@ -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 <math.h>
#include <stdio.h>
#include <stdarg.h>
// Common dependencies
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Menu.h>
#include <AP_Param.h>
#include <StorageManager.h>
// AP_HAL
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_VRBRAIN.h>
#include <AP_HAL_FLYMAPLE.h>
#include <AP_HAL_Linux.h>
#include <AP_HAL_Empty.h>
// Application dependencies
#include <GCS.h>
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager.h> // Serial manager library
#include <AP_GPS.h> // ArduPilot GPS library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
#include <AP_Baro.h>
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS.h>
#include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library
#include <AP_Rally.h> // Rally point library
#include <AC_PID.h> // PID library
#include <AC_PI_2D.h> // PID library (2-axis)
#include <AC_HELI_PID.h> // Heli specific Rate PID library
#include <AC_P.h> // P library
#include <AC_AttitudeControl.h> // Attitude control library
#include <AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_PosControl.h> // Position control library
#include <RC_Channel.h> // RC Channel Library
#include <AP_Motors.h> // AP Motors library
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents.h>
#include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed.h> // needed for AHRS build
#include <AP_Vehicle.h> // needed for AHRS build
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AC_Circle.h> // circle navigation library
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence.h> // Arducopter Fence library
#include <SITL.h> // software in the loop support
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper.h> // RC input mapping library
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_BoardConfig.h> // board configuration library
#include <AP_Frsky_Telem.h>
#if SPRAYER == ENABLED
#include <AC_Sprayer.h> // crop sprayer library
#endif
#if EPM_ENABLED == ENABLED
#include <AP_EPM.h> // EPM cargo gripper stuff
#endif
#if PARACHUTE == ENABLED
#include <AP_Parachute.h> // Parachute release library
#endif
#include <AP_LandingGear.h> // Landing Gear library
#include <AP_Terrain.h>
// 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();

33
ArduCopter/Attitude.cpp
View File

@ -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

96
ArduCopter/Copter.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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;

961
ArduCopter/Copter.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
This is the main Copter class
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <math.h>
#include <stdio.h>
#include <stdarg.h>
// Common dependencies
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Menu.h>
#include <AP_Param.h>
#include <StorageManager.h>
// AP_HAL
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_VRBRAIN.h>
#include <AP_HAL_FLYMAPLE.h>
#include <AP_HAL_Linux.h>
#include <AP_HAL_Empty.h>
// Application dependencies
#include <GCS.h>
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager.h> // Serial manager library
#include <AP_GPS.h> // ArduPilot GPS library
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
#include <AP_Baro.h>
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve.h> // Curve used to linearlise throttle pwm to thrust
#include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS.h>
#include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library
#include <AP_Rally.h> // Rally point library
#include <AC_PID.h> // PID library
#include <AC_PI_2D.h> // PID library (2-axis)
#include <AC_HELI_PID.h> // Heli specific Rate PID library
#include <AC_P.h> // P library
#include <AC_AttitudeControl.h> // Attitude control library
#include <AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_PosControl.h> // Position control library
#include <RC_Channel.h> // RC Channel Library
#include <AP_Motors.h> // AP Motors library
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // Optical Flow library
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents.h>
#include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed.h> // needed for AHRS build
#include <AP_Vehicle.h> // needed for AHRS build
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AC_Circle.h> // circle navigation library
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence.h> // Arducopter Fence library
#include <SITL.h> // software in the loop support
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper.h> // RC input mapping library
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_BoardConfig.h> // board configuration library
#include <AP_Frsky_Telem.h>
#if SPRAYER == ENABLED
#include <AC_Sprayer.h> // crop sprayer library
#endif
#if EPM_ENABLED == ENABLED
#include <AP_EPM.h> // EPM cargo gripper stuff
#endif
#if PARACHUTE == ENABLED
#include <AP_Parachute.h> // Parachute release library
#endif
#include <AP_LandingGear.h> // Landing Gear library
#include <AP_Terrain.h>
// 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;

366
ArduCopter/GCS_Mavlink.cpp
View File

File diff suppressed because it is too large Load Diff

141
ArduCopter/Log.cpp
View File

@ -1,34 +1,29 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Copter.h"
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#if CLI_ENABLED == ENABLED
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static bool print_log_menu(void);
static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options
// 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 log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs}
static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", MENU_FUNC(dump_log)},
{"erase", MENU_FUNC(erase_logs)},
{"enable", MENU_FUNC(select_logs)},
{"disable", MENU_FUNC(select_logs)}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&copter, &Copter::print_log_menu, bool));
static bool
print_log_menu(void)
bool Copter::print_log_menu(void)
{
cliSerial->printf_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<uint16_t>(dump_log) > last_log_num)) {
} else if ((argc != 2) || ((uint16_t)dump_log_num <= (last_log_num - DataFlash.get_num_logs())) || (static_cast<uint16_t>(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

17
ArduCopter/Parameters.cpp
View File

@ -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"));

16
ArduCopter/UserCode.cpp
View File

@ -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
#endif

19
ArduCopter/commands.cpp
View File

@ -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) {

123
ArduCopter/commands_logic.cpp
View File

@ -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);

4
ArduCopter/compassmot.cpp
View File

@ -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

8
ArduCopter/compat.cpp
View File

@ -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();
}

9
ArduCopter/compat.h
View File

@ -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__

8
ArduCopter/control_acro.cpp
View File

@ -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;

6
ArduCopter/control_althold.cpp
View File

@ -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;

54
ArduCopter/control_auto.cpp
View File

@ -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) {

46
ArduCopter/control_autotune.cpp
View File

@ -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;

6
ArduCopter/control_brake.cpp
View File

@ -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;

6
ArduCopter/control_circle.cpp
View File

@ -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;

8
ArduCopter/control_drift.cpp
View File

@ -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

6
ArduCopter/control_flip.cpp
View File

@ -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;

36
ArduCopter/control_guided.cpp
View File

@ -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)) {

18
ArduCopter/control_land.cpp
View File

@ -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);
}

6
ArduCopter/control_loiter.cpp
View File

@ -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;

29
ArduCopter/control_poshold.cpp
View File

@ -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;

26
ArduCopter/control_rtl.cpp
View File

@ -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);

6
ArduCopter/control_sport.cpp
View File

@ -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;

6
ArduCopter/control_stabilize.cpp
View File

@ -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;

10
ArduCopter/crash_check.cpp
View File

@ -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()) {

10
ArduCopter/ekf_check.cpp
View File

@ -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) {

8
ArduCopter/esc_calibration.cpp
View File

@ -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;

16
ArduCopter/events.cpp
View File

@ -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();
}

9
ArduCopter/failsafe.cpp
View File

@ -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();

6
ArduCopter/fence.cpp
View File

@ -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

19
ArduCopter/flight_mode.cpp
View File

@ -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:

16
ArduCopter/heli.cpp
View File

@ -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);
}

9
ArduCopter/heli_control_acro.cpp
View File

@ -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;

7
ArduCopter/heli_control_stabilize.cpp
View File

@ -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;

6
ArduCopter/inertia.cpp
View File

@ -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) {

9
ArduCopter/land_detector.cpp
View File

@ -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

5
ArduCopter/landing_gear.cpp
View File

@ -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)){

5
ArduCopter/leds.cpp
View File

@ -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();
}

65
ArduCopter/make.inc Normal file
View File

@ -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

10
ArduCopter/motor_test.cpp
View File

@ -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) {

30
ArduCopter/motors.cpp
View File

@ -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;

16
ArduCopter/navigation.cpp
View File

@ -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

17
ArduCopter/perf_info.cpp
View File

@ -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;
}

14
ArduCopter/position_vector.cpp
View File

@ -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);
}

17
ArduCopter/radio.cpp
View File

@ -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();

22
ArduCopter/sensors.cpp
View File

@ -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();
}

72
ArduCopter/setup.cpp
View File

@ -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();

24
ArduCopter/switches.cpp
View File

@ -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--;

60
ArduCopter/system.cpp
View File

@ -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) {

12
ArduCopter/takeoff.cpp
View File

@ -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) {

43
ArduCopter/test.cpp
View File

@ -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"));
}

4
ArduCopter/tuning.cpp
View File

@ -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) {