mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
Copter: finished conversion to .cpp files
Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
parent
356ece3402
commit
278883c521
@ -1,7 +1,9 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
// set_home_state - update home state
|
// 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 no change, exit immediately
|
||||||
if (ap.home_state == new_home_state)
|
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)
|
// 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);
|
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 no change, exit immediately
|
||||||
if( ap.auto_armed == b )
|
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(ap.simple_mode != b){
|
||||||
if(b == 0){
|
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
|
// 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;
|
failsafe.battery = b;
|
||||||
AP_Notify::flags.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;
|
failsafe.gcs = b;
|
||||||
}
|
}
|
||||||
|
|
||||||
// ---------------------------------------------
|
// ---------------------------------------------
|
||||||
void set_land_complete(bool b)
|
void Copter::set_land_complete(bool b)
|
||||||
{
|
{
|
||||||
// if no change, exit immediately
|
// if no change, exit immediately
|
||||||
if( ap.land_complete == b )
|
if( ap.land_complete == b )
|
||||||
@ -110,7 +112,7 @@ void set_land_complete(bool b)
|
|||||||
// ---------------------------------------------
|
// ---------------------------------------------
|
||||||
|
|
||||||
// set land complete maybe flag
|
// 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 no change, exit immediately
|
||||||
if (ap.land_complete_maybe == b)
|
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) {
|
if(ap.pre_arm_check != b) {
|
||||||
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) {
|
if(ap.pre_arm_rc_check != b) {
|
||||||
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) {
|
if(ap.using_interlock != b) {
|
||||||
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) {
|
if(ap.motor_emergency_stop != b) {
|
||||||
ap.motor_emergency_stop = b;
|
ap.motor_emergency_stop = b;
|
||||||
|
@ -1,6 +1,5 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
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
|
it under the terms of the GNU General Public License as published by
|
||||||
@ -74,637 +73,9 @@
|
|||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
#include "Copter.h"
|
||||||
// Header includes
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
|
||||||
|
|
||||||
#include <math.h>
|
#define SCHED_TASK(func) FUNCTOR_BIND_VOID(&copter, &Copter::func, void)
|
||||||
#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);
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
|
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
|
4000 = 0.1hz
|
||||||
|
|
||||||
*/
|
*/
|
||||||
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = {
|
||||||
{ rc_loop, 4, 130 }, // 0
|
{ SCHED_TASK(rc_loop), 4, 130 }, // 0
|
||||||
{ throttle_loop, 8, 75 }, // 1
|
{ SCHED_TASK(throttle_loop), 8, 75 }, // 1
|
||||||
{ update_GPS, 8, 200 }, // 2
|
{ SCHED_TASK(update_GPS), 8, 200 }, // 2
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
{ update_optical_flow, 2, 160 }, // 3
|
{ SCHED_TASK(update_optical_flow), 2, 160 }, // 3
|
||||||
#endif
|
#endif
|
||||||
{ update_batt_compass, 40, 120 }, // 4
|
{ SCHED_TASK(update_batt_compass), 40, 120 }, // 4
|
||||||
{ read_aux_switches, 40, 50 }, // 5
|
{ SCHED_TASK(read_aux_switches), 40, 50 }, // 5
|
||||||
{ arm_motors_check, 40, 50 }, // 6
|
{ SCHED_TASK(arm_motors_check), 40, 50 }, // 6
|
||||||
{ auto_trim, 40, 75 }, // 7
|
{ SCHED_TASK(auto_trim), 40, 75 }, // 7
|
||||||
{ update_altitude, 40, 140 }, // 8
|
{ SCHED_TASK(update_altitude), 40, 140 }, // 8
|
||||||
{ run_nav_updates, 8, 100 }, // 9
|
{ SCHED_TASK(run_nav_updates), 8, 100 }, // 9
|
||||||
{ update_thr_average, 4, 90 }, // 10
|
{ SCHED_TASK(update_thr_average), 4, 90 }, // 10
|
||||||
{ three_hz_loop, 133, 75 }, // 11
|
{ SCHED_TASK(three_hz_loop), 133, 75 }, // 11
|
||||||
{ compass_accumulate, 8, 100 }, // 12
|
{ SCHED_TASK(compass_accumulate), 8, 100 }, // 12
|
||||||
{ barometer_accumulate, 8, 90 }, // 13
|
{ SCHED_TASK(barometer_accumulate), 8, 90 }, // 13
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
{ check_dynamic_flight, 8, 75 },
|
{ SCHED_TASK(check_dynamic_flight), 8, 75 },
|
||||||
#endif
|
#endif
|
||||||
{ update_notify, 8, 90 }, // 14
|
{ SCHED_TASK(update_notify), 8, 90 }, // 14
|
||||||
{ one_hz_loop, 400, 100 }, // 15
|
{ SCHED_TASK(one_hz_loop), 400, 100 }, // 15
|
||||||
{ ekf_check, 40, 75 }, // 16
|
{ SCHED_TASK(ekf_check), 40, 75 }, // 16
|
||||||
{ crash_check, 40, 75 }, // 17
|
{ SCHED_TASK(crash_check), 40, 75 }, // 17
|
||||||
{ landinggear_update, 40, 75 }, // 18
|
{ SCHED_TASK(landinggear_update), 40, 75 }, // 18
|
||||||
{ lost_vehicle_check, 40, 50 }, // 19
|
{ SCHED_TASK(lost_vehicle_check), 40, 50 }, // 19
|
||||||
{ gcs_check_input, 1, 180 }, // 20
|
{ SCHED_TASK(gcs_check_input), 1, 180 }, // 20
|
||||||
{ gcs_send_heartbeat, 400, 110 }, // 21
|
{ SCHED_TASK(gcs_send_heartbeat), 400, 110 }, // 21
|
||||||
{ gcs_send_deferred, 8, 550 }, // 22
|
{ SCHED_TASK(gcs_send_deferred), 8, 550 }, // 22
|
||||||
{ gcs_data_stream_send, 8, 550 }, // 23
|
{ SCHED_TASK(gcs_data_stream_send), 8, 550 }, // 23
|
||||||
{ update_mount, 8, 75 }, // 24
|
{ SCHED_TASK(update_mount), 8, 75 }, // 24
|
||||||
{ ten_hz_logging_loop, 40, 350 }, // 25
|
{ SCHED_TASK(ten_hz_logging_loop), 40, 350 }, // 25
|
||||||
{ fifty_hz_logging_loop, 8, 110 }, // 26
|
{ SCHED_TASK(fifty_hz_logging_loop), 8, 110 }, // 26
|
||||||
{ full_rate_logging_loop,1, 100 }, // 27
|
{ SCHED_TASK(full_rate_logging_loop),1, 100 }, // 27
|
||||||
{ perf_update, 4000, 75 }, // 28
|
{ SCHED_TASK(perf_update), 4000, 75 }, // 28
|
||||||
{ read_receiver_rssi, 40, 75 }, // 29
|
{ SCHED_TASK(read_receiver_rssi), 40, 75 }, // 29
|
||||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||||
{ frsky_telemetry_send, 80, 75 }, // 30
|
{ SCHED_TASK(frsky_telemetry_send), 80, 75 }, // 30
|
||||||
#endif
|
#endif
|
||||||
#if EPM_ENABLED == ENABLED
|
#if EPM_ENABLED == ENABLED
|
||||||
{ epm_update, 40, 75 }, // 31
|
{ SCHED_TASK(epm_update), 40, 75 }, // 31
|
||||||
#endif
|
#endif
|
||||||
#ifdef USERHOOK_FASTLOOP
|
#ifdef USERHOOK_FASTLOOP
|
||||||
{ userhook_FastLoop, 4, 75 },
|
{ SCHED_TASK(userhook_FastLoop), 4, 75 },
|
||||||
#endif
|
#endif
|
||||||
#ifdef USERHOOK_50HZLOOP
|
#ifdef USERHOOK_50HZLOOP
|
||||||
{ userhook_50Hz, 8, 75 },
|
{ SCHED_TASK(userhook_50Hz), 8, 75 },
|
||||||
#endif
|
#endif
|
||||||
#ifdef USERHOOK_MEDIUMLOOP
|
#ifdef USERHOOK_MEDIUMLOOP
|
||||||
{ userhook_MediumLoop, 40, 75 },
|
{ SCHED_TASK(userhook_MediumLoop), 40, 75 },
|
||||||
#endif
|
#endif
|
||||||
#ifdef USERHOOK_SLOWLOOP
|
#ifdef USERHOOK_SLOWLOOP
|
||||||
{ userhook_SlowLoop, 120, 75 },
|
{ SCHED_TASK(userhook_SlowLoop), 120, 75 },
|
||||||
#endif
|
#endif
|
||||||
#ifdef USERHOOK_SUPERSLOWLOOP
|
#ifdef USERHOOK_SUPERSLOWLOOP
|
||||||
{ userhook_SuperSlowLoop,400, 75 },
|
{ SCHED_TASK(userhook_SuperSlowLoop),400, 75 },
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
void setup()
|
|
||||||
|
void Copter::setup()
|
||||||
{
|
{
|
||||||
cliSerial = hal.console;
|
cliSerial = hal.console;
|
||||||
|
|
||||||
@ -804,7 +176,7 @@ void setup()
|
|||||||
/*
|
/*
|
||||||
if the compass is enabled then try to accumulate a reading
|
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) {
|
if (g.compass_enabled) {
|
||||||
compass.accumulate();
|
compass.accumulate();
|
||||||
@ -814,12 +186,12 @@ static void compass_accumulate(void)
|
|||||||
/*
|
/*
|
||||||
try to accumulate a baro reading
|
try to accumulate a baro reading
|
||||||
*/
|
*/
|
||||||
static void barometer_accumulate(void)
|
void Copter::barometer_accumulate(void)
|
||||||
{
|
{
|
||||||
barometer.accumulate();
|
barometer.accumulate();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void perf_update(void)
|
void Copter::perf_update(void)
|
||||||
{
|
{
|
||||||
if (should_log(MASK_LOG_PM))
|
if (should_log(MASK_LOG_PM))
|
||||||
Log_Write_Performance();
|
Log_Write_Performance();
|
||||||
@ -834,7 +206,7 @@ static void perf_update(void)
|
|||||||
pmTest1 = 0;
|
pmTest1 = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void Copter::loop()
|
||||||
{
|
{
|
||||||
// wait for an INS sample
|
// wait for an INS sample
|
||||||
ins.wait_for_sample();
|
ins.wait_for_sample();
|
||||||
@ -869,7 +241,7 @@ void loop()
|
|||||||
|
|
||||||
|
|
||||||
// Main loop - 400hz
|
// Main loop - 400hz
|
||||||
static void fast_loop()
|
void Copter::fast_loop()
|
||||||
{
|
{
|
||||||
|
|
||||||
// IMU DCM Algorithm
|
// IMU DCM Algorithm
|
||||||
@ -902,7 +274,7 @@ static void fast_loop()
|
|||||||
|
|
||||||
// rc_loops - reads user input from transmitter/receiver
|
// rc_loops - reads user input from transmitter/receiver
|
||||||
// called at 100hz
|
// called at 100hz
|
||||||
static void rc_loop()
|
void Copter::rc_loop()
|
||||||
{
|
{
|
||||||
// Read radio and 3-position switch on radio
|
// Read radio and 3-position switch on radio
|
||||||
// -----------------------------------------
|
// -----------------------------------------
|
||||||
@ -912,7 +284,7 @@ static void rc_loop()
|
|||||||
|
|
||||||
// throttle_loop - should be run at 50 hz
|
// throttle_loop - should be run at 50 hz
|
||||||
// ---------------------------
|
// ---------------------------
|
||||||
static void throttle_loop()
|
void Copter::throttle_loop()
|
||||||
{
|
{
|
||||||
// get altitude and climb rate from inertial lib
|
// get altitude and climb rate from inertial lib
|
||||||
read_inertial_altitude();
|
read_inertial_altitude();
|
||||||
@ -934,7 +306,7 @@ static void throttle_loop()
|
|||||||
|
|
||||||
// update_mount - update camera mount position
|
// update_mount - update camera mount position
|
||||||
// should be run at 50hz
|
// should be run at 50hz
|
||||||
static void update_mount()
|
void Copter::update_mount()
|
||||||
{
|
{
|
||||||
#if MOUNT == ENABLED
|
#if MOUNT == ENABLED
|
||||||
// update camera mount's position
|
// update camera mount's position
|
||||||
@ -948,7 +320,7 @@ static void update_mount()
|
|||||||
|
|
||||||
// update_batt_compass - read battery and compass
|
// update_batt_compass - read battery and compass
|
||||||
// should be called at 10hz
|
// 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 before compass because it may be used for motor interference compensation
|
||||||
read_battery();
|
read_battery();
|
||||||
@ -966,7 +338,7 @@ static void update_batt_compass(void)
|
|||||||
|
|
||||||
// ten_hz_logging_loop
|
// ten_hz_logging_loop
|
||||||
// should be run at 10hz
|
// 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
|
// 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)) {
|
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
|
// fifty_hz_logging_loop
|
||||||
// should be run at 50hz
|
// should be run at 50hz
|
||||||
static void fifty_hz_logging_loop()
|
void Copter::fifty_hz_logging_loop()
|
||||||
{
|
{
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
#if HIL_MODE != HIL_MODE_DISABLED
|
||||||
// HIL for a copter needs very fast update of the servo values
|
// 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
|
// full_rate_logging_loop
|
||||||
// should be run at the MAIN_LOOP_RATE
|
// 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)) {
|
if (should_log(MASK_LOG_IMU_FAST)) {
|
||||||
DataFlash.Log_Write_IMU(ins);
|
DataFlash.Log_Write_IMU(ins);
|
||||||
@ -1031,7 +403,7 @@ static void full_rate_logging_loop()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// three_hz_loop - 3.3hz 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
|
// check if we've lost contact with the ground station
|
||||||
failsafe_gcs_check();
|
failsafe_gcs_check();
|
||||||
@ -1052,7 +424,7 @@ static void three_hz_loop()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// one_hz_loop - runs at 1Hz
|
// one_hz_loop - runs at 1Hz
|
||||||
static void one_hz_loop()
|
void Copter::one_hz_loop()
|
||||||
{
|
{
|
||||||
if (should_log(MASK_LOG_ANY)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
Log_Write_Data(DATA_AP_STATE, ap.value);
|
Log_Write_Data(DATA_AP_STATE, ap.value);
|
||||||
@ -1108,7 +480,7 @@ static void one_hz_loop()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// called at 50hz
|
// 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
|
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
|
||||||
bool gps_updated = false;
|
bool gps_updated = false;
|
||||||
@ -1145,8 +517,7 @@ static void update_GPS(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::init_simple_bearing()
|
||||||
init_simple_bearing()
|
|
||||||
{
|
{
|
||||||
// capture current cos_yaw and sin_yaw values
|
// capture current cos_yaw and sin_yaw values
|
||||||
simple_cos_yaw = ahrs.cos_yaw();
|
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
|
// 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;
|
float rollx, pitchx;
|
||||||
|
|
||||||
@ -1193,7 +564,7 @@ void update_simple_mode(void)
|
|||||||
|
|
||||||
// update_super_simple_bearing - adjusts simple bearing based on location
|
// update_super_simple_bearing - adjusts simple bearing based on location
|
||||||
// should be called after home_bearing has been updated
|
// 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
|
// 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)) {
|
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
|
// Perform IMU calculations and get attitude info
|
||||||
//-----------------------------------------------
|
//-----------------------------------------------
|
||||||
@ -1220,7 +591,7 @@ static void read_AHRS(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// read baro and sonar altitude at 10hz
|
// read baro and sonar altitude at 10hz
|
||||||
static void update_altitude()
|
void Copter::update_altitude()
|
||||||
{
|
{
|
||||||
// read in baro altitude
|
// read in baro altitude
|
||||||
read_barometer();
|
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();
|
AP_HAL_MAIN();
|
||||||
|
|
||||||
|
@ -1,15 +1,17 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
// 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
|
// 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);
|
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
|
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
|
||||||
// returns desired angle in centi-degrees
|
// 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 angle_max = constrain_float(aparm.angle_max,1000,8000);
|
||||||
float scaler = (float)angle_max/(float)ROLL_PITCH_INPUT_MAX;
|
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
|
// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
|
||||||
// returns desired angle in centi-degrees
|
// returns desired angle in centi-degrees
|
||||||
// To-Do: return heading as a float?
|
// 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
|
// convert pilot input to the desired yaw rate
|
||||||
return stick_angle * g.acro_yaw_p;
|
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
|
// get_roi_yaw - returns heading towards location held in roi_WP
|
||||||
// should be called at 100hz
|
// 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
|
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;
|
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();
|
const Vector3f& vel = inertial_nav.get_velocity();
|
||||||
float speed = pythagorous2(vel.x,vel.y);
|
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)
|
// update_thr_average - update estimated throttle required to hover (if necessary)
|
||||||
// should be called at 100hz
|
// should be called at 100hz
|
||||||
static void update_thr_average()
|
void Copter::update_thr_average()
|
||||||
{
|
{
|
||||||
// ensure throttle_average has been initialised
|
// ensure throttle_average has been initialised
|
||||||
if( is_zero(throttle_average) ) {
|
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
|
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
|
||||||
static void
|
void Copter::set_throttle_takeoff()
|
||||||
set_throttle_takeoff()
|
|
||||||
{
|
{
|
||||||
// tell position controller to reset alt target and reset I terms
|
// tell position controller to reset alt target and reset I terms
|
||||||
pos_control.init_takeoff();
|
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
|
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
|
||||||
// used only for manual throttle modes
|
// used only for manual throttle modes
|
||||||
// returns throttle output 0 to 1000
|
// 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;
|
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
|
// 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
|
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
|
||||||
// without any deadzone at the bottom
|
// 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
|
// throttle failsafe check
|
||||||
if( failsafe.radio ) {
|
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
|
// 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);
|
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;
|
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
|
// get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
|
||||||
// used only for althold, loiter, hybrid flight modes
|
// used only for althold, loiter, hybrid flight modes
|
||||||
// returns throttle output 0 to 1000
|
// 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
|
// exit immediately if input_thr is zero
|
||||||
if (input_thr <= 0.0f) {
|
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
|
// 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
|
// 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;
|
static uint32_t last_call_ms = 0;
|
||||||
float distance_error;
|
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
|
// 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
|
// shift difference between pilot's throttle and hover throttle into accelerometer I
|
||||||
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
|
g.pid_accel_z.set_integrator(pilot_throttle-throttle_average);
|
||||||
}
|
}
|
||||||
|
|
||||||
// updates position controller's maximum altitude using fence and EKF limits
|
// 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
|
float alt_limit_cm = 0.0f; // interpreted as no limit if left as zero
|
||||||
|
|
||||||
|
96
ArduCopter/Copter.cpp
Normal file
96
ArduCopter/Copter.cpp
Normal 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
961
ArduCopter/Copter.h
Normal 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;
|
File diff suppressed because it is too large
Load Diff
@ -1,34 +1,29 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
|
|
||||||
// Code to Write and Read packets from DataFlash log memory
|
// Code to Write and Read packets from DataFlash log memory
|
||||||
// Code to interact with the user to dump or erase logs
|
// Code to interact with the user to dump or erase logs
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#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
|
// Creates a constant array of structs representing menu options
|
||||||
// and stores them in Flash memory, not RAM.
|
// and stores them in Flash memory, not RAM.
|
||||||
// User enters the string in the console to call the functions on the right.
|
// User enters the string in the console to call the functions on the right.
|
||||||
// See class Menu in AP_Coommon for implementation details
|
// See class Menu in AP_Coommon for implementation details
|
||||||
const struct Menu::command log_menu_commands[] PROGMEM = {
|
static const struct Menu::command log_menu_commands[] PROGMEM = {
|
||||||
{"dump", dump_log},
|
{"dump", MENU_FUNC(dump_log)},
|
||||||
{"erase", erase_logs},
|
{"erase", MENU_FUNC(erase_logs)},
|
||||||
{"enable", select_logs},
|
{"enable", MENU_FUNC(select_logs)},
|
||||||
{"disable", select_logs}
|
{"disable", MENU_FUNC(select_logs)}
|
||||||
};
|
};
|
||||||
|
|
||||||
// A Macro to create the Menu
|
// 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
|
bool Copter::print_log_menu(void)
|
||||||
print_log_menu(void)
|
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("logs enabled: "));
|
cliSerial->printf_P(PSTR("logs enabled: "));
|
||||||
|
|
||||||
@ -60,38 +55,36 @@ print_log_menu(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#if CLI_ENABLED == ENABLED
|
||||||
static int8_t
|
int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv)
|
||||||
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_start;
|
||||||
uint16_t dump_log_end;
|
uint16_t dump_log_end;
|
||||||
uint16_t last_log_num;
|
uint16_t last_log_num;
|
||||||
|
|
||||||
// check that the requested log number can be read
|
// 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();
|
last_log_num = DataFlash.find_last_log();
|
||||||
|
|
||||||
if (dump_log == -2) {
|
if (dump_log_num == -2) {
|
||||||
DataFlash.DumpPageInfo(cliSerial);
|
DataFlash.DumpPageInfo(cliSerial);
|
||||||
return(-1);
|
return(-1);
|
||||||
} else if (dump_log <= 0) {
|
} else if (dump_log_num <= 0) {
|
||||||
cliSerial->printf_P(PSTR("dumping all\n"));
|
cliSerial->printf_P(PSTR("dumping all\n"));
|
||||||
Log_Read(0, 1, 0);
|
Log_Read(0, 1, 0);
|
||||||
return(-1);
|
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"));
|
cliSerial->printf_P(PSTR("bad log number\n"));
|
||||||
return(-1);
|
return(-1);
|
||||||
}
|
}
|
||||||
|
|
||||||
DataFlash.get_log_boundaries(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, dump_log_start, dump_log_end);
|
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::erase_logs(uint8_t argc, const Menu::arg *argv)
|
||||||
erase_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
in_mavlink_delay = true;
|
in_mavlink_delay = true;
|
||||||
do_erase_logs();
|
do_erase_logs();
|
||||||
@ -99,8 +92,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv)
|
||||||
select_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
uint16_t bits;
|
uint16_t bits;
|
||||||
|
|
||||||
@ -148,15 +140,14 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||||||
return(0);
|
return(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::process_logs(uint8_t argc, const Menu::arg *argv)
|
||||||
process_logs(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
log_menu.run();
|
log_menu.run();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
#endif // CLI_ENABLED
|
#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"));
|
gcs_send_text_P(SEVERITY_HIGH, PSTR("Erasing logs\n"));
|
||||||
DataFlash.EraseAll();
|
DataFlash.EraseAll();
|
||||||
@ -178,7 +169,7 @@ struct PACKED log_AutoTune {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an Autotune data packet
|
// 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 = {
|
struct log_AutoTune pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG),
|
||||||
@ -203,7 +194,7 @@ struct PACKED log_AutoTuneDetails {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an Autotune data packet
|
// 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 = {
|
struct log_AutoTuneDetails pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG),
|
||||||
@ -216,7 +207,7 @@ static void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Write a Current data packet
|
// 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()));
|
DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle()));
|
||||||
|
|
||||||
@ -235,7 +226,7 @@ struct PACKED log_Optflow {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an optical flow packet
|
// Write an optical flow packet
|
||||||
static void Log_Write_Optflow()
|
void Copter::Log_Write_Optflow()
|
||||||
{
|
{
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
// exit immediately if not enabled
|
// exit immediately if not enabled
|
||||||
@ -273,7 +264,7 @@ struct PACKED log_Nav_Tuning {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an Nav Tuning packet
|
// 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 &pos_target = pos_control.get_pos_target();
|
||||||
const Vector3f &vel_target = pos_control.get_vel_target();
|
const Vector3f &vel_target = pos_control.get_vel_target();
|
||||||
@ -314,7 +305,7 @@ struct PACKED log_Control_Tuning {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write a control tuning packet
|
// Write a control tuning packet
|
||||||
static void Log_Write_Control_Tuning()
|
void Copter::Log_Write_Control_Tuning()
|
||||||
{
|
{
|
||||||
struct log_Control_Tuning pkt = {
|
struct log_Control_Tuning pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
||||||
@ -345,7 +336,7 @@ struct PACKED log_Performance {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write a performance monitoring packet
|
// Write a performance monitoring packet
|
||||||
static void Log_Write_Performance()
|
void Copter::Log_Write_Performance()
|
||||||
{
|
{
|
||||||
struct log_Performance pkt = {
|
struct log_Performance pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
|
||||||
@ -361,7 +352,7 @@ static void Log_Write_Performance()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Write a mission command. Total length : 36 bytes
|
// 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 = {};
|
mavlink_mission_item_t mav_cmd = {};
|
||||||
AP_Mission::mission_cmd_to_mavlink(cmd,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
|
// Write an attitude packet
|
||||||
static void Log_Write_Attitude()
|
void Copter::Log_Write_Attitude()
|
||||||
{
|
{
|
||||||
Vector3f targets = attitude_control.angle_ef_targets();
|
Vector3f targets = attitude_control.angle_ef_targets();
|
||||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||||
@ -406,7 +397,7 @@ struct PACKED log_Rate {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an rate packet
|
// 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 &rate_targets = attitude_control.rate_bf_targets();
|
||||||
const Vector3f &accel_target = pos_control.get_accel_target();
|
const Vector3f &accel_target = pos_control.get_accel_target();
|
||||||
@ -439,7 +430,7 @@ struct PACKED log_MotBatt {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an rate packet
|
// Write an rate packet
|
||||||
static void Log_Write_MotBatt()
|
void Copter::Log_Write_MotBatt()
|
||||||
{
|
{
|
||||||
struct log_MotBatt pkt_mot = {
|
struct log_MotBatt pkt_mot = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
|
||||||
@ -458,7 +449,7 @@ struct PACKED log_Startup {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write Startup packet
|
// Write Startup packet
|
||||||
static void Log_Write_Startup()
|
void Copter::Log_Write_Startup()
|
||||||
{
|
{
|
||||||
struct log_Startup pkt = {
|
struct log_Startup pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
||||||
@ -469,7 +460,7 @@ static void Log_Write_Startup()
|
|||||||
Log_Write_EntireMission();
|
Log_Write_EntireMission();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void Log_Write_EntireMission()
|
void Copter::Log_Write_EntireMission()
|
||||||
{
|
{
|
||||||
DataFlash.Log_Write_Message_P(PSTR("New mission"));
|
DataFlash.Log_Write_Message_P(PSTR("New mission"));
|
||||||
|
|
||||||
@ -488,7 +479,7 @@ struct PACKED log_Event {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Wrote an event packet
|
// 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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Event pkt = {
|
struct log_Event pkt = {
|
||||||
@ -509,7 +500,7 @@ struct PACKED log_Data_Int16t {
|
|||||||
|
|
||||||
// Write an int16_t data packet
|
// Write an int16_t data packet
|
||||||
UNUSED_FUNCTION
|
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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Data_Int16t pkt = {
|
struct log_Data_Int16t pkt = {
|
||||||
@ -531,7 +522,7 @@ struct PACKED log_Data_UInt16t {
|
|||||||
|
|
||||||
// Write an uint16_t data packet
|
// Write an uint16_t data packet
|
||||||
UNUSED_FUNCTION
|
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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Data_UInt16t pkt = {
|
struct log_Data_UInt16t pkt = {
|
||||||
@ -552,7 +543,7 @@ struct PACKED log_Data_Int32t {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an int32_t data packet
|
// 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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Data_Int32t pkt = {
|
struct log_Data_Int32t pkt = {
|
||||||
@ -573,7 +564,7 @@ struct PACKED log_Data_UInt32t {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write a uint32_t data packet
|
// 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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Data_UInt32t pkt = {
|
struct log_Data_UInt32t pkt = {
|
||||||
@ -595,7 +586,7 @@ struct PACKED log_Data_Float {
|
|||||||
|
|
||||||
// Write a float data packet
|
// Write a float data packet
|
||||||
UNUSED_FUNCTION
|
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)) {
|
if (should_log(MASK_LOG_ANY)) {
|
||||||
struct log_Data_Float pkt = {
|
struct log_Data_Float pkt = {
|
||||||
@ -616,7 +607,7 @@ struct PACKED log_Error {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Write an error packet
|
// 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 = {
|
struct log_Error pkt = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
|
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));
|
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void Log_Write_Baro(void)
|
void Copter::Log_Write_Baro(void)
|
||||||
{
|
{
|
||||||
DataFlash.Log_Write_Baro(barometer);
|
DataFlash.Log_Write_Baro(barometer);
|
||||||
}
|
}
|
||||||
@ -642,7 +633,7 @@ struct PACKED log_ParameterTuning {
|
|||||||
int16_t tuning_high; // tuning high end value
|
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 = {
|
struct log_ParameterTuning pkt_tune = {
|
||||||
LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
|
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));
|
DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
|
||||||
}
|
}
|
||||||
|
|
||||||
static const struct LogStructure log_structure[] PROGMEM = {
|
const struct LogStructure Copter::log_structure[] PROGMEM = {
|
||||||
LOG_COMMON_STRUCTURES,
|
LOG_COMMON_STRUCTURES,
|
||||||
#if AUTOTUNE_ENABLED == ENABLED
|
#if AUTOTUNE_ENABLED == ENABLED
|
||||||
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
|
{ LOG_AUTOTUNE_MSG, sizeof(log_AutoTune),
|
||||||
@ -699,7 +690,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
|||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#if CLI_ENABLED == ENABLED
|
||||||
// Read the DataFlash log memory
|
// 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
|
cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
|
||||||
"\nFree RAM: %u\n"
|
"\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));
|
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
|
||||||
|
|
||||||
DataFlash.LogReadProcess(log_num, start_page, end_page,
|
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);
|
cliSerial);
|
||||||
}
|
}
|
||||||
#endif // CLI_ENABLED
|
#endif // CLI_ENABLED
|
||||||
|
|
||||||
// start a new log
|
// start a new log
|
||||||
static void start_logging()
|
void Copter::start_logging()
|
||||||
{
|
{
|
||||||
if (g.log_bitmask != 0) {
|
if (g.log_bitmask != 0) {
|
||||||
if (!ap.logging_started) {
|
if (!ap.logging_started) {
|
||||||
@ -744,33 +735,17 @@ static void start_logging()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#else // LOGGING_ENABLED
|
void Copter::log_init(void)
|
||||||
|
{
|
||||||
static void Log_Write_Startup() {}
|
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
|
||||||
static void Log_Write_EntireMission() {}
|
if (!DataFlash.CardInserted()) {
|
||||||
#if AUTOTUNE_ENABLED == ENABLED
|
gcs_send_text_P(SEVERITY_HIGH, PSTR("No dataflash inserted"));
|
||||||
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) {}
|
g.log_bitmask.set(0);
|
||||||
static void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) {}
|
} else if (DataFlash.NeedErase()) {
|
||||||
#endif
|
gcs_send_text_P(SEVERITY_HIGH, PSTR("ERASING LOGS"));
|
||||||
static void Log_Write_Current() {}
|
do_erase_logs();
|
||||||
static void Log_Write_Attitude() {}
|
gcs[0].reset_cli_timeout();
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // LOGGING_DISABLED
|
#endif // LOGGING_DISABLED
|
||||||
|
@ -1,4 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
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
|
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 GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} }
|
||||||
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.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, &g.v, {group_info : class::var_info} }
|
#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, &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, &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
|
// @Param: SYSID_SW_MREV
|
||||||
// @DisplayName: Eeprom format version number
|
// @DisplayName: Eeprom format version number
|
||||||
// @Description: This value is incremented when changes are made to the eeprom format
|
// @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" },
|
{ 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()) {
|
if (!AP_Param::check_var_info()) {
|
||||||
cliSerial->printf_P(PSTR("Bad var table\n"));
|
cliSerial->printf_P(PSTR("Bad var table\n"));
|
||||||
|
@ -1,7 +1,9 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#ifdef USERHOOK_INIT
|
#ifdef USERHOOK_INIT
|
||||||
void userhook_init()
|
void Copter::userhook_init()
|
||||||
{
|
{
|
||||||
// put your initialisation code here
|
// put your initialisation code here
|
||||||
// this will be called once at start-up
|
// this will be called once at start-up
|
||||||
@ -9,35 +11,35 @@ void userhook_init()
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USERHOOK_FASTLOOP
|
#ifdef USERHOOK_FASTLOOP
|
||||||
void userhook_FastLoop()
|
void Copter::userhook_FastLoop()
|
||||||
{
|
{
|
||||||
// put your 100Hz code here
|
// put your 100Hz code here
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USERHOOK_50HZLOOP
|
#ifdef USERHOOK_50HZLOOP
|
||||||
void userhook_50Hz()
|
void Copter::userhook_50Hz()
|
||||||
{
|
{
|
||||||
// put your 50Hz code here
|
// put your 50Hz code here
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USERHOOK_MEDIUMLOOP
|
#ifdef USERHOOK_MEDIUMLOOP
|
||||||
void userhook_MediumLoop()
|
void Copter::userhook_MediumLoop()
|
||||||
{
|
{
|
||||||
// put your 10Hz code here
|
// put your 10Hz code here
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USERHOOK_SLOWLOOP
|
#ifdef USERHOOK_SLOWLOOP
|
||||||
void userhook_SlowLoop()
|
void Copter::userhook_SlowLoop()
|
||||||
{
|
{
|
||||||
// put your 3.3Hz code here
|
// put your 3.3Hz code here
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USERHOOK_SUPERSLOWLOOP
|
#ifdef USERHOOK_SUPERSLOWLOOP
|
||||||
void userhook_SuperSlowLoop()
|
void Copter::userhook_SuperSlowLoop()
|
||||||
{
|
{
|
||||||
// put your 1Hz code here
|
// put your 1Hz code here
|
||||||
}
|
}
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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)
|
* 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
|
* 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
|
// 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
|
// exit immediately if home already set
|
||||||
if (ap.home_state != HOME_UNSET) {
|
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
|
// 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
|
// get current location from EKF
|
||||||
Location temp_loc;
|
Location temp_loc;
|
||||||
if (inertial_nav.get_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
|
// 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
|
// get current location from EKF
|
||||||
Location temp_loc;
|
Location temp_loc;
|
||||||
if (inertial_nav.get_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
|
// 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()) {
|
if (set_home_to_current_location()) {
|
||||||
set_home_state(HOME_SET_AND_LOCKED);
|
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
|
// 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
|
// 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)) {
|
if (set_home(loc)) {
|
||||||
set_home_state(HOME_SET_AND_LOCKED);
|
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
|
// set_home - sets ahrs home (used for RTL) to specified location
|
||||||
// initialises inertial nav and compass on first call
|
// initialises inertial nav and compass on first call
|
||||||
// returns true if home location set successfully
|
// 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
|
// check location is valid
|
||||||
if (loc.lat == 0 && loc.lng == 0) {
|
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
|
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
||||||
scaleLongDown = longitude_scale(loc);
|
scaleLongDown = longitude_scale(loc);
|
||||||
scaleLongUp = 1.0f/scaleLongDown;
|
|
||||||
// record home is set
|
// record home is set
|
||||||
set_home_state(HOME_SET_NOT_LOCKED);
|
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
|
// far_from_EKF_origin - checks if a location is too far from the EKF origin
|
||||||
// returns true if too far
|
// 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
|
// check distance to EKF origin
|
||||||
const struct Location &ekf_origin = inertial_nav.get_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
|
// 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
|
// exit immediately if system time already set
|
||||||
if (ap.system_time_set) {
|
if (ap.system_time_set) {
|
||||||
|
@ -1,45 +1,9 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
// forward declarations to make compiler happy
|
#include "Copter.h"
|
||||||
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);
|
|
||||||
|
|
||||||
// start_command - this function will be called when the ap_mission lib wishes to start a new command
|
// 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
|
// To-Do: logging when new commands start/end
|
||||||
if (should_log(MASK_LOG_CMD)) {
|
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
|
// 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
|
// should return true once the active navigation command completes successfully
|
||||||
// called at 10hz or higher
|
// 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) {
|
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
|
// exit_mission - function that is called once the mission completes
|
||||||
static void exit_mission()
|
void Copter::exit_mission()
|
||||||
{
|
{
|
||||||
// play a tone
|
// play a tone
|
||||||
AP_Notify::events.mission_complete = 1;
|
AP_Notify::events.mission_complete = 1;
|
||||||
@ -305,7 +269,7 @@ static void exit_mission()
|
|||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
|
||||||
// do_RTL - start Return-to-Launch
|
// do_RTL - start Return-to-Launch
|
||||||
static void do_RTL(void)
|
void Copter::do_RTL(void)
|
||||||
{
|
{
|
||||||
// start rtl in auto flight mode
|
// start rtl in auto flight mode
|
||||||
auto_rtl_start();
|
auto_rtl_start();
|
||||||
@ -316,7 +280,7 @@ static void do_RTL(void)
|
|||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
|
||||||
// do_takeoff - initiate takeoff navigation command
|
// 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
|
// Set wp navigation target to safe altitude above current position
|
||||||
float takeoff_alt = cmd.content.location.alt;
|
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
|
// 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 &curr_pos = inertial_nav.get_position();
|
||||||
const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
|
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
|
// 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
|
// 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
|
// do_loiter_unlimited - start loitering with no end conditions
|
||||||
// note: caller should set yaw_mode
|
// 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;
|
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
|
// 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 curr_pos = inertial_nav.get_position();
|
||||||
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
|
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
|
// do_loiter_time - initiate loitering at a point for a given time period
|
||||||
// note: caller should set yaw_mode
|
// 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;
|
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
|
// 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();
|
const Vector3f& curr_pos = inertial_nav.get_position();
|
||||||
Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
|
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
|
#if NAV_GUIDED == ENABLED
|
||||||
// do_nav_guided_enable - initiate accepting commands from external nav computer
|
// 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) {
|
if (cmd.p1 > 0) {
|
||||||
// initialise guided limits
|
// initialise guided limits
|
||||||
@ -527,7 +491,7 @@ static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
|||||||
|
|
||||||
#if PARACHUTE == ENABLED
|
#if PARACHUTE == ENABLED
|
||||||
// do_parachute - configure or release parachute
|
// 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) {
|
switch (cmd.p1) {
|
||||||
case PARACHUTE_DISABLE:
|
case PARACHUTE_DISABLE:
|
||||||
@ -550,7 +514,7 @@ static void do_parachute(const AP_Mission::Mission_Command& cmd)
|
|||||||
|
|
||||||
#if EPM_ENABLED == ENABLED
|
#if EPM_ENABLED == ENABLED
|
||||||
// do_gripper - control EPM gripper
|
// 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
|
// Note: we ignore the gripper num parameter because we only support one gripper
|
||||||
switch (cmd.content.gripper.action) {
|
switch (cmd.content.gripper.action) {
|
||||||
@ -571,7 +535,7 @@ static void do_gripper(const AP_Mission::Mission_Command& cmd)
|
|||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED == ENABLED
|
||||||
// do_guided_limits - pass guided limits to guided controller
|
// 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
|
guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
|
||||||
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
|
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
|
// verify_takeoff - check if we have completed the takeoff
|
||||||
static bool verify_takeoff()
|
bool Copter::verify_takeoff()
|
||||||
{
|
{
|
||||||
// have we reached our target altitude?
|
// have we reached our target altitude?
|
||||||
return wp_nav.reached_wp_destination();
|
return wp_nav.reached_wp_destination();
|
||||||
}
|
}
|
||||||
|
|
||||||
// verify_land - returns true if landing has been completed
|
// verify_land - returns true if landing has been completed
|
||||||
static bool verify_land()
|
bool Copter::verify_land()
|
||||||
{
|
{
|
||||||
bool retval = false;
|
bool retval = false;
|
||||||
|
|
||||||
@ -628,7 +592,7 @@ static bool verify_land()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// verify_nav_wp - check if we have reached the next way point
|
// 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
|
// check if we have reached the waypoint
|
||||||
if( !wp_nav.reached_wp_destination() ) {
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// verify_loiter_time - check if we have loitered long enough
|
// 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
|
// return immediately if we haven't reached our destination
|
||||||
if (!wp_nav.reached_wp_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
|
// 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
|
// check if we've reached the edge
|
||||||
if (auto_mode == Auto_CircleMoveToEdge) {
|
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);
|
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
|
// verify_RTL - handles any state changes required to implement RTL
|
||||||
// do_RTL should have been called once first to initialise all variables
|
// do_RTL should have been called once first to initialise all variables
|
||||||
// returns true with RTL has completed successfully
|
// 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));
|
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
|
// 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
|
// check if we have reached the waypoint
|
||||||
if( !wp_nav.reached_wp_destination() ) {
|
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
|
#if NAV_GUIDED == ENABLED
|
||||||
// verify_nav_guided - check if we have breached any limits
|
// 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 disabling guided mode then immediately return true so we move to next command
|
||||||
if (cmd.p1 == 0) {
|
if (cmd.p1 == 0) {
|
||||||
@ -756,23 +717,23 @@ static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
|||||||
// Condition (May) commands
|
// 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_start = millis();
|
||||||
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
|
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
|
// 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;
|
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(
|
set_auto_yaw_look_at_heading(
|
||||||
cmd.content.yaw.angle_deg,
|
cmd.content.yaw.angle_deg,
|
||||||
@ -786,7 +747,7 @@ static void do_yaw(const AP_Mission::Mission_Command& cmd)
|
|||||||
// Verify Condition (May) commands
|
// Verify Condition (May) commands
|
||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
|
||||||
static bool verify_wait_delay()
|
bool Copter::verify_wait_delay()
|
||||||
{
|
{
|
||||||
if (millis() - condition_start > (uint32_t)max(condition_value,0)) {
|
if (millis() - condition_start > (uint32_t)max(condition_value,0)) {
|
||||||
condition_value = 0;
|
condition_value = 0;
|
||||||
@ -795,13 +756,13 @@ static bool verify_wait_delay()
|
|||||||
return false;
|
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
|
// To-Do: use recorded target altitude to verify we have reached the target
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool verify_within_distance()
|
bool Copter::verify_within_distance()
|
||||||
{
|
{
|
||||||
// update distance calculation
|
// update distance calculation
|
||||||
calc_wp_distance();
|
calc_wp_distance();
|
||||||
@ -813,7 +774,7 @@ static bool verify_within_distance()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// verify_yaw - return true if we have reached the desired heading
|
// 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)
|
// 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) {
|
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
|
||||||
@ -833,7 +794,7 @@ static bool verify_yaw()
|
|||||||
/********************************************************************************/
|
/********************************************************************************/
|
||||||
|
|
||||||
// do_guided - start guided mode
|
// 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
|
Vector3f pos_or_vel; // target location or velocity
|
||||||
|
|
||||||
@ -866,14 +827,14 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd)
|
|||||||
return true;
|
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) {
|
if (cmd.content.speed.target_ms > 0) {
|
||||||
wp_nav.set_speed_xy(cmd.content.speed.target_ms * 100.0f);
|
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)) {
|
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
|
||||||
set_home_to_current_location();
|
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)
|
// 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
|
// 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
|
// 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);
|
set_auto_yaw_roi(cmd.content.location);
|
||||||
}
|
}
|
||||||
|
|
||||||
// do_digicam_configure Send Digicam Configure message with the camera library
|
// 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
|
#if CAMERA == ENABLED
|
||||||
camera.configure_cmd(cmd);
|
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
|
// 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
|
#if CAMERA == ENABLED
|
||||||
camera.control_cmd(cmd);
|
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
|
// do_take_picture - take a picture with the camera library
|
||||||
static void do_take_picture()
|
void Copter::do_take_picture()
|
||||||
{
|
{
|
||||||
#if CAMERA == ENABLED
|
#if CAMERA == ENABLED
|
||||||
camera.trigger_pic(true);
|
camera.trigger_pic(true);
|
||||||
@ -920,7 +881,7 @@ static void do_take_picture()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// log_picture - log picture taken and send feedback to GCS
|
// log_picture - log picture taken and send feedback to GCS
|
||||||
static void log_picture()
|
void Copter::log_picture()
|
||||||
{
|
{
|
||||||
gcs_send_message(MSG_CAMERA_FEEDBACK);
|
gcs_send_message(MSG_CAMERA_FEEDBACK);
|
||||||
if (should_log(MASK_LOG_CAMERA)) {
|
if (should_log(MASK_LOG_CAMERA)) {
|
||||||
@ -929,7 +890,7 @@ static void log_picture()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// point the camera to a specified angle
|
// 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
|
#if MOUNT == ENABLED
|
||||||
camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
|
camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
compass/motor interference calibration
|
compass/motor interference calibration
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// setup_compassmot - sets compass's motor interference parameters
|
// 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
|
int8_t comp_type; // throttle or current based compensation
|
||||||
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
|
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
|
||||||
|
@ -1,16 +1,16 @@
|
|||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
|
void Copter::delay(uint32_t ms)
|
||||||
static void delay(uint32_t ms)
|
|
||||||
{
|
{
|
||||||
hal.scheduler->delay(ms);
|
hal.scheduler->delay(ms);
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint32_t millis()
|
uint32_t Copter::millis()
|
||||||
{
|
{
|
||||||
return hal.scheduler->millis();
|
return hal.scheduler->millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint32_t micros()
|
uint32_t Copter::micros()
|
||||||
{
|
{
|
||||||
return hal.scheduler->micros();
|
return hal.scheduler->micros();
|
||||||
}
|
}
|
||||||
|
@ -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__
|
|
||||||
|
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_acro.pde - init and run calls for acro flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// acro_init - initialise acro controller
|
// acro_init - initialise acro controller
|
||||||
static bool acro_init(bool ignore_checks)
|
bool Copter::acro_init(bool ignore_checks)
|
||||||
{
|
{
|
||||||
// always successfully enter acro
|
// always successfully enter acro
|
||||||
return true;
|
return true;
|
||||||
@ -13,7 +15,7 @@ static bool acro_init(bool ignore_checks)
|
|||||||
|
|
||||||
// acro_run - runs the acro controller
|
// acro_run - runs the acro controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void acro_run()
|
void Copter::acro_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch, target_yaw;
|
float target_roll, target_pitch, target_yaw;
|
||||||
int16_t pilot_throttle_scaled;
|
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
|
// 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
|
// 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;
|
float rate_limit;
|
||||||
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
|
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_althold.pde - init and run calls for althold, flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// althold_init - initialise althold controller
|
// 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
|
// initialize vertical speeds and leash lengths
|
||||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
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
|
// althold_run - runs the althold controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void althold_run()
|
void Copter::althold_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch;
|
float target_roll, target_pitch;
|
||||||
float target_yaw_rate;
|
float target_yaw_rate;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_auto.pde - init and run calls for auto flight mode
|
||||||
*
|
*
|
||||||
@ -18,7 +20,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// auto_init - initialise auto controller
|
// 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) {
|
if ((position_ok() && mission.num_commands() > 1) || ignore_checks) {
|
||||||
auto_mode = Auto_Loiter;
|
auto_mode = Auto_Loiter;
|
||||||
@ -46,7 +48,7 @@ static bool auto_init(bool ignore_checks)
|
|||||||
// auto_run - runs the auto controller
|
// auto_run - runs the auto controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
|
// 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
|
// call the correct auto controller
|
||||||
switch (auto_mode) {
|
switch (auto_mode) {
|
||||||
@ -89,7 +91,7 @@ static void auto_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// auto_takeoff_start - initialises waypoint controller to implement take-off
|
// 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;
|
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
|
// auto_takeoff_run - takeoff in auto mode
|
||||||
// called by auto_run at 100hz or more
|
// 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
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
|
// 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;
|
auto_mode = Auto_WP;
|
||||||
|
|
||||||
@ -154,7 +156,7 @@ static void auto_wp_start(const Vector3f& destination)
|
|||||||
|
|
||||||
// auto_wp_run - runs the auto waypoint controller
|
// auto_wp_run - runs the auto waypoint controller
|
||||||
// called by auto_run at 100hz or more
|
// 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
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
|
// 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
|
// 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;
|
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
|
// auto_spline_run - runs the auto spline controller
|
||||||
// called by auto_run at 100hz or more
|
// 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
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
|
// auto_land_start - initialises controller to implement a landing
|
||||||
static void auto_land_start()
|
void Copter::auto_land_start()
|
||||||
{
|
{
|
||||||
// set target to stopping point
|
// set target to stopping point
|
||||||
Vector3f stopping_point;
|
Vector3f stopping_point;
|
||||||
@ -260,7 +264,7 @@ static void auto_land_start()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// auto_land_start - initialises controller to implement a landing
|
// 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;
|
auto_mode = Auto_Land;
|
||||||
|
|
||||||
@ -276,7 +280,7 @@ static void auto_land_start(const Vector3f& destination)
|
|||||||
|
|
||||||
// auto_land_run - lands in auto mode
|
// auto_land_run - lands in auto mode
|
||||||
// called by auto_run at 100hz or more
|
// 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;
|
int16_t roll_control = 0, pitch_control = 0;
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -328,7 +332,7 @@ static void auto_land_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// auto_rtl_start - initialises RTL in AUTO flight mode
|
// auto_rtl_start - initialises RTL in AUTO flight mode
|
||||||
static void auto_rtl_start()
|
void Copter::auto_rtl_start()
|
||||||
{
|
{
|
||||||
auto_mode = Auto_RTL;
|
auto_mode = Auto_RTL;
|
||||||
|
|
||||||
@ -338,7 +342,7 @@ static void auto_rtl_start()
|
|||||||
|
|
||||||
// auto_rtl_run - rtl in AUTO flight mode
|
// auto_rtl_run - rtl in AUTO flight mode
|
||||||
// called by auto_run at 100hz or more
|
// called by auto_run at 100hz or more
|
||||||
void auto_rtl_run()
|
void Copter::auto_rtl_run()
|
||||||
{
|
{
|
||||||
// call regular rtl flight mode run function
|
// call regular rtl flight mode run function
|
||||||
rtl_run();
|
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
|
// 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 set the circle's circle with circle_nav.set_center()
|
||||||
// we assume the caller has performed all required GPS_ok checks
|
// 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
|
// check our distance from edge of circle
|
||||||
Vector3f circle_edge;
|
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
|
// 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;
|
auto_mode = Auto_Circle;
|
||||||
|
|
||||||
@ -383,7 +387,7 @@ static void auto_circle_start()
|
|||||||
|
|
||||||
// auto_circle_run - circle in AUTO flight mode
|
// auto_circle_run - circle in AUTO flight mode
|
||||||
// called by auto_run at 100hz or more
|
// called by auto_run at 100hz or more
|
||||||
void auto_circle_run()
|
void Copter::auto_circle_run()
|
||||||
{
|
{
|
||||||
// call circle controller
|
// call circle controller
|
||||||
circle_nav.update();
|
circle_nav.update();
|
||||||
@ -397,7 +401,7 @@ void auto_circle_run()
|
|||||||
|
|
||||||
#if NAV_GUIDED == ENABLED
|
#if NAV_GUIDED == ENABLED
|
||||||
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
|
// 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;
|
auto_mode = Auto_NavGuided;
|
||||||
|
|
||||||
@ -410,7 +414,7 @@ void auto_nav_guided_start()
|
|||||||
|
|
||||||
// auto_nav_guided_run - allows control by external navigation controller
|
// auto_nav_guided_run - allows control by external navigation controller
|
||||||
// called by auto_run at 100hz or more
|
// 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
|
// call regular guided flight mode run function
|
||||||
guided_run();
|
guided_run();
|
||||||
@ -419,7 +423,7 @@ void auto_nav_guided_run()
|
|||||||
|
|
||||||
// auto_loiter_start - initialises loitering in auto mode
|
// auto_loiter_start - initialises loitering in auto mode
|
||||||
// returns success/failure because this can be called by exit_mission
|
// 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
|
// return failure if GPS is bad
|
||||||
if (!position_ok()) {
|
if (!position_ok()) {
|
||||||
@ -445,7 +449,7 @@ bool auto_loiter_start()
|
|||||||
|
|
||||||
// auto_loiter_run - loiter in AUTO flight mode
|
// auto_loiter_run - loiter in AUTO flight mode
|
||||||
// called by auto_run at 100hz or more
|
// 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 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()) {
|
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
|
// 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
|
// 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) {
|
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
|
// 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
|
// return immediately if no change
|
||||||
if (auto_yaw_mode == yaw_mode) {
|
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
|
// 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
|
// get current yaw target
|
||||||
int32_t curr_yaw_target = attitude_control.angle_ef_targets().z;
|
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
|
// 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 location is zero lat, lon and altitude turn off ROI
|
||||||
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
|
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
|
// get_auto_heading - returns target heading depending upon auto_yaw_mode
|
||||||
// 100hz update rate
|
// 100hz update rate
|
||||||
float get_auto_heading(void)
|
float Copter::get_auto_heading(void)
|
||||||
{
|
{
|
||||||
switch(auto_yaw_mode) {
|
switch(auto_yaw_mode) {
|
||||||
|
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if AUTOTUNE_ENABLED == ENABLED
|
#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;
|
static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel;
|
||||||
|
|
||||||
// autotune_init - should be called when autotune mode is selected
|
// 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;
|
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
|
// 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
|
// set gains to their original values
|
||||||
autotune_load_orig_gains();
|
autotune_load_orig_gains();
|
||||||
@ -224,7 +226,7 @@ static void autotune_stop()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// autotune_start - Initialize autotune flight mode
|
// 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
|
// only allow flip from Stabilize or AltHold flight modes
|
||||||
if (control_mode != STABILIZE && control_mode != ALT_HOLD) {
|
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
|
// autotune_run - runs the autotune flight mode
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void autotune_run()
|
void Copter::autotune_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch;
|
float target_roll, target_pitch;
|
||||||
float target_yaw_rate;
|
float target_yaw_rate;
|
||||||
@ -329,7 +331,7 @@ static void autotune_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// autotune_attitude_controller - sets attitude control targets during tuning
|
// 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 rotation_rate = 0.0f; // rotation rate in radians/second
|
||||||
float lean_angle = 0.0f;
|
float lean_angle = 0.0f;
|
||||||
@ -729,7 +731,7 @@ static void autotune_attitude_control()
|
|||||||
|
|
||||||
// autotune_backup_gains_and_initialise - store current gains as originals
|
// autotune_backup_gains_and_initialise - store current gains as originals
|
||||||
// called before tuning starts to backup original gains
|
// 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
|
// initialise state because this is our first time
|
||||||
if (autotune_roll_enabled()) {
|
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
|
// autotune_load_orig_gains - set gains to their original values
|
||||||
// called by autotune_stop and autotune_failed functions
|
// called by autotune_stop and autotune_failed functions
|
||||||
static void autotune_load_orig_gains()
|
void Copter::autotune_load_orig_gains()
|
||||||
{
|
{
|
||||||
// sanity check the gains
|
// sanity check the gains
|
||||||
bool failed = false;
|
bool failed = false;
|
||||||
@ -825,7 +827,7 @@ static void autotune_load_orig_gains()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// autotune_load_tuned_gains - load tuned gains
|
// autotune_load_tuned_gains - load tuned gains
|
||||||
static void autotune_load_tuned_gains()
|
void Copter::autotune_load_tuned_gains()
|
||||||
{
|
{
|
||||||
// sanity check the gains
|
// sanity check the gains
|
||||||
bool failed = true;
|
bool failed = true;
|
||||||
@ -865,7 +867,7 @@ static void autotune_load_tuned_gains()
|
|||||||
|
|
||||||
// autotune_load_intra_test_gains - gains used between tests
|
// 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
|
// 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)
|
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
|
||||||
// sanity check the gains
|
// 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
|
// 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)
|
// 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;
|
bool failed = true;
|
||||||
switch (autotune_state.axis) {
|
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
|
// 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)
|
// 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 we successfully completed tuning
|
||||||
if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) {
|
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
|
// 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) {
|
switch (message_id) {
|
||||||
case AUTOTUNE_MESSAGE_STARTED:
|
case AUTOTUNE_MESSAGE_STARTED:
|
||||||
@ -1055,21 +1057,21 @@ void autotune_update_gcs(uint8_t message_id)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// axis helper functions
|
// axis helper functions
|
||||||
inline bool autotune_roll_enabled() {
|
inline bool Copter::autotune_roll_enabled() {
|
||||||
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL;
|
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;
|
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;
|
return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW;
|
||||||
}
|
}
|
||||||
|
|
||||||
// autotune_twitching_test - twitching tests
|
// autotune_twitching_test - twitching tests
|
||||||
// update min and max and test for end conditions
|
// 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
|
// capture maximum measurement
|
||||||
if (measurement > measurement_max) {
|
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
|
// 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
|
// 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 (measurement_max > target) {
|
||||||
// if maximum measurement was higher than 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
|
// 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
|
// 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 (measurement_max > target) {
|
||||||
// if maximum measurement was higher than 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
|
// 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
|
// 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 (measurement_max < target) {
|
||||||
if (autotune_state.ignore_next == 0){
|
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
|
// autotune_updating_p_up - increase P to ensure the target is reached
|
||||||
// P is increased until we achieve our target within a reasonable time
|
// 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) {
|
if (measurement_max > target) {
|
||||||
// ignore the next result unless it is the same as this one
|
// 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
|
// 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
|
// 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) {
|
if (measurement_max > target) {
|
||||||
// ignore the next result unless it is the same as this one
|
// 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
|
// 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) {
|
if (rate_measurement_max < rate_measurement) {
|
||||||
rate_measurement_max = rate_measurement;
|
rate_measurement_max = rate_measurement;
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_brake.pde - init and run calls for brake flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// brake_init - initialise brake controller
|
// 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) {
|
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
|
// brake_run - runs the brake controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void brake_run()
|
void Copter::brake_run()
|
||||||
{
|
{
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
float target_climb_rate = 0;
|
float target_climb_rate = 0;
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_circle.pde - init and run calls for circle flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// circle_init - initialise circle controller 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) {
|
if (position_ok() || ignore_checks) {
|
||||||
circle_pilot_yaw_override = false;
|
circle_pilot_yaw_override = false;
|
||||||
@ -27,7 +29,7 @@ static bool circle_init(bool ignore_checks)
|
|||||||
|
|
||||||
// circle_run - runs the circle flight mode
|
// circle_run - runs the circle flight mode
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void circle_run()
|
void Copter::circle_run()
|
||||||
{
|
{
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
float target_climb_rate = 0;
|
float target_climb_rate = 0;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_drift.pde - init and run calls for drift flight mode
|
||||||
*/
|
*/
|
||||||
@ -27,7 +29,7 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
// drift_init - initialise drift controller
|
// drift_init - initialise drift controller
|
||||||
static bool drift_init(bool ignore_checks)
|
bool Copter::drift_init(bool ignore_checks)
|
||||||
{
|
{
|
||||||
if (position_ok() || ignore_checks) {
|
if (position_ok() || ignore_checks) {
|
||||||
return true;
|
return true;
|
||||||
@ -38,7 +40,7 @@ static bool drift_init(bool ignore_checks)
|
|||||||
|
|
||||||
// drift_run - runs the drift controller
|
// drift_run - runs the drift controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void drift_run()
|
void Copter::drift_run()
|
||||||
{
|
{
|
||||||
static float breaker = 0.0f;
|
static float breaker = 0.0f;
|
||||||
static float roll_input = 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
|
// 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
|
// throttle assist - adjusts throttle to slow the vehicle's vertical velocity
|
||||||
// Only active when pilot's throttle is between 213 ~ 787
|
// Only active when pilot's throttle is between 213 ~ 787
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_flip.pde - init and run calls for flip flight mode
|
||||||
* original implementation in 2010 by Jose Julio
|
* 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)
|
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
|
||||||
|
|
||||||
// flip_init - initialise flip controller
|
// 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
|
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
|
||||||
if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
|
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
|
// flip_run - runs the flip controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void flip_run()
|
void Copter::flip_run()
|
||||||
{
|
{
|
||||||
int16_t throttle_out;
|
int16_t throttle_out;
|
||||||
float recovery_angle;
|
float recovery_angle;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_guided.pde - init and run calls for guided flight mode
|
||||||
*/
|
*/
|
||||||
@ -24,7 +26,7 @@ struct Guided_Limit {
|
|||||||
} guided_limit;
|
} guided_limit;
|
||||||
|
|
||||||
// guided_init - initialise guided controller
|
// guided_init - initialise guided controller
|
||||||
static bool guided_init(bool ignore_checks)
|
bool Copter::guided_init(bool ignore_checks)
|
||||||
{
|
{
|
||||||
if (position_ok() || ignore_checks) {
|
if (position_ok() || ignore_checks) {
|
||||||
// initialise yaw
|
// initialise yaw
|
||||||
@ -39,7 +41,7 @@ static bool guided_init(bool ignore_checks)
|
|||||||
|
|
||||||
|
|
||||||
// guided_takeoff_start - initialises waypoint controller to implement take-off
|
// 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;
|
guided_mode = Guided_TakeOff;
|
||||||
|
|
||||||
@ -56,7 +58,7 @@ static void guided_takeoff_start(float final_alt_above_home)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// initialise guided mode's position controller
|
// initialise guided mode's position controller
|
||||||
static void guided_pos_control_start()
|
void Copter::guided_pos_control_start()
|
||||||
{
|
{
|
||||||
// set to position control mode
|
// set to position control mode
|
||||||
guided_mode = Guided_WP;
|
guided_mode = Guided_WP;
|
||||||
@ -77,7 +79,7 @@ static void guided_pos_control_start()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// initialise guided mode's velocity controller
|
// initialise guided mode's velocity controller
|
||||||
static void guided_vel_control_start()
|
void Copter::guided_vel_control_start()
|
||||||
{
|
{
|
||||||
// set guided_mode to velocity controller
|
// set guided_mode to velocity controller
|
||||||
guided_mode = Guided_Velocity;
|
guided_mode = Guided_Velocity;
|
||||||
@ -91,7 +93,7 @@ static void guided_vel_control_start()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// initialise guided mode's posvel controller
|
// initialise guided mode's posvel controller
|
||||||
static void guided_posvel_control_start()
|
void Copter::guided_posvel_control_start()
|
||||||
{
|
{
|
||||||
// set guided_mode to velocity controller
|
// set guided_mode to velocity controller
|
||||||
guided_mode = Guided_PosVel;
|
guided_mode = Guided_PosVel;
|
||||||
@ -118,7 +120,7 @@ static void guided_posvel_control_start()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// guided_set_destination - sets guided mode's target destination
|
// 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
|
// ensure we are in position control mode
|
||||||
if (guided_mode != Guided_WP) {
|
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
|
// 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
|
// check we are in velocity control mode
|
||||||
if (guided_mode != Guided_Velocity) {
|
if (guided_mode != Guided_Velocity) {
|
||||||
@ -141,7 +143,7 @@ static void guided_set_velocity(const Vector3f& velocity)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// set guided mode posvel target
|
// 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
|
// check we are in velocity control mode
|
||||||
if (guided_mode != Guided_PosVel) {
|
if (guided_mode != Guided_PosVel) {
|
||||||
guided_posvel_control_start();
|
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
|
// guided_run - runs the guided controller
|
||||||
// should be called at 100hz or more
|
// 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 not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
if(!ap.auto_armed || !motors.get_interlock()) {
|
||||||
@ -193,7 +195,7 @@ static void guided_run()
|
|||||||
|
|
||||||
// guided_takeoff_run - takeoff in guided mode
|
// guided_takeoff_run - takeoff in guided mode
|
||||||
// called by guided_run at 100hz or more
|
// 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 not auto armed or motors interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
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
|
// guided_pos_control_run - runs the guided position controller
|
||||||
// called from guided_run
|
// called from guided_run
|
||||||
static void guided_pos_control_run()
|
void Copter::guided_pos_control_run()
|
||||||
{
|
{
|
||||||
// process pilot's yaw input
|
// process pilot's yaw input
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -255,7 +257,7 @@ static void guided_pos_control_run()
|
|||||||
|
|
||||||
// guided_vel_control_run - runs the guided velocity controller
|
// guided_vel_control_run - runs the guided velocity controller
|
||||||
// called from guided_run
|
// called from guided_run
|
||||||
static void guided_vel_control_run()
|
void Copter::guided_vel_control_run()
|
||||||
{
|
{
|
||||||
// process pilot's yaw input
|
// process pilot's yaw input
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -293,7 +295,7 @@ static void guided_vel_control_run()
|
|||||||
|
|
||||||
// guided_posvel_control_run - runs the guided spline controller
|
// guided_posvel_control_run - runs the guided spline controller
|
||||||
// called from guided_run
|
// called from guided_run
|
||||||
static void guided_posvel_control_run()
|
void Copter::guided_posvel_control_run()
|
||||||
{
|
{
|
||||||
// process pilot's yaw input
|
// process pilot's yaw input
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -348,7 +350,7 @@ static void guided_posvel_control_run()
|
|||||||
// Guided Limit code
|
// Guided Limit code
|
||||||
|
|
||||||
// guided_limit_clear - clear/turn off guided limits
|
// 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.timeout_ms = 0;
|
||||||
guided_limit.alt_min_cm = 0.0f;
|
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
|
// 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.timeout_ms = timeout_ms;
|
||||||
guided_limit.alt_min_cm = alt_min_cm;
|
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
|
// 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
|
// 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
|
// initialise start time
|
||||||
guided_limit.start_time = hal.scheduler->millis();
|
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
|
// guided_limit_check - returns true if guided mode has breached a limit
|
||||||
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
|
// 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
|
// check if we have passed the timeout
|
||||||
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
|
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
|
||||||
|
@ -1,12 +1,14 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
static bool land_with_gps;
|
static bool land_with_gps;
|
||||||
|
|
||||||
static uint32_t land_start_time;
|
static uint32_t land_start_time;
|
||||||
static bool land_pause;
|
static bool land_pause;
|
||||||
|
|
||||||
// land_init - initialise land controller
|
// 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
|
// check if we have GPS and decide which LAND we're going to do
|
||||||
land_with_gps = position_ok();
|
land_with_gps = position_ok();
|
||||||
@ -33,7 +35,7 @@ static bool land_init(bool ignore_checks)
|
|||||||
|
|
||||||
// land_run - runs the land controller
|
// land_run - runs the land controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void land_run()
|
void Copter::land_run()
|
||||||
{
|
{
|
||||||
if (land_with_gps) {
|
if (land_with_gps) {
|
||||||
land_gps_run();
|
land_gps_run();
|
||||||
@ -45,7 +47,7 @@ static void land_run()
|
|||||||
// land_run - runs the land controller
|
// land_run - runs the land controller
|
||||||
// horizontal position controlled with loiter controller
|
// horizontal position controlled with loiter controller
|
||||||
// should be called at 100hz or more
|
// 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;
|
int16_t roll_control = 0, pitch_control = 0;
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -118,7 +120,7 @@ static void land_gps_run()
|
|||||||
// land_nogps_run - runs the land controller
|
// land_nogps_run - runs the land controller
|
||||||
// pilot controls roll and pitch angles
|
// pilot controls roll and pitch angles
|
||||||
// should be called at 100hz or more
|
// 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_roll = 0.0f, target_pitch = 0.0f;
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -177,7 +179,7 @@ static void land_nogps_run()
|
|||||||
// get_land_descent_speed - high level landing logic
|
// get_land_descent_speed - high level landing logic
|
||||||
// returns climb rate (in cm/s) which should be passed to the position controller
|
// returns climb rate (in cm/s) which should be passed to the position controller
|
||||||
// should be called at 100hz or higher
|
// should be called at 100hz or higher
|
||||||
static float get_land_descent_speed()
|
float Copter::get_land_descent_speed()
|
||||||
{
|
{
|
||||||
#if CONFIG_SONAR == ENABLED
|
#if CONFIG_SONAR == ENABLED
|
||||||
bool sonar_ok = sonar_enabled && (sonar.status() == RangeFinder::RangeFinder_Good);
|
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
|
// 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
|
// 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
|
// 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;
|
land_with_gps = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
|
// 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
|
// 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);
|
set_mode(LAND);
|
||||||
land_pause = true;
|
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
|
// 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);
|
return (control_mode == LAND && land_with_gps);
|
||||||
}
|
}
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_loiter.pde - init and run calls for loiter flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// loiter_init - initialise loiter controller
|
// 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) {
|
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
|
// loiter_run - runs the loiter controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void loiter_run()
|
void Copter::loiter_run()
|
||||||
{
|
{
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
float target_climb_rate = 0;
|
float target_climb_rate = 0;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if POSHOLD_ENABLED == ENABLED
|
#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_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
|
#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
|
// mission state enumeration
|
||||||
enum poshold_rp_mode {
|
enum poshold_rp_mode {
|
||||||
POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
|
POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
|
||||||
@ -82,7 +75,7 @@ static struct {
|
|||||||
} poshold;
|
} poshold;
|
||||||
|
|
||||||
// poshold_init - initialise PosHold controller
|
// 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
|
// fail to initialise PosHold mode if no GPS lock
|
||||||
if (!position_ok() && !ignore_checks) {
|
if (!position_ok() && !ignore_checks) {
|
||||||
@ -130,7 +123,7 @@ static bool poshold_init(bool ignore_checks)
|
|||||||
|
|
||||||
// poshold_run - runs the PosHold controller
|
// poshold_run - runs the PosHold controller
|
||||||
// should be called at 100hz or more
|
// 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_roll, target_pitch; // pilot's roll and pitch angle inputs
|
||||||
float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec
|
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
|
// 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 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)) {
|
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
|
// 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
|
// 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);
|
mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
|
||||||
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control));
|
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
|
// 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
|
// 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)
|
// 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;
|
float lean_angle;
|
||||||
int16_t brake_rate = g.poshold_brake_rate;
|
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
|
// poshold_update_wind_comp_estimate - updates wind compensation estimate
|
||||||
// should be called at the maximum loop rate when loiter is engaged
|
// 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
|
// check wind estimate start has not been delayed
|
||||||
if (poshold.wind_comp_start_timer > 0) {
|
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
|
// 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
|
// 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
|
// reduce rate to 10hz
|
||||||
poshold.wind_comp_timer++;
|
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
|
// 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.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
|
||||||
poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
|
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
|
// 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.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE;
|
||||||
poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
|
poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_rtl.pde - init and run calls for RTL flight mode
|
||||||
*
|
*
|
||||||
@ -8,7 +10,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// rtl_init - initialise rtl controller
|
// rtl_init - initialise rtl controller
|
||||||
static bool rtl_init(bool ignore_checks)
|
bool Copter::rtl_init(bool ignore_checks)
|
||||||
{
|
{
|
||||||
if (position_ok() || ignore_checks) {
|
if (position_ok() || ignore_checks) {
|
||||||
rtl_climb_start();
|
rtl_climb_start();
|
||||||
@ -20,7 +22,7 @@ static bool rtl_init(bool ignore_checks)
|
|||||||
|
|
||||||
// rtl_run - runs the return-to-launch controller
|
// rtl_run - runs the return-to-launch controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void rtl_run()
|
void Copter::rtl_run()
|
||||||
{
|
{
|
||||||
// check if we need to move to next state
|
// check if we need to move to next state
|
||||||
if (rtl_state_complete) {
|
if (rtl_state_complete) {
|
||||||
@ -73,7 +75,7 @@ static void rtl_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// rtl_climb_start - initialise climb to RTL altitude
|
// rtl_climb_start - initialise climb to RTL altitude
|
||||||
static void rtl_climb_start()
|
void Copter::rtl_climb_start()
|
||||||
{
|
{
|
||||||
rtl_state = RTL_InitialClimb;
|
rtl_state = RTL_InitialClimb;
|
||||||
rtl_state_complete = false;
|
rtl_state_complete = false;
|
||||||
@ -104,7 +106,7 @@ static void rtl_climb_start()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// rtl_return_start - initialise return to home
|
// rtl_return_start - initialise return to home
|
||||||
static void rtl_return_start()
|
void Copter::rtl_return_start()
|
||||||
{
|
{
|
||||||
rtl_state = RTL_ReturnHome;
|
rtl_state = RTL_ReturnHome;
|
||||||
rtl_state_complete = false;
|
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
|
// 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
|
// 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
if(!ap.auto_armed || !motors.get_interlock()) {
|
||||||
@ -169,7 +171,7 @@ static void rtl_climb_return_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// rtl_return_start - initialise return to home
|
// rtl_return_start - initialise return to home
|
||||||
static void rtl_loiterathome_start()
|
void Copter::rtl_loiterathome_start()
|
||||||
{
|
{
|
||||||
rtl_state = RTL_LoiterAtHome;
|
rtl_state = RTL_LoiterAtHome;
|
||||||
rtl_state_complete = false;
|
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
|
// 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
|
// 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||||
if(!ap.auto_armed || !motors.get_interlock()) {
|
if(!ap.auto_armed || !motors.get_interlock()) {
|
||||||
@ -235,7 +237,7 @@ static void rtl_loiterathome_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// rtl_descent_start - initialise descent to final alt
|
// rtl_descent_start - initialise descent to final alt
|
||||||
static void rtl_descent_start()
|
void Copter::rtl_descent_start()
|
||||||
{
|
{
|
||||||
rtl_state = RTL_FinalDescent;
|
rtl_state = RTL_FinalDescent;
|
||||||
rtl_state_complete = false;
|
rtl_state_complete = false;
|
||||||
@ -252,7 +254,7 @@ static void rtl_descent_start()
|
|||||||
|
|
||||||
// rtl_descent_run - implements the final descent to the RTL_ALT
|
// rtl_descent_run - implements the final descent to the RTL_ALT
|
||||||
// called by rtl_run at 100hz or more
|
// 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;
|
int16_t roll_control = 0, pitch_control = 0;
|
||||||
float target_yaw_rate = 0;
|
float target_yaw_rate = 0;
|
||||||
@ -298,7 +300,7 @@ static void rtl_descent_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// rtl_loiterathome_start - initialise controllers to loiter over home
|
// 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 = RTL_Land;
|
||||||
rtl_state_complete = false;
|
rtl_state_complete = false;
|
||||||
@ -315,7 +317,7 @@ static void rtl_land_start()
|
|||||||
|
|
||||||
// rtl_returnhome_run - return home
|
// rtl_returnhome_run - return home
|
||||||
// called by rtl_run at 100hz or more
|
// 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;
|
int16_t roll_control = 0, pitch_control = 0;
|
||||||
float target_yaw_rate = 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
|
// get_RTL_alt - return altitude which vehicle should return home at
|
||||||
// altitude is in cm above home
|
// altitude is in cm above home
|
||||||
static float get_RTL_alt()
|
float Copter::get_RTL_alt()
|
||||||
{
|
{
|
||||||
// maximum of current altitude and rtl altitude
|
// maximum of current altitude and rtl altitude
|
||||||
float rtl_alt = max(current_loc.alt, g.rtl_altitude);
|
float rtl_alt = max(current_loc.alt, g.rtl_altitude);
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_sport.pde - init and run calls for sport flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// sport_init - initialise sport controller
|
// sport_init - initialise sport controller
|
||||||
static bool sport_init(bool ignore_checks)
|
bool Copter::sport_init(bool ignore_checks)
|
||||||
{
|
{
|
||||||
// initialize vertical speed and accelerationj
|
// initialize vertical speed and accelerationj
|
||||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
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
|
// sport_run - runs the sport controller
|
||||||
// should be called at 100hz or more
|
// 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_roll_rate, target_pitch_rate, target_yaw_rate;
|
||||||
float target_climb_rate = 0;
|
float target_climb_rate = 0;
|
||||||
|
@ -1,11 +1,13 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* control_stabilize.pde - init and run calls for stabilize flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// stabilize_init - initialise stabilize controller
|
// 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
|
// 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?
|
// 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
|
// stabilize_run - runs the main stabilize controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void stabilize_run()
|
void Copter::stabilize_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch;
|
float target_roll, target_pitch;
|
||||||
float target_yaw_rate;
|
float target_yaw_rate;
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
// Code to detect a crash main ArduCopter code
|
// Code to detect a crash main ArduCopter code
|
||||||
#ifndef CRASH_CHECK_ITERATIONS_MAX
|
#ifndef CRASH_CHECK_ITERATIONS_MAX
|
||||||
# define CRASH_CHECK_ITERATIONS_MAX 20 // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash
|
# 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
|
// 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
|
// 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
|
// 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 uint8_t inverted_count; // number of iterations we have been inverted
|
||||||
static int32_t baro_alt_prev;
|
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
|
// 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
|
// 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
|
// 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 uint8_t control_loss_count; // number of iterations we have been out of control
|
||||||
static int32_t baro_alt_start;
|
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
|
// 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
|
// send message to gcs and dataflash
|
||||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Released!"));
|
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
|
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
|
||||||
// checks if the vehicle is landed
|
// checks if the vehicle is landed
|
||||||
static void parachute_manual_release()
|
void Copter::parachute_manual_release()
|
||||||
{
|
{
|
||||||
// exit immediately if parachute is not enabled
|
// exit immediately if parachute is not enabled
|
||||||
if (!parachute.enabled()) {
|
if (!parachute.enabled()) {
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* 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
|
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
|
||||||
// should be called at 10hz
|
// 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
|
// 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)) {
|
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
|
// 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
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||||
// return false immediately if disabled
|
// return false immediately if disabled
|
||||||
@ -112,7 +114,7 @@ static bool ekf_over_threshold()
|
|||||||
|
|
||||||
|
|
||||||
// failsafe_ekf_event - perform ekf failsafe
|
// failsafe_ekf_event - perform ekf failsafe
|
||||||
static void failsafe_ekf_event()
|
void Copter::failsafe_ekf_event()
|
||||||
{
|
{
|
||||||
// return immediately if ekf failsafe already triggered
|
// return immediately if ekf failsafe already triggered
|
||||||
if (failsafe.ekf) {
|
if (failsafe.ekf) {
|
||||||
@ -140,7 +142,7 @@ static void failsafe_ekf_event()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
|
// 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
|
// return immediately if not in ekf failsafe
|
||||||
if (!failsafe.ekf) {
|
if (!failsafe.ekf) {
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
* esc_calibration.pde : functions to check and perform ESC calibration
|
||||||
*****************************************************************************/
|
*****************************************************************************/
|
||||||
@ -15,7 +17,7 @@ enum ESCCalibrationModes {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// check if we should enter esc calibration mode
|
// 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
|
// exit immediately if pre-arm rc checks fail
|
||||||
pre_arm_rc_checks();
|
pre_arm_rc_checks();
|
||||||
@ -66,7 +68,7 @@ static void esc_calibration_startup_check()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// esc_calibration_passthrough - pass through pilot throttle to escs
|
// 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
|
// clear esc flag for next time
|
||||||
g.esc_calibrate.set_and_save(ESCCAL_NONE);
|
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
|
// 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;
|
bool printed_msg = false;
|
||||||
|
|
||||||
|
@ -1,10 +1,12 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
* This event will be called when the failsafe changes
|
||||||
* boolean failsafe reflects the current state
|
* 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 are not armed there is nothing to do
|
||||||
if( !motors.armed() ) {
|
if( !motors.armed() ) {
|
||||||
@ -91,14 +93,14 @@ static void failsafe_radio_on_event()
|
|||||||
// failsafe_off_event - respond to radio contact being regained
|
// failsafe_off_event - respond to radio contact being regained
|
||||||
// we must be in AUTO, LAND or RTL modes
|
// we must be in AUTO, LAND or RTL modes
|
||||||
// or Stabilize or ACRO mode but with motors disarmed
|
// 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
|
// 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
|
// 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);
|
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
|
// return immediately if low battery event has already been triggered
|
||||||
if (failsafe.battery) {
|
if (failsafe.battery) {
|
||||||
@ -163,7 +165,7 @@ static void failsafe_battery_event(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// failsafe_gcs_check - check for ground station failsafe
|
// failsafe_gcs_check - check for ground station failsafe
|
||||||
static void failsafe_gcs_check()
|
void Copter::failsafe_gcs_check()
|
||||||
{
|
{
|
||||||
uint32_t last_gcs_update_ms;
|
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
|
// 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 recovery of GCS in logs?
|
||||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
|
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
|
// 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
|
// 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
|
// attempt to switch to RTL, if this fails then switch to Land
|
||||||
if (!set_mode(RTL)) {
|
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();
|
ServoRelayEvents.update_events();
|
||||||
}
|
}
|
||||||
|
@ -1,4 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
//
|
//
|
||||||
// failsafe support
|
// failsafe support
|
||||||
// Andrew Tridgell, December 2011
|
// Andrew Tridgell, December 2011
|
||||||
@ -14,7 +17,7 @@ static bool in_failsafe;
|
|||||||
//
|
//
|
||||||
// failsafe_enable - enable failsafe
|
// failsafe_enable - enable failsafe
|
||||||
//
|
//
|
||||||
void failsafe_enable()
|
void Copter::failsafe_enable()
|
||||||
{
|
{
|
||||||
failsafe_enabled = true;
|
failsafe_enabled = true;
|
||||||
failsafe_last_timestamp = micros();
|
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
|
// failsafe_disable - used when we know we are going to delay the mainloop significantly
|
||||||
//
|
//
|
||||||
void failsafe_disable()
|
void Copter::failsafe_disable()
|
||||||
{
|
{
|
||||||
failsafe_enabled = false;
|
failsafe_enabled = false;
|
||||||
}
|
}
|
||||||
@ -31,7 +34,7 @@ void failsafe_disable()
|
|||||||
//
|
//
|
||||||
// failsafe_check - this function is called from the core timer interrupt at 1kHz.
|
// 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();
|
uint32_t tnow = hal.scheduler->micros();
|
||||||
|
|
||||||
|
@ -1,12 +1,14 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
// Code to integrate AC_Fence library with main ArduCopter code
|
||||||
|
|
||||||
#if AC_FENCE == ENABLED
|
#if AC_FENCE == ENABLED
|
||||||
|
|
||||||
// fence_check - ask fence library to check for breaches and initiate the response
|
// fence_check - ask fence library to check for breaches and initiate the response
|
||||||
// called at 1hz
|
// called at 1hz
|
||||||
void fence_check()
|
void Copter::fence_check()
|
||||||
{
|
{
|
||||||
uint8_t new_breaches; // the type of fence that has been breached
|
uint8_t new_breaches; // the type of fence that has been breached
|
||||||
uint8_t orig_breaches = fence.get_breaches();
|
uint8_t orig_breaches = fence.get_breaches();
|
||||||
@ -56,7 +58,7 @@ void fence_check()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// fence_send_mavlink_status - send fence status to ground station
|
// 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()) {
|
if (fence.enabled()) {
|
||||||
// traslate fence library breach types to mavlink breach types
|
// traslate fence library breach types to mavlink breach types
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* 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
|
* 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)
|
// optional force parameter used to force the flight mode change (used only first time mode is set)
|
||||||
// returns true if mode was succesfully 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
|
// 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
|
// boolean to record if flight mode could be set
|
||||||
bool success = false;
|
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
|
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
|
||||||
// called at 100hz or more
|
// called at 100hz or more
|
||||||
static void update_flight_mode()
|
void Copter::update_flight_mode()
|
||||||
{
|
{
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||||
// Update EKF speed limit - used to limit speed when we are using optical flow
|
// 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
|
// 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 AUTOTUNE_ENABLED == ENABLED
|
||||||
if (old_control_mode == AUTOTUNE) {
|
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
|
// 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) {
|
switch(mode) {
|
||||||
case AUTO:
|
case AUTO:
|
||||||
case GUIDED:
|
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)
|
// 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) {
|
switch(mode) {
|
||||||
case ACRO:
|
case ACRO:
|
||||||
case STABILIZE:
|
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
|
// 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
|
// 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)) {
|
if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || (arming_from_gcs && mode == GUIDED)) {
|
||||||
return true;
|
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
|
// 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) {
|
switch(mode) {
|
||||||
case AUTO:
|
case AUTO:
|
||||||
case GUIDED:
|
case GUIDED:
|
||||||
@ -309,8 +311,7 @@ static void notify_flight_mode(uint8_t mode) {
|
|||||||
//
|
//
|
||||||
// print_flight_mode - prints flight mode to serial port.
|
// print_flight_mode - prints flight mode to serial port.
|
||||||
//
|
//
|
||||||
static void
|
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
||||||
print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|
||||||
{
|
{
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case STABILIZE:
|
case STABILIZE:
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
// Traditional helicopter variables and functions
|
// Traditional helicopter variables and functions
|
||||||
|
|
||||||
#include "heli.h"
|
#include "heli.h"
|
||||||
@ -14,13 +16,13 @@
|
|||||||
static int8_t heli_dynamic_flight_counter;
|
static int8_t heli_dynamic_flight_counter;
|
||||||
|
|
||||||
// heli_init - perform any special initialisation required for the tradheli
|
// 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?
|
// 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
|
// 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
|
// 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) {
|
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
|
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
|
||||||
// should be called at 50hz
|
// should be called at 50hz
|
||||||
static void check_dynamic_flight(void)
|
void Copter::check_dynamic_flight(void)
|
||||||
{
|
{
|
||||||
if (!motors.armed() || !motors.rotor_runup_complete() ||
|
if (!motors.armed() || !motors.rotor_runup_complete() ||
|
||||||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
|
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.
|
// update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
|
||||||
// should be run between the rate controller and the servo updates.
|
// 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
|
// Use Leaky_I if we are not moving fast
|
||||||
attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
|
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
|
// 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
|
// 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) {
|
switch(control_mode) {
|
||||||
case ACRO:
|
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
|
// 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
|
// get rotor control method
|
||||||
uint8_t rsc_control_mode = motors.get_rsc_mode();
|
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
|
// 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);
|
motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in);
|
||||||
}
|
}
|
||||||
|
@ -1,11 +1,14 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
/*
|
/*
|
||||||
* heli_control_acro.pde - init and run calls for acro flight mode for trad heli
|
* heli_control_acro.pde - init and run calls for acro flight mode for trad heli
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// heli_acro_init - initialise acro controller
|
// 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
|
// 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());
|
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
|
// heli_acro_run - runs the acro controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void heli_acro_run()
|
void Copter::heli_acro_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch, target_yaw;
|
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
|
// 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
|
// 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
|
// calculate rate request
|
||||||
float rate_bf_yaw_request = yaw_in * g.acro_yaw_p;
|
float rate_bf_yaw_request = yaw_in * g.acro_yaw_p;
|
||||||
|
@ -1,11 +1,14 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
/*
|
/*
|
||||||
* heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli
|
* heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// stabilize_init - initialise stabilize controller
|
// 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
|
// 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?
|
// 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
|
// stabilize_run - runs the main stabilize controller
|
||||||
// should be called at 100hz or more
|
// should be called at 100hz or more
|
||||||
static void heli_stabilize_run()
|
void Copter::heli_stabilize_run()
|
||||||
{
|
{
|
||||||
float target_roll, target_pitch;
|
float target_roll, target_pitch;
|
||||||
float target_yaw_rate;
|
float target_yaw_rate;
|
||||||
|
@ -1,14 +1,16 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
// read_inertia - read inertia in from accelerometers
|
// read_inertia - read inertia in from accelerometers
|
||||||
static void read_inertia()
|
void Copter::read_inertia()
|
||||||
{
|
{
|
||||||
// inertial altitude estimates
|
// inertial altitude estimates
|
||||||
inertial_nav.update(G_Dt);
|
inertial_nav.update(G_Dt);
|
||||||
}
|
}
|
||||||
|
|
||||||
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
|
// 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
|
// exit immediately if we do not have an altitude estimate
|
||||||
if (!inertial_nav.get_filter_status().flags.vert_pos) {
|
if (!inertial_nav.get_filter_status().flags.vert_pos) {
|
||||||
|
@ -1,17 +1,20 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
|
|
||||||
// counter to verify landings
|
// counter to verify landings
|
||||||
static uint32_t land_detector = ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE; // we assume we are landed
|
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)
|
// 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);
|
return (ap.land_complete || ap.land_complete_maybe);
|
||||||
}
|
}
|
||||||
|
|
||||||
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
|
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
|
||||||
// called at MAIN_LOOP_RATE
|
// 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
|
// land detector can not use the following sensors because they are unreliable during landing
|
||||||
// barometer altitude : ground effect can cause errors larger than 4m
|
// 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
|
// 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
|
// 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
|
// 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)) {
|
if (mode_has_manual_throttle(control_mode)) {
|
||||||
// manual throttle
|
// manual throttle
|
||||||
|
@ -1,7 +1,10 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
|
|
||||||
// Run landing gear controller at 10Hz
|
// Run landing gear controller at 10Hz
|
||||||
static void landinggear_update(){
|
void Copter::landinggear_update(){
|
||||||
|
|
||||||
// If landing gear control is active, run update function.
|
// If landing gear control is active, run update function.
|
||||||
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
|
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
|
||||||
|
@ -1,8 +1,11 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
|
|
||||||
// updates the status of notify
|
// updates the status of notify
|
||||||
// should be called at 50hz
|
// should be called at 50hz
|
||||||
static void update_notify()
|
void Copter::update_notify()
|
||||||
{
|
{
|
||||||
notify.update();
|
notify.update();
|
||||||
}
|
}
|
||||||
|
65
ArduCopter/make.inc
Normal file
65
ArduCopter/make.inc
Normal 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
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
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.
|
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
|
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
|
// 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
|
// exit immediately if the motor test is not running
|
||||||
if (!ap.motor_test) {
|
if (!ap.motor_test) {
|
||||||
@ -67,7 +69,7 @@ static void motor_test_output()
|
|||||||
|
|
||||||
// mavlink_motor_test_check - perform checks before motor tests can begin
|
// mavlink_motor_test_check - perform checks before motor tests can begin
|
||||||
// return true if tests can continue, false if not
|
// 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
|
// check rc has been calibrated
|
||||||
pre_arm_rc_checks();
|
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
|
// 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
|
// 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 test has not started try to start it
|
||||||
if (!ap.motor_test) {
|
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
|
// 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
|
// exit immediately if the test is not running
|
||||||
if (!ap.motor_test) {
|
if (!ap.motor_test) {
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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 ARM_DELAY 20 // called at 10hz so 2 seconds
|
||||||
#define DISARM_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
|
#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
|
// arm_motors_check - checks for pilot input to arm or disarm the copter
|
||||||
// called at 10hz
|
// called at 10hz
|
||||||
static void arm_motors_check()
|
void Copter::arm_motors_check()
|
||||||
{
|
{
|
||||||
static int16_t arming_counter;
|
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
|
// 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
|
// 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,
|
// exit immediately if we are already disarmed or throttle output is not zero,
|
||||||
if (!motors.armed() || !ap.throttle_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
|
// 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
|
// obvious the copter is armed as the motors will not be spinning
|
||||||
if (ap.using_interlock || ap.motor_emergency_stop){
|
if (ap.using_interlock || ap.motor_emergency_stop){
|
||||||
delay = AUTO_DISARMING_DELAY_SHORT;
|
disarm_delay = AUTO_DISARMING_DELAY_SHORT;
|
||||||
} else {
|
} 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();
|
init_disarm_motors();
|
||||||
auto_disarming_counter = 0;
|
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
|
// 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
|
// 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
|
// arming marker
|
||||||
// Flag used to track if we have armed the motors the first time.
|
// 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
|
// perform pre-arm checks and set ap.pre_arm_check flag
|
||||||
// return true if the checks pass successfully
|
// 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
|
// exit immediately if already armed
|
||||||
if (motors.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
|
// 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
|
// exit immediately if we've already successfully performed the pre-arm rc check
|
||||||
if( ap.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
|
// 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
|
// always check if inertial nav has started and is ready
|
||||||
if(!ahrs.get_NavEKF().healthy()) {
|
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
|
// arm_checks - perform final checks before arming
|
||||||
// always called just before arming. Return true if ok to arm
|
// always called just before arming. Return true if ok to arm
|
||||||
// has side-effect that logging is started
|
// 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
|
#if LOGGING_ENABLED == ENABLED
|
||||||
// start dataflash
|
// start dataflash
|
||||||
@ -760,7 +762,7 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// init_disarm_motors - disarm motors
|
// init_disarm_motors - disarm motors
|
||||||
static void init_disarm_motors()
|
void Copter::init_disarm_motors()
|
||||||
{
|
{
|
||||||
// return immediately if we are already disarmed
|
// return immediately if we are already disarmed
|
||||||
if (!motors.armed()) {
|
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
|
// 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
|
// check if we are performing the motor test
|
||||||
if (ap.motor_test) {
|
if (ap.motor_test) {
|
||||||
@ -824,7 +826,7 @@ static void motors_output()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// check for pilot stick input to trigger lost vehicle alarm
|
// 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;
|
static uint8_t soundalarm_counter;
|
||||||
|
|
||||||
|
@ -1,9 +1,11 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
// run_nav_updates - top level call for the autopilot
|
||||||
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
|
// 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
|
// 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
|
// fetch position from inertial navigation
|
||||||
calc_position();
|
calc_position();
|
||||||
@ -16,7 +18,7 @@ static void run_nav_updates(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// calc_position - get lat and lon positions from inertial nav library
|
// 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
|
// pull position from interial nav library
|
||||||
current_loc.lng = inertial_nav.get_longitude();
|
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
|
// 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_distance();
|
||||||
calc_wp_bearing();
|
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
|
// 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
|
// get target from loiter or wpinav controller
|
||||||
if (control_mode == LOITER || control_mode == CIRCLE) {
|
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
|
// 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
|
// get target from loiter or wpinav controller
|
||||||
if (control_mode == LOITER || control_mode == CIRCLE) {
|
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
|
// 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();
|
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
|
// run_autopilot - highest level call to process mission commands
|
||||||
static void run_autopilot()
|
void Copter::run_autopilot()
|
||||||
{
|
{
|
||||||
if (control_mode == AUTO) {
|
if (control_mode == AUTO) {
|
||||||
// update state of mission
|
// update state of mission
|
||||||
|
@ -1,4 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
//
|
//
|
||||||
// high level performance monitoring
|
// high level performance monitoring
|
||||||
//
|
//
|
||||||
@ -15,7 +18,7 @@ static uint16_t perf_info_long_running;
|
|||||||
static bool perf_ignore_loop = false;
|
static bool perf_ignore_loop = false;
|
||||||
|
|
||||||
// perf_info_reset - reset all records of loop time to zero
|
// 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_loop_count = 0;
|
||||||
perf_info_max_time = 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)
|
// 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_ignore_loop = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold
|
// 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++;
|
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
|
// 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;
|
return perf_info_loop_count;
|
||||||
}
|
}
|
||||||
|
|
||||||
// perf_info_get_max_time - return maximum loop time (in microseconds)
|
// 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;
|
return perf_info_max_time;
|
||||||
}
|
}
|
||||||
|
|
||||||
// perf_info_get_max_time - return maximum loop time (in microseconds)
|
// 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;
|
return perf_info_min_time;
|
||||||
}
|
}
|
||||||
|
|
||||||
// perf_info_get_num_long_running - get number of long running loops
|
// 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;
|
return perf_info_long_running;
|
||||||
}
|
}
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
// position_vector.pde related utility functions
|
// position_vector.pde related utility functions
|
||||||
|
|
||||||
// position vectors are Vector3f
|
// position vectors are Vector3f
|
||||||
@ -8,7 +10,7 @@
|
|||||||
// .z = altitude above home in cm
|
// .z = altitude above home in cm
|
||||||
|
|
||||||
// pv_location_to_vector - convert lat/lon coordinates to a position vector
|
// 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();
|
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
|
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,
|
// 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
|
// 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);
|
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
|
// 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();
|
const struct Location &origin = inertial_nav.get_origin();
|
||||||
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
|
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
|
||||||
}
|
}
|
||||||
|
|
||||||
// pv_alt_above_home - convert altitude above EKF origin to altitude above home
|
// 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();
|
const struct Location &origin = inertial_nav.get_origin();
|
||||||
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
|
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
|
||||||
}
|
}
|
||||||
|
|
||||||
// pv_get_bearing_cd - return bearing in centi-degrees between two positions
|
// 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;
|
float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
|
||||||
if (bearing < 0) {
|
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
|
// 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);
|
return pythagorous2(destination.x-origin.x,destination.y-origin.y);
|
||||||
}
|
}
|
||||||
|
@ -1,9 +1,12 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
// 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_roll->set_default_dead_zone(30);
|
||||||
channel_pitch->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);
|
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_roll = RC_Channel::rc_channel(rcmap.roll()-1);
|
||||||
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-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
|
// 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_update_rate(g.rc_speed);
|
||||||
motors.set_frame_orientation(g.frame_orientation);
|
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
|
// enable_motor_output() - enable and output lowest possible value to motors
|
||||||
void enable_motor_output()
|
void Copter::enable_motor_output()
|
||||||
{
|
{
|
||||||
// enable motors
|
// enable motors
|
||||||
motors.enable();
|
motors.enable();
|
||||||
motors.output_min();
|
motors.output_min();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void read_radio()
|
void Copter::read_radio()
|
||||||
{
|
{
|
||||||
static uint32_t last_update_ms = 0;
|
static uint32_t last_update_ms = 0;
|
||||||
uint32_t tnow_ms = millis();
|
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
|
#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 failsafe not enabled pass through throttle and exit
|
||||||
if(g.failsafe_throttle == FS_THR_DISABLED) {
|
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
|
// 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
|
// 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
|
// 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;
|
static uint32_t last_nonzero_throttle_ms = 0;
|
||||||
uint32_t tnow_ms = millis();
|
uint32_t tnow_ms = millis();
|
||||||
|
@ -1,6 +1,8 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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"));
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
|
||||||
if (full_calibration) {
|
if (full_calibration) {
|
||||||
@ -12,7 +14,7 @@ static void init_barometer(bool full_calibration)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return barometric altitude in centimeters
|
// return barometric altitude in centimeters
|
||||||
static void read_barometer(void)
|
void Copter::read_barometer(void)
|
||||||
{
|
{
|
||||||
barometer.update();
|
barometer.update();
|
||||||
if (should_log(MASK_LOG_IMU)) {
|
if (should_log(MASK_LOG_IMU)) {
|
||||||
@ -25,14 +27,14 @@ static void read_barometer(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
#if CONFIG_SONAR == ENABLED
|
#if CONFIG_SONAR == ENABLED
|
||||||
static void init_sonar(void)
|
void Copter::init_sonar(void)
|
||||||
{
|
{
|
||||||
sonar.init();
|
sonar.init();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// return sonar altitude in centimeters
|
// return sonar altitude in centimeters
|
||||||
static int16_t read_sonar(void)
|
int16_t Copter::read_sonar(void)
|
||||||
{
|
{
|
||||||
#if CONFIG_SONAR == ENABLED
|
#if CONFIG_SONAR == ENABLED
|
||||||
sonar.update();
|
sonar.update();
|
||||||
@ -68,7 +70,7 @@ static int16_t read_sonar(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// initialise compass
|
// initialise compass
|
||||||
static void init_compass()
|
void Copter::init_compass()
|
||||||
{
|
{
|
||||||
if (!compass.init() || !compass.read()) {
|
if (!compass.init() || !compass.read()) {
|
||||||
// make sure we don't pass a broken compass to DCM
|
// make sure we don't pass a broken compass to DCM
|
||||||
@ -80,7 +82,7 @@ static void init_compass()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// initialise optical flow sensor
|
// initialise optical flow sensor
|
||||||
static void init_optflow()
|
void Copter::init_optflow()
|
||||||
{
|
{
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
// exit immediately if not enabled
|
// exit immediately if not enabled
|
||||||
@ -95,7 +97,7 @@ static void init_optflow()
|
|||||||
|
|
||||||
// called at 200hz
|
// called at 200hz
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
static void update_optical_flow(void)
|
void Copter::update_optical_flow(void)
|
||||||
{
|
{
|
||||||
static uint32_t last_of_update = 0;
|
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
|
// read_battery - check battery voltage and current and invoke failsafe if necessary
|
||||||
// called at 10hz
|
// called at 10hz
|
||||||
static void read_battery(void)
|
void Copter::read_battery(void)
|
||||||
{
|
{
|
||||||
battery.read();
|
battery.read();
|
||||||
|
|
||||||
@ -154,7 +156,7 @@ static void read_battery(void)
|
|||||||
|
|
||||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||||
// RC_CHANNELS_SCALED message
|
// RC_CHANNELS_SCALED message
|
||||||
void read_receiver_rssi(void)
|
void Copter::read_receiver_rssi(void)
|
||||||
{
|
{
|
||||||
// avoid divide by zero
|
// avoid divide by zero
|
||||||
if (g.rssi_range <= 0) {
|
if (g.rssi_range <= 0) {
|
||||||
@ -168,7 +170,7 @@ void read_receiver_rssi(void)
|
|||||||
|
|
||||||
#if EPM_ENABLED == ENABLED
|
#if EPM_ENABLED == ENABLED
|
||||||
// epm update - moves epm pwm output back to neutral after grab or release is completed
|
// epm update - moves epm pwm output back to neutral after grab or release is completed
|
||||||
void epm_update()
|
void Copter::epm_update()
|
||||||
{
|
{
|
||||||
epm.update();
|
epm.update();
|
||||||
}
|
}
|
||||||
|
@ -1,28 +1,20 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#if CLI_ENABLED == ENABLED
|
||||||
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||||
#define WITH_ESC_CALIB
|
#define WITH_ESC_CALIB
|
||||||
#endif
|
#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
|
// Command/function table for the setup menu
|
||||||
const struct Menu::command setup_menu_commands[] PROGMEM = {
|
static const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||||
// command function called
|
{"reset", MENU_FUNC(setup_factory)},
|
||||||
// ======= ===============
|
{"show", MENU_FUNC(setup_show)},
|
||||||
{"reset", setup_factory},
|
{"set", MENU_FUNC(setup_set)},
|
||||||
{"show", setup_show},
|
|
||||||
{"set", setup_set},
|
|
||||||
#ifdef WITH_ESC_CALIB
|
#ifdef WITH_ESC_CALIB
|
||||||
{"esc_calib", esc_calib},
|
{"esc_calib", MENU_FUNC(esc_calib)},
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -30,8 +22,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
|
|||||||
MENU(setup_menu, "setup", setup_menu_commands);
|
MENU(setup_menu, "setup", setup_menu_commands);
|
||||||
|
|
||||||
// Called from the top-level menu to run the setup menu.
|
// Called from the top-level menu to run the setup menu.
|
||||||
static int8_t
|
int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv)
|
||||||
setup_mode(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
// Give the user some guidance
|
// Give the user some guidance
|
||||||
cliSerial->printf_P(PSTR("Setup Mode\n\n\n"));
|
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).
|
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
||||||
// Called by the setup menu 'factoryreset' command.
|
// Called by the setup menu 'factoryreset' command.
|
||||||
static int8_t
|
int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||||
setup_factory(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
int16_t c;
|
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
|
//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
|
//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;
|
int8_t value_int8;
|
||||||
int16_t value_int16;
|
int16_t value_int16;
|
||||||
@ -129,8 +119,7 @@ static int8_t setup_set(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
// Print the current configuration.
|
// Print the current configuration.
|
||||||
// Called by the setup menu 'show' command.
|
// Called by the setup menu 'show' command.
|
||||||
static int8_t
|
int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv)
|
||||||
setup_show(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
AP_Param *param;
|
AP_Param *param;
|
||||||
ap_var_type type;
|
ap_var_type type;
|
||||||
@ -175,8 +164,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
|||||||
#define PWM_HIGHEST_MIN 1800
|
#define PWM_HIGHEST_MIN 1800
|
||||||
#define PWM_LOWEST_MIN 800
|
#define PWM_LOWEST_MIN 800
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
|
||||||
esc_calib(uint8_t argc,const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
@ -318,7 +306,7 @@ esc_calib(uint8_t argc,const Menu::arg *argv)
|
|||||||
// CLI reports
|
// CLI reports
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
|
|
||||||
static void report_batt_monitor()
|
void Copter::report_batt_monitor()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
|
cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -332,7 +320,7 @@ static void report_batt_monitor()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void report_frame()
|
void Copter::report_frame()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Frame\n"));
|
cliSerial->printf_P(PSTR("Frame\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -354,7 +342,7 @@ static void report_frame()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void report_radio()
|
void Copter::report_radio()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Radio\n"));
|
cliSerial->printf_P(PSTR("Radio\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -363,7 +351,7 @@ static void report_radio()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void report_ins()
|
void Copter::report_ins()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("INS\n"));
|
cliSerial->printf_P(PSTR("INS\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -373,7 +361,7 @@ static void report_ins()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void report_flight_modes()
|
void Copter::report_flight_modes()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Flight modes\n"));
|
cliSerial->printf_P(PSTR("Flight modes\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -384,7 +372,7 @@ static void report_flight_modes()
|
|||||||
print_blanks(2);
|
print_blanks(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
void report_optflow()
|
void Copter::report_optflow()
|
||||||
{
|
{
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
cliSerial->printf_P(PSTR("OptFlow\n"));
|
cliSerial->printf_P(PSTR("OptFlow\n"));
|
||||||
@ -400,8 +388,7 @@ void report_optflow()
|
|||||||
// CLI utilities
|
// CLI utilities
|
||||||
/***************************************************************************/
|
/***************************************************************************/
|
||||||
|
|
||||||
static void
|
void Copter::print_radio_values()
|
||||||
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("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);
|
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);
|
cliSerial->printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::print_switch(uint8_t p, uint8_t m, bool b)
|
||||||
print_switch(uint8_t p, uint8_t m, bool b)
|
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Pos %d:\t"),p);
|
cliSerial->printf_P(PSTR("Pos %d:\t"),p);
|
||||||
print_flight_mode(cliSerial, m);
|
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"));
|
cliSerial->printf_P(PSTR("OFF\n"));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::print_accel_offsets_and_scaling(void)
|
||||||
print_accel_offsets_and_scaling(void)
|
|
||||||
{
|
{
|
||||||
const Vector3f &accel_offsets = ins.get_accel_offsets();
|
const Vector3f &accel_offsets = ins.get_accel_offsets();
|
||||||
const Vector3f &accel_scale = ins.get_accel_scale();
|
const Vector3f &accel_scale = ins.get_accel_scale();
|
||||||
@ -439,8 +424,7 @@ print_accel_offsets_and_scaling(void)
|
|||||||
(double)accel_scale.z); // YAW
|
(double)accel_scale.z); // YAW
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::print_gyro_offsets(void)
|
||||||
print_gyro_offsets(void)
|
|
||||||
{
|
{
|
||||||
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
|
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
|
||||||
cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
|
cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
|
||||||
@ -452,7 +436,7 @@ print_gyro_offsets(void)
|
|||||||
#endif // CLI_ENABLED
|
#endif // CLI_ENABLED
|
||||||
|
|
||||||
// report_compass - displays compass information. Also called by compassmot.pde
|
// report_compass - displays compass information. Also called by compassmot.pde
|
||||||
static void report_compass()
|
void Copter::report_compass()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Compass\n"));
|
cliSerial->printf_P(PSTR("Compass\n"));
|
||||||
print_divider();
|
print_divider();
|
||||||
@ -499,8 +483,7 @@ static void report_compass()
|
|||||||
print_blanks(1);
|
print_blanks(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::print_blanks(int16_t num)
|
||||||
print_blanks(int16_t num)
|
|
||||||
{
|
{
|
||||||
while(num > 0) {
|
while(num > 0) {
|
||||||
num--;
|
num--;
|
||||||
@ -508,8 +491,7 @@ print_blanks(int16_t num)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void
|
void Copter::print_divider(void)
|
||||||
print_divider(void)
|
|
||||||
{
|
{
|
||||||
for (int i = 0; i < 40; i++) {
|
for (int i = 0; i < 40; i++) {
|
||||||
cliSerial->print_P(PSTR("-"));
|
cliSerial->print_P(PSTR("-"));
|
||||||
@ -517,7 +499,7 @@ print_divider(void)
|
|||||||
cliSerial->println();
|
cliSerial->println();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void print_enabled(bool b)
|
void Copter::print_enabled(bool b)
|
||||||
{
|
{
|
||||||
if(b)
|
if(b)
|
||||||
cliSerial->print_P(PSTR("en"));
|
cliSerial->print_P(PSTR("en"));
|
||||||
@ -526,7 +508,7 @@ static void print_enabled(bool b)
|
|||||||
cliSerial->print_P(PSTR("abled\n"));
|
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);
|
cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
|
||||||
print_divider();
|
print_divider();
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
|
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
|
||||||
|
|
||||||
//Documentation of Aux Switch Flags:
|
//Documentation of Aux Switch Flags:
|
||||||
@ -16,7 +18,7 @@ static union {
|
|||||||
uint32_t value;
|
uint32_t value;
|
||||||
} aux_con;
|
} aux_con;
|
||||||
|
|
||||||
static void read_control_switch()
|
void Copter::read_control_switch()
|
||||||
{
|
{
|
||||||
uint32_t tnow_ms = millis();
|
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.
|
// 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
|
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;
|
|| 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
|
// 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 ||
|
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 ||
|
g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option ||
|
||||||
@ -103,14 +105,14 @@ static bool check_duplicate_auxsw(void)
|
|||||||
return ret;
|
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;
|
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
|
||||||
read_control_switch();
|
read_control_switch();
|
||||||
}
|
}
|
||||||
|
|
||||||
// read_3pos_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_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
|
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
|
// 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;
|
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
|
// 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
|
// set the CH7 ~ CH12 flags
|
||||||
aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in);
|
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
|
// 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
|
// init channel options
|
||||||
switch(ch_option) {
|
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
|
// 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) {
|
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
|
// 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
|
// save roll and pitch trim
|
||||||
float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
|
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
|
// 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
|
// 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) {
|
if(auto_trim_counter > 0) {
|
||||||
auto_trim_counter--;
|
auto_trim_counter--;
|
||||||
|
@ -1,4 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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
|
* 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
|
* 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);
|
static int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
|
||||||
|
|
||||||
// This is the help function
|
// 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"
|
cliSerial->printf_P(PSTR("Commands:\n"
|
||||||
" logs\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 = {
|
const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||||
// command function called
|
// command function called
|
||||||
// ======= ===============
|
// ======= ===============
|
||||||
{"logs", process_logs},
|
{"logs", MENU_FUNC(process_logs)},
|
||||||
{"setup", setup_mode},
|
{"setup", MENU_FUNC(setup_mode)},
|
||||||
{"test", test_mode},
|
{"test", MENU_FUNC(test_mode)},
|
||||||
{"reboot", reboot_board},
|
{"reboot", MENU_FUNC(reboot_board)},
|
||||||
{"help", main_menu_help},
|
{"help", MENU_FUNC(main_menu_help)},
|
||||||
};
|
};
|
||||||
|
|
||||||
// Create the top-level menu object.
|
// Create the top-level menu object.
|
||||||
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
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);
|
hal.scheduler->reboot(false);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// the user wants the CLI. It never exits
|
// 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;
|
cliSerial = port;
|
||||||
Menu::set_port(port);
|
Menu::set_port(port);
|
||||||
@ -71,7 +74,18 @@ static void run_cli(AP_HAL::UARTDriver *port)
|
|||||||
|
|
||||||
#endif // CLI_ENABLED
|
#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()) {
|
if (!hal.gpio->usb_connected()) {
|
||||||
// USB is not connected, this means UART0 may be a Xbee, with
|
// 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
|
// Register the mavlink service callback. This will run
|
||||||
// anytime there are more than 5ms remaining in a call to
|
// anytime there are more than 5ms remaining in a call to
|
||||||
// hal.scheduler->delay.
|
// 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
|
// we start by assuming USB connected, as we initialed the serial
|
||||||
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
|
// 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;
|
mavlink_system.sysid = g.sysid_this_mav;
|
||||||
|
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
|
log_init();
|
||||||
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
|
#endif
|
||||||
|
|
||||||
init_rc_in(); // sets up rc channels from radio
|
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
|
* setup the 'main loop is dead' check. Note that this relies on
|
||||||
* the RC library being initialised.
|
* 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
|
// Do GPS init
|
||||||
gps.init(&DataFlash, serial_manager);
|
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
|
//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"));
|
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
|
// 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()) {
|
if (!ahrs.have_inertial_nav()) {
|
||||||
// do not allow navigation with dcm position
|
// 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
|
// 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
|
#if OPTFLOW != ENABLED
|
||||||
return false;
|
return false;
|
||||||
@ -348,7 +354,7 @@ static bool optflow_position_ok()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// update_auto_armed - update status of auto_armed flag
|
// update_auto_armed - update status of auto_armed flag
|
||||||
static void update_auto_armed()
|
void Copter::update_auto_armed()
|
||||||
{
|
{
|
||||||
// disarm checks
|
// disarm checks
|
||||||
if(ap.auto_armed){
|
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();
|
bool usb_check = hal.gpio->usb_connected();
|
||||||
if (usb_check == ap.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
|
// frsky_telemetry_send - sends telemetry data using frsky telemetry
|
||||||
// should be called at 5Hz by scheduler
|
// should be called at 5Hz by scheduler
|
||||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
#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);
|
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?
|
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 LOGGING_ENABLED == ENABLED
|
||||||
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- 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.
|
// 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
|
// 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
|
// 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
|
// return true if this flight mode supports user takeoff
|
||||||
// must_nagivate is true if mode must also control horizontal position
|
// 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) {
|
switch (control_mode) {
|
||||||
case GUIDED:
|
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
|
// 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) {
|
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
|
||||||
switch(control_mode) {
|
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
|
// start takeoff to specified altitude above home
|
||||||
static void takeoff_timer_start(float alt)
|
void Copter::takeoff_timer_start(float alt)
|
||||||
{
|
{
|
||||||
// calculate climb rate
|
// 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));
|
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
|
// stop takeoff
|
||||||
static void takeoff_stop()
|
void Copter::takeoff_stop()
|
||||||
{
|
{
|
||||||
takeoff_state.running = false;
|
takeoff_state.running = false;
|
||||||
takeoff_state.start_ms = 0;
|
takeoff_state.start_ms = 0;
|
||||||
@ -72,7 +74,7 @@ static void takeoff_stop()
|
|||||||
// pilot_climb_rate is both an input and an output
|
// pilot_climb_rate is both an input and an output
|
||||||
// takeoff_climb_rate is only an output
|
// takeoff_climb_rate is only an output
|
||||||
// has side-effect of turning takeoff off when timeout as expired
|
// 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
|
// return pilot_climb_rate if take-off inactive
|
||||||
if (!takeoff_state.running) {
|
if (!takeoff_state.running) {
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
|
#include "Copter.h"
|
||||||
|
|
||||||
#if CLI_ENABLED == ENABLED
|
#if CLI_ENABLED == ENABLED
|
||||||
|
|
||||||
// These are function definitions so the Menu can be constructed before the functions
|
// 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.
|
// and stores them in Flash memory, not RAM.
|
||||||
// User enters the string in the console to call the functions on the right.
|
// User enters the string in the console to call the functions on the right.
|
||||||
// See class Menu in AP_Coommon for implementation details
|
// 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
|
#if HIL_MODE == HIL_MODE_DISABLED
|
||||||
{"baro", test_baro},
|
{"baro", MENU_FUNC(test_baro)},
|
||||||
#endif
|
#endif
|
||||||
{"compass", test_compass},
|
{"compass", MENU_FUNC(test_compass)},
|
||||||
{"ins", test_ins},
|
{"ins", MENU_FUNC(test_ins)},
|
||||||
{"optflow", test_optflow},
|
{"optflow", MENU_FUNC(test_optflow)},
|
||||||
{"relay", test_relay},
|
{"relay", MENU_FUNC(test_relay)},
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||||
{"shell", test_shell},
|
{"shell", MENU_FUNC(test_shell)},
|
||||||
#endif
|
#endif
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
#if HIL_MODE == HIL_MODE_DISABLED
|
||||||
{"rangefinder", test_sonar},
|
{"rangefinder", MENU_FUNC(test_sonar)},
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
// A Macro to create the Menu
|
// A Macro to create the Menu
|
||||||
MENU(test_menu, "test", test_menu_commands);
|
MENU(test_menu, "test", test_menu_commands);
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::test_mode(uint8_t argc, const Menu::arg *argv)
|
||||||
test_mode(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
test_menu.run();
|
test_menu.run();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HIL_MODE == HIL_MODE_DISABLED
|
#if HIL_MODE == HIL_MODE_DISABLED
|
||||||
static int8_t
|
int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv)
|
||||||
test_baro(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
init_barometer(true);
|
init_barometer(true);
|
||||||
@ -75,8 +75,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv)
|
||||||
test_compass(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
uint8_t delta_ms_fast_loop;
|
uint8_t delta_ms_fast_loop;
|
||||||
uint8_t medium_loopCounter = 0;
|
uint8_t medium_loopCounter = 0;
|
||||||
@ -162,8 +161,7 @@ test_compass(uint8_t argc, const Menu::arg *argv)
|
|||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv)
|
||||||
test_ins(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
Vector3f gyro, accel;
|
Vector3f gyro, accel;
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
@ -196,8 +194,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static int8_t
|
int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv)
|
||||||
test_optflow(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
#if OPTFLOW == ENABLED
|
#if OPTFLOW == ENABLED
|
||||||
if(optflow.enabled()) {
|
if(optflow.enabled()) {
|
||||||
@ -227,7 +224,7 @@ test_optflow(uint8_t argc, const Menu::arg *argv)
|
|||||||
#endif // OPTFLOW == ENABLED
|
#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();
|
print_hit_enter();
|
||||||
delay(1000);
|
delay(1000);
|
||||||
@ -253,8 +250,7 @@ static int8_t test_relay(uint8_t argc, const Menu::arg *argv)
|
|||||||
/*
|
/*
|
||||||
* run a debug shell
|
* run a debug shell
|
||||||
*/
|
*/
|
||||||
static int8_t
|
int8_t Copter::test_shell(uint8_t argc, const Menu::arg *argv)
|
||||||
test_shell(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
hal.util->run_debug_shell(cliSerial);
|
hal.util->run_debug_shell(cliSerial);
|
||||||
return 0;
|
return 0;
|
||||||
@ -265,8 +261,7 @@ test_shell(uint8_t argc, const Menu::arg *argv)
|
|||||||
/*
|
/*
|
||||||
* test the rangefinders
|
* test the rangefinders
|
||||||
*/
|
*/
|
||||||
static int8_t
|
int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||||
test_sonar(uint8_t argc, const Menu::arg *argv)
|
|
||||||
{
|
{
|
||||||
#if CONFIG_SONAR == ENABLED
|
#if CONFIG_SONAR == ENABLED
|
||||||
sonar.init();
|
sonar.init();
|
||||||
@ -291,7 +286,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static void print_hit_enter()
|
void Copter::print_hit_enter()
|
||||||
{
|
{
|
||||||
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
|
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
|
||||||
}
|
}
|
||||||
|
@ -1,5 +1,7 @@
|
|||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
/// -*- 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
|
* 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
|
* 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
|
// tuning - updates parameters based on the ch6 tuning knob's position
|
||||||
// should be called at 3.3hz
|
// 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
|
// 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) {
|
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0) {
|
||||||
|
Loading…
Reference in New Issue
Block a user