ardupilot/ArduCopter/ArduCopter.cpp

633 lines
20 KiB
C++
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 Version 3.0
* Creator: Jason Short
* Lead Developer: Randy Mackay
2013-11-15 23:10:38 -04:00
* 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
2012-08-21 23:19:50 -03:00
*
2013-11-15 23:10:38 -04:00
* Special Thanks to contributors (in alphabetical order by first name):
2012-08-21 23:19:50 -03:00
*
* Adam M Rivera :Auto Compass Declination
* Amilcar Lucas :Camera mount library
* Andrew Tridgell :General development, Mavlink Support
* Angel Fernandez :Alpha testing
2013-11-15 23:10:38 -04:00
* AndreasAntonopoulous:GeoFence
* Arthur Benemann :DroidPlanner GCS
* Benjamin Pelletier :Libraries
* Bill King :Single Copter
* Christof Schmid :Alpha testing
2013-11-15 23:10:38 -04:00
* 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
2013-11-15 23:10:38 -04:00
* Hein Hollander :Octo Support, Heli Testing
2012-08-21 23:19:50 -03:00
* Igor van Airde :Control Law optimization
* Jack Dunkle :Alpha testing
* James Goppert :Mavlink Support
* Jani Hiriven :Testing feedback
2013-11-15 23:10:38 -04:00
* Jean-Louis Naudin :Auto Landing
2012-08-21 23:19:50 -03:00
* John Arne Birkeland :PPM Encoder
* Jose Julio :Stabilization Control laws, MPU6k driver
2014-07-11 02:08:09 -03:00
* Julien Dubois :PosHold flight mode
2013-11-15 23:10:38 -04:00
* 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
2013-11-15 23:10:38 -04:00
* Olivier Adler :PPM Encoder, piezo buzzer
* Pat Hickey :Hardware Abstraction Layer (HAL)
* Robert Lefebvre :Heli Support, Copter LEDs
2013-11-15 23:10:38 -04:00
* Roberto Navoni :Library testing, Porting to VRBrain
* Sandro Benigno :Camera support, MinimOSD
2014-07-11 02:08:09 -03:00
* Sandro Tognana :PosHold flight mode
2013-11-15 23:10:38 -04:00
* ..and many more.
2012-08-21 23:19:50 -03:00
*
2016-03-25 06:46:54 -03:00
* Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors
* Wiki: http://copter.ardupilot.org/
2013-11-15 23:10:38 -04:00
* Requires modified version of Arduino, which can be found here: http://ardupilot.com/downloads/?category=6
2012-08-21 23:19:50 -03:00
*
*/
#include "Copter.h"
2014-03-31 04:07:46 -03:00
2015-12-26 00:00:03 -04:00
#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()
2016-04-19 04:36:45 -03:00
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[] = {
2015-12-26 00:00:03 -04:00
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
2015-12-26 00:00:03 -04:00
SCHED_TASK(update_optical_flow, 200, 160),
#endif
2015-12-26 00:00:03 -04:00
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
2016-05-20 22:19:53 -03:00
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
2015-12-26 00:00:03 -04:00
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
2015-12-26 00:00:03 -04:00
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
2015-12-26 00:00:03 -04:00
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
2015-12-26 00:00:03 -04:00
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
2015-12-26 00:00:03 -04:00
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
2015-12-26 00:00:03 -04:00
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
2015-10-16 19:05:53 -03:00
SCHED_TASK(accel_cal_update, 10, 100),
2015-11-27 00:53:08 -04:00
#if ADSB_ENABLED == ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 400),
2015-11-27 00:53:08 -04:00
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
2015-12-26 00:00:03 -04:00
SCHED_TASK(frsky_telemetry_send, 5, 75),
#endif
SCHED_TASK(terrain_update, 10, 100),
2014-09-15 03:52:21 -03:00
#if EPM_ENABLED == ENABLED
2015-12-26 00:00:03 -04:00
SCHED_TASK(epm_update, 10, 75),
2014-09-15 03:52:21 -03:00
#endif
#ifdef USERHOOK_FASTLOOP
2015-12-26 00:00:03 -04:00
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
2015-12-26 00:00:03 -04:00
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
2015-12-26 00:00:03 -04:00
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
2015-12-26 00:00:03 -04:00
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
2015-12-26 00:00:03 -04:00
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
2016-07-21 22:24:13 -03:00
SCHED_TASK(button_update, 5, 100),
};
2013-01-11 21:01:10 -04:00
void Copter::setup()
2013-12-28 01:02:32 -04:00
{
cliSerial = hal.console;
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
// setup storage layout for copter
StorageManager::set_layout_copter();
2012-08-21 23:19:50 -03:00
init_ardupilot();
2013-01-11 21:01:10 -04:00
// initialise the main loop scheduler
2015-07-06 12:30:40 -03:00
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
// setup initial performance counters
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}
/*
if the compass is enabled then try to accumulate a reading
*/
void Copter::compass_accumulate(void)
{
if (g.compass_enabled) {
compass.accumulate();
2013-10-29 01:28:27 -03:00
}
}
/*
try to accumulate a baro reading
*/
void Copter::barometer_accumulate(void)
{
barometer.accumulate();
}
void Copter::perf_update(void)
{
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
if (scheduler.debug()) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu\n",
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(),
(unsigned long)perf_info_get_min_time());
}
perf_info_reset();
2013-04-29 09:30:22 -03:00
pmTest1 = 0;
}
void Copter::loop()
{
// wait for an INS sample
ins.wait_for_sample();
uint32_t timer = micros();
2012-08-21 23:19:50 -03:00
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;
2012-08-21 23:19:50 -03:00
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
2012-08-21 23:19:50 -03:00
// tell the scheduler one tick has passed
scheduler.tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
scheduler.run(time_available);
}
// Main loop - 400hz
void Copter::fast_loop()
{
2013-12-17 00:58:11 -04:00
2012-08-21 23:19:50 -03:00
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
2013-12-06 02:08:11 -04:00
attitude_control.rate_controller_run();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// send outputs to the motors library
motors_output();
2012-08-21 23:19:50 -03:00
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading
check_ekf_yaw_reset();
// run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
2015-04-16 15:35:17 -03:00
// check if we've landed or crashed
update_land_and_crash_detectors();
2016-01-13 23:16:42 -04:00
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
2016-01-13 23:16:42 -04:00
#endif
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
}
}
// 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();
read_control_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_thr_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
#if GNDEFFECT_COMPENSATION == ENABLED
update_ground_effect_detector();
#endif // GNDEFFECT_COMPENSATION == ENABLED
}
2013-10-11 08:40:20 -03:00
// update_mount - update camera mount position
// should be run at 50hz
void Copter::update_mount()
{
#if MOUNT == ENABLED
// update camera mount's position
camera_mount.update();
#endif
}
// update camera trigger
void Copter::update_trigger(void)
{
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
if (camera.check_trigger_pin()) {
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
}
#endif
}
2013-10-11 08:40:20 -03:00
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
2013-10-11 08:40:20 -03:00
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
if(g.compass_enabled) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors.get_throttle());
compass.read();
2013-10-11 08:40:20 -03:00
// log compass information
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
DataFlash.Log_Write_Compass(compass);
2013-10-11 08:40:20 -03:00
}
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
2013-10-11 08:40:20 -03:00
{
// 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)) {
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
}
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
2015-09-04 12:50:48 -03:00
if (rssi.enabled()) {
DataFlash.Log_Write_RSSI(rssi);
}
}
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
2013-10-11 08:40:20 -03:00
}
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) {
2014-01-21 23:56:07 -04:00
Log_Write_Nav_Tuning();
}
2015-06-11 22:28:13 -03:00
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_Vibration(ins);
}
if (should_log(MASK_LOG_CTUN)) {
attitude_control.control_monitor_log();
}
2015-06-18 18:09:39 -03:00
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
2013-10-11 08:40:20 -03:00
}
// fifty_hz_logging_loop
// should be run at 50hz
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_RADIO_OUT);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
// log IMU data if we're not already logging at the higher rate
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
2013-11-03 23:37:46 -04:00
DataFlash.Log_Write_IMU(ins);
}
#endif
#if PRECISION_LANDING == ENABLED
// log output
Log_Write_Precland();
#endif
}
void Copter::dataflash_periodic(void)
{
DataFlash.periodic_tasks();
}
2013-10-11 08:40:20 -03:00
// three_hz_loop - 3.3hz loop
void Copter::three_hz_loop()
{
2013-10-11 08:40:20 -03:00
// check if we've lost contact with the ground station
failsafe_gcs_check();
2016-04-22 08:37:48 -03:00
// check if we've lost terrain data
failsafe_terrain_check();
#if AC_FENCE == ENABLED
2013-10-11 08:40:20 -03:00
// check if we have breached a fence
fence_check();
#endif // AC_FENCE_ENABLED
2013-08-04 11:20:21 -03:00
#if SPRAYER == ENABLED
2013-10-11 08:40:20 -03:00
sprayer.update();
2012-08-21 23:19:50 -03:00
#endif
2013-10-11 08:40:20 -03:00
update_events();
2015-01-16 02:12:58 -04:00
// update ch6 in flight tuning
tuning();
}
2013-10-11 08:40:20 -03:00
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_AP_STATE, ap.value);
}
2012-11-11 21:59:53 -04:00
update_arming_checks();
if (!motors.armed()) {
2013-10-11 08:40:20 -03:00
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
2013-10-11 08:40:20 -03:00
update_using_interlock();
#if FRAME_CONFIG != HELI_FRAME
2013-10-11 08:40:20 -03:00
// check the user hasn't updated the frame orientation
motors.set_frame_orientation(g.frame_orientation);
// 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
2014-02-05 19:12:31 -04:00
RC_Channel_aux::enable_aux_servos();
2013-10-11 08:40:20 -03:00
check_usb_mux();
2014-07-22 05:22:36 -03:00
// update position controller alt limits
update_poscon_alt_max();
// enable/disable raw gyro/accel logging
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// log terrain data
terrain_logging();
adsb.set_is_flying(!ap.land_complete);
}
2012-05-21 13:58:14 -03:00
// called at 50hz
void Copter::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool gps_updated = false;
2012-08-21 23:19:50 -03:00
2014-03-31 04:07:46 -03:00
gps.update();
2012-08-21 23:19:50 -03:00
// log after every gps message
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
2012-08-21 23:19:50 -03:00
// log GPS message
if (should_log(MASK_LOG_GPS) && !ahrs.have_ekf_logging()) {
DataFlash.Log_Write_GPS(gps, i);
}
gps_updated = true;
}
}
2012-08-21 23:19:50 -03:00
if (gps_updated) {
// set system time if necessary
set_system_time_from_GPS();
2014-03-31 04:07:46 -03:00
// checks to initialise home and take location based pictures
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
#if CAMERA == ENABLED
if (camera.update_location(current_loc, copter.ahrs) == true) {
2014-03-31 04:07:46 -03:00
do_take_picture();
}
2013-10-29 01:28:27 -03:00
#endif
2014-03-31 04:07:46 -03:00
}
2012-08-21 23:19:50 -03:00
}
}
void Copter::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values
2014-02-08 03:40:45 -04:00
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 to dataflash
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_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 (ap.simple_mode == 0 || !ap.new_radio_frame) {
return;
2012-08-21 23:19:50 -03:00
}
// mark radio frame as consumed
ap.new_radio_frame = false;
if (ap.simple_mode == 1) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
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)
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
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
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
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)
{
// 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)) {
// check the bearing to home has changed by at least 5 degrees
if (labs(super_simple_last_bearing - home_bearing) > 500) {
super_simple_last_bearing = home_bearing;
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)
{
2012-08-21 23:19:50 -03:00
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input();
2012-08-21 23:19:50 -03:00
#endif
ahrs.update();
}
2016-04-27 08:37:04 -03:00
// read baro and rangefinder altitude at 10hz
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
2012-08-21 23:19:50 -03:00
// write altitude info to dataflash logs
2014-10-16 21:37:49 -03:00
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
2012-08-21 23:19:50 -03:00
}
}
AP_HAL_MAIN_CALLBACKS(&copter);