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

671 lines
22 KiB
C++
Raw Blame History

/*
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/>.
*/
/*
* ArduCopter (also known as APM, APM:Copter or just Copter)
* Wiki: copter.ardupilot.org
* Creator: Jason Short
* Lead Developer: Randy Mackay
* Lead Tester: Marco Robustini
* Based on code and ideas from the Arducopter team: Leonard Hall, Andrew Tridgell, Robert Lefebvre, Pat Hickey, Michael Oborne, Jani Hirvinen,
Olivier Adler, Kevin Hester, Arthur Benemann, Jonathan Challinger, John Arne Birkeland,
Jean-Louis Naudin, Mike Smith, and more
* Thanks to: Chris Anderson, Jordi Munoz, Jason Short, Doug Weibel, Jose Julio
*
* Special Thanks to contributors (in alphabetical order by first name):
*
* Adam M Rivera :Auto Compass Declination
* Amilcar Lucas :Camera mount library
* Andrew Tridgell :General development, Mavlink Support
* Andy Piper :Harmonic notch, In-flight FFT, Bi-directional DShot, various drivers
* Angel Fernandez :Alpha testing
* AndreasAntonopoulous:GeoFence
* Arthur Benemann :DroidPlanner GCS
* Benjamin Pelletier :Libraries
* Bill King :Single Copter
* Christof Schmid :Alpha testing
* Craig Elder :Release Management, Support
* Dani Saez :V Octo Support
* Doug Weibel :DCM, Libraries, Control law advice
* Emile Castelnuovo :VRBrain port, bug fixes
* Gregory Fletcher :Camera mount orientation math
* Guntars :Arming safety suggestion
* HappyKillmore :Mavlink GCS
* Hein Hollander :Octo Support, Heli Testing
* Igor van Airde :Control Law optimization
* Jack Dunkle :Alpha testing
* James Goppert :Mavlink Support
* Jani Hiriven :Testing feedback
* Jean-Louis Naudin :Auto Landing
* John Arne Birkeland :PPM Encoder
* Jose Julio :Stabilization Control laws, MPU6k driver
* Julien Dubois :PosHold flight mode
* Julian Oes :Pixhawk
* Jonathan Challinger :Inertial Navigation, CompassMot, Spin-When-Armed
* Kevin Hester :Andropilot GCS
* Max Levine :Tri Support, Graphics
* Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
* Marco Robustini :Lead tester
* Michael Oborne :Mission Planner GCS
* Mike Smith :Pixhawk driver, coding support
* Olivier Adler :PPM Encoder, piezo buzzer
* Pat Hickey :Hardware Abstraction Layer (HAL)
* Robert Lefebvre :Heli Support, Copter LEDs
* Roberto Navoni :Library testing, Porting to VRBrain
* Sandro Benigno :Camera support, MinimOSD
* Sandro Tognana :PosHold flight mode
* Sebastian Quilter :SmartRTL
* ..and many more.
*
* Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors
* Wiki: https://copter.ardupilot.org/
*
*/
#include "Copter.h"
#define FORCE_VERSION_H_INCLUDE
#include "version.h"
#undef FORCE_VERSION_H_INCLUDE
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
/*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called (in hz)
and the maximum time they are expected to take (in microseconds)
*/
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK_CLASS(OpticalFlow, &copter.optflow, update, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
#if TOY_MODE_ENABLED == ENABLED
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50),
#endif
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
#if RANGEFINDER_ENABLED == ENABLED
SCHED_TASK(read_rangefinder, 20, 100),
#endif
#if PROXIMITY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50),
#endif
#if BEACON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
#endif
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
#if MODE_SMARTRTL_ENABLED == ENABLED
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100),
#endif
#if SPRAYER_ENABLED == ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90),
#endif
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75),
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90),
#if AC_FENCE == ENABLED
SCHED_TASK_CLASS(AC_Fence, &copter.fence, update, 10, 100),
#endif
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(fourhundred_hz_logging,400, 50),
#endif
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(check_vibration, 10, 50),
SCHED_TASK(gpsglitch_check, 10, 50),
#if LANDING_GEAR_ENABLED == ENABLED
SCHED_TASK(landinggear_update, 10, 75),
#endif
SCHED_TASK(standby_update, 100, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550),
#if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75),
#endif
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
#if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 40, 200),
#endif
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100),
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100),
#endif
#if AC_TERRAIN == ENABLED
SCHED_TASK(terrain_update, 10, 100),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75),
#endif
#if WINCH_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Winch, &copter.g2.winch, update, 50, 50),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
#if BUTTON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100),
#endif
};
void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
uint8_t &task_count,
uint32_t &log_bit)
{
tasks = &scheduler_tasks[0];
task_count = ARRAY_SIZE(scheduler_tasks);
log_bit = MASK_LOG_PM;
}
constexpr int8_t Copter::_failsafe_priorities[7];
// Main loop - 400hz
void Copter::fast_loop()
{
// update INS immediately to get current gyro data populated
ins.update();
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
// send outputs to the motors library immediately
motors_output();
// run EKF state estimator (expensive)
// --------------------
read_AHRS();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#if MODE_AUTOROTATE_ENABLED == ENABLED
heli_update_autorotation();
#endif
#endif //HELI_FRAME
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading or position
check_ekf_reset();
// run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
#if HAL_MOUNT_ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
}
AP_Vehicle::fast_loop();
}
// start takeoff to given altitude (for use by scripting)
bool Copter::start_takeoff(float alt)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
copter.set_auto_armed(true);
return true;
}
return false;
}
// set target location (for use by scripting)
bool Copter::set_target_location(const Location& target_loc)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
return mode_guided.set_destination(target_loc);
}
bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
// convert vector to neu in cm
const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
mode_guided.set_velocity(vel_neu_cms);
return true;
}
bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
{
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!flightmode->in_guided_mode()) {
return false;
}
Quaternion q;
q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false);
return true;
}
// rc_loops - reads user input from transmitter/receiver
// called at 100hz
void Copter::rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
read_radio();
rc().read_mode_switch();
}
// throttle_loop - should be run at 50 hz
// ---------------------------
void Copter::throttle_loop()
{
// update throttle_low_comp value (controls priority of throttle vs attitude control)
update_throttle_mix();
// check auto_armed status
update_auto_armed();
#if FRAME_CONFIG == HELI_FRAME
// update rotor speed
heli_update_rotor_speed_targets();
// update trad heli swash plate movement
heli_update_landing_swash();
#endif
// compensate for ground effect (if enabled)
update_ground_effect_detector();
update_ekf_terrain_height_stable();
}
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
battery.read();
if(AP::compass().enabled()) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors->get_throttle());
compass.set_voltage(battery.voltage());
compass.read();
}
}
// Full rate logging of attitude, rate and pid loops
// should be run at 400hz
void Copter::fourhundred_hz_logging()
{
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
{
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
}
// log EKF attitude data
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_EKF_POS();
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
}
if (should_log(MASK_LOG_RCIN)) {
logger.Write_RCIN();
if (rssi.enabled()) {
logger.Write_RSSI();
}
}
if (should_log(MASK_LOG_RCOUT)) {
logger.Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS())) {
pos_control->write_log();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
logger.Write_Vibration();
}
if (should_log(MASK_LOG_CTUN)) {
attitude_control->control_monitor_log();
#if PROXIMITY_ENABLED == ENABLED
logger.Write_Proximity(g2.proximity); // Write proximity sensor distances
#endif
#if BEACON_ENABLED == ENABLED
logger.Write_Beacon(g2.beacon);
#endif
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
#if WINCH_ENABLED == ENABLED
if (should_log(MASK_LOG_ANY)) {
g2.winch.write_log();
}
#endif
}
// twentyfive_hz_logging - should be run at 25hz
void Copter::twentyfive_hz_logging()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
gcs().send_message(MSG_SERVO_OUTPUT_RAW);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_EKF_POS();
}
if (should_log(MASK_LOG_IMU)) {
logger.Write_IMU();
}
#endif
#if PRECISION_LANDING == ENABLED
// log output
Log_Write_Precland();
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
}
// three_hz_loop - 3.3hz loop
void Copter::three_hz_loop()
{
// check if we've lost contact with the ground station
failsafe_gcs_check();
// check if we've lost terrain data
failsafe_terrain_check();
#if AC_FENCE == ENABLED
// check if we have breached a fence
fence_check();
#endif // AC_FENCE_ENABLED
// update ch6 in flight tuning
tuning();
// check if avoidance should be enabled based on alt
low_alt_avoidance();
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::AP_STATE, ap.value);
}
arming.update();
if (!motors->armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.update_orientation();
update_using_interlock();
// check the user hasn't updated the frame class or type
motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
#if FRAME_CONFIG != HELI_FRAME
// set all throttle channel settings
motors->set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
}
// update assigned functions and enable auxiliary servos
SRV_Channels::enable_aux_servos();
// log terrain data
terrain_logging();
#if HAL_ADSB_ENABLED
adsb.set_is_flying(!ap.land_complete);
#endif
AP_Notify::flags.flying = !ap.land_complete;
}
void Copter::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values
simple_cos_yaw = ahrs.cos_yaw();
simple_sin_yaw = ahrs.sin_yaw();
// initialise super simple heading (i.e. heading towards home) to be 180 deg from simple mode heading
super_simple_last_bearing = wrap_360_cd(ahrs.yaw_sensor+18000);
super_simple_cos_yaw = simple_cos_yaw;
super_simple_sin_yaw = simple_sin_yaw;
// log the simple bearing
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor);
}
}
// update_simple_mode - rotates pilot input if we are in simple mode
void Copter::update_simple_mode(void)
{
float rollx, pitchx;
// exit immediately if no new radio frame or not in simple mode
if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
return;
}
// mark radio frame as consumed
ap.new_radio_frame = false;
if (simple_mode == SimpleMode::SIMPLE) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
}else{
// rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
}
// rotate roll, pitch input from north facing to vehicle's perspective
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}
// update_super_simple_bearing - adjusts simple bearing based on location
// should be called after home_bearing has been updated
void Copter::update_super_simple_bearing(bool force_update)
{
if (!force_update) {
if (simple_mode != SimpleMode::SUPERSIMPLE) {
return;
}
if (home_distance() < SUPER_SIMPLE_RADIUS) {
return;
}
}
const int32_t bearing = home_bearing();
// check the bearing to home has changed by at least 5 degrees
if (labs(super_simple_last_bearing - bearing) < 500) {
return;
}
super_simple_last_bearing = bearing;
const float angle_rad = radians((super_simple_last_bearing+18000)/100);
super_simple_cos_yaw = cosf(angle_rad);
super_simple_sin_yaw = sinf(angle_rad);
}
void Copter::read_AHRS(void)
{
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs().update();
#endif
// we tell AHRS to skip INS update as we have already done it in fast_loop()
ahrs.update(true);
}
// read baro and log control tuning
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#else
write_notch_log_messages();
#endif
}
}
// vehicle specific waypoint info helpers
bool Copter::get_wp_distance_m(float &distance) const
{
// see GCS_MAVLINK_Copter::send_nav_controller_output()
distance = flightmode->wp_distance() * 0.01;
return true;
}
// vehicle specific waypoint info helpers
bool Copter::get_wp_bearing_deg(float &bearing) const
{
// see GCS_MAVLINK_Copter::send_nav_controller_output()
bearing = flightmode->wp_bearing() * 0.01;
return true;
}
// vehicle specific waypoint info helpers
bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// see GCS_MAVLINK_Copter::send_nav_controller_output()
xtrack_error = flightmode->crosstrack_error() * 0.01;
return true;
}
/*
constructor for main Copter class
*/
Copter::Copter(void)
: logger(g.log_bitmask),
flight_modes(&g.flight_mode1),
control_mode(Mode::Number::STABILIZE),
simple_cos_yaw(1.0f),
super_simple_cos_yaw(1.0),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
rc_throttle_control_in_filter(1.0f),
inertial_nav(ahrs),
param_loader(var_info),
flightmode(&mode_stabilize)
{
// init sensor error logging flags
sensor_health.baro = true;
sensor_health.compass = true;
}
Copter copter;
AP_Vehicle& vehicle = copter;
AP_HAL_MAIN_CALLBACKS(&copter);
Reference in New Issue View Git Blame Copy Permalink