/* 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 . */ /* * 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 HAL_PROXIMITY_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); } // set target position and velocity (for use by scripting) bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f); const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f); return mode_guided.set_destination_posvel(pos_neu_cm, vel_neu_cms); } 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() || !flightmode->has_manual_throttle())) { pos_control->write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { AP::ins().Write_Vibration(); } if (should_log(MASK_LOG_CTUN)) { attitude_control->control_monitor_log(); #if HAL_PROXIMITY_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)) { AP::ins().Write_IMU(); } #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), 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);