diff --git a/.travis.yml b/.travis.yml index b07cd22b08..928b867bc9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,25 +1,11 @@ language: cpp -sudo: false +sudo: required cache: directories: - $HOME/opt addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - build-essential - - ccache - - g++-4.8 - - gcc-4.8 - - genromfs - - libc6-i386 - - python-argparse - - python-empy - - python-serial - - zlib1g-dev coverity_scan: project: name: "diydrones/ardupilot" @@ -30,49 +16,11 @@ addons: branch_pattern: coverity_scan before_install: - - pushd $HOME - - pushd $HOME/opt - # PX4 toolchain - - compiler="gcc-arm-none-eabi-4_9-2015q3" - - if [ ! -d "$HOME/opt/$compiler" ]; then - wget http://firmware.diydrones.com/Tools/PX4-tools/gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2 - && tar -xf gcc-arm-none-eabi-4_9-2015q3-20150921-linux.tar.bz2 - ; - fi - # RPi toolchain - - compiler="tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64" - - if [ ! -d "$HOME/opt/$compiler" ]; then - wget http://firmware.diydrones.com/Tools/Travis/NavIO/master.tar.gz - && tar -xf master.tar.gz - ; - fi - - popd - - mkdir -p $HOME/bin - && ln -sf /usr/bin/gcc-4.8 $HOME/bin/gcc - && ln -sf /usr/bin/g++-4.8 $HOME/bin/g++ - && exportline="export PATH=$HOME/bin:$HOME/opt/gcc-arm-none-eabi-4_9-2015q3/bin:$HOME/opt/tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64/bin:\$PATH" - && if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - && . ~/.profile - - popd - -before_script: - - mkdir -p $HOME/ccache-bin - - ln -s /usr/bin/ccache ~/bin/g++-4.8 - - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - - ln -s /usr/bin/ccache ~/bin/gcc-4.8-size - - ln -s /usr/bin/ccache ~/bin/gcc-4.8-objcopy - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size - - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy - - ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-g++ - - ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-gcc - - ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-size - - ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-objcopy + - APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/configure-ci.sh && . ~/.profile && popd script: - python Tools/autotest/param_metadata/param_parse.py - - Tools/scripts/build_all_travis.sh + - Tools/scripts/build_ci.sh notifications: webhooks: @@ -88,5 +36,5 @@ env: # via the "travis encrypt" command using the project repo's public key - secure: "FjIwqZQV2FhNPWYITX5LZXTE38yYqBaQdbm3QmbEg/30wnPTm1ZOLIU7o/aSvX615ImR8kHoryvFPDQDWc6wWfqTEs3Ytq2kIvcIJS2Y5l/0PFfpWJoH5gRd6hDThnoi+1oVMLvj1+bhn4yFlCCQ2vT/jxoGfiQqqgvHtv4fLzI=" matrix: - - TRAVIS_BUILD_TARGET="px4-v2 sitl linux" - - TRAVIS_BUILD_TARGET="navio raspilot minlure bebop" + - CI_BUILD_TARGET="px4-v2 sitl linux" + - CI_BUILD_TARGET="navio raspilot minlure bebop" diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 18dac70025..af6f838573 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -866,6 +866,19 @@ void Plane::update_alt() geofence_check(true); update_flight_stage(); + + if (auto_throttle_mode && !throttle_suppressed) { + SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), + target_airspeed_cm, + flight_stage, + auto_state.takeoff_pitch_cd, + throttle_nudge, + tecs_hgt_afe(), + aerodynamic_load_factor); + if (should_log(MASK_LOG_TECS)) { + Log_Write_TECS_Tuning(); + } + } } /* @@ -907,17 +920,6 @@ void Plane::update_flight_stage(void) } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } - - SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), - target_airspeed_cm, - flight_stage, - auto_state.takeoff_pitch_cd, - throttle_nudge, - tecs_hgt_afe(), - aerodynamic_load_factor); - if (should_log(MASK_LOG_TECS)) { - Log_Write_TECS_Tuning(); - } } else if (control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0f2ed305af..cb66f31993 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -691,13 +691,16 @@ void QuadPlane::set_armed(bool armed) */ float QuadPlane::assist_climb_rate_cms(void) { + float climb_rate; if (plane.auto_throttle_mode) { - // ask TECS for its desired climb rate - return plane.TECS_controller.get_height_rate_demand()*100; + // use altitude_error_cm, spread over 10s interval + climb_rate = plane.altitude_error_cm / 10; + } else { + // otherwise estimate from pilot input + climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); + climb_rate *= plane.channel_throttle->control_in; } - // otherwise estimate from pilot input - float climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); - climb_rate *= plane.channel_throttle->control_in; + climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up()); return climb_rate; } @@ -752,6 +755,8 @@ void QuadPlane::update_transition(void) if (transition_state < TRANSITION_TIMER) { // set a single loop pitch limit in TECS plane.TECS_controller.set_pitch_max_limit(transition_pitch_max); + } else if (transition_state < TRANSITION_DONE) { + plane.TECS_controller.set_pitch_max_limit((transition_pitch_max+1)*2); } switch (transition_state) { @@ -1068,10 +1073,12 @@ void QuadPlane::control_auto(const Location &loc) switch (plane.mission.get_current_nav_cmd().id) { case MAV_CMD_NAV_VTOL_LAND: - if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) { - pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true); + if (land_state == QLAND_POSITION) { + pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false); + } else if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) { + pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true); } else { - pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true); + pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true); } break; case MAV_CMD_NAV_VTOL_TAKEOFF: @@ -1145,6 +1152,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) target.y = diff2d.y * 100; target.z = plane.next_WP_loc.alt - origin.alt; wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target); + pos_control->set_alt_target(inertial_nav.get_altitude()); // also update nav_controller for status output plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); diff --git a/README.md b/README.md index 2a0e4c457f..c0b1b40c04 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,10 @@ [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/diydrones/ardupilot?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) +[![Build SemaphoreCI](https://semaphoreci.com/api/v1/projects/4d28a40d-b6a6-4bfb-9780-95d92aabb178/667563/badge.svg)](https://semaphoreci.com/diydrones/ardupilot) + +[![Build Travis](https://travis-ci.org/diydrones/ardupilot.svg?branch=master)](https://travis-ci.org/diydrones/ardupilot) + ### The ArduPilot project is made up of: ### >>ArduCopter (or APM:Copter) : [code](https://github.com/diydrones/ardupilot/tree/master/ArduCopter), [wiki](http://copter.ardupilot.com) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 61a93a4850..0ebd60008b 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -26,14 +26,14 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): mavproxy.expect('Requesting [0-9]+ waypoints') mavproxy.send('mode AUTO\n') wait_mode(mav, 'AUTO') - if not wait_waypoint(mav, 1, 9, max_dist=60): + if not wait_waypoint(mav, 1, 9, max_dist=60, timeout=1200): return False mavproxy.expect('DISARMED') # wait for blood sample here mavproxy.send('wp set 10\n') mavproxy.send('arm throttle\n') mavproxy.expect('ARMED') - if not wait_waypoint(mav, 10, 18, max_dist=60): + if not wait_waypoint(mav, 10, 18, max_dist=60, timeout=1200): return False mavproxy.expect('DISARMED') print("Mission OK") @@ -54,7 +54,7 @@ def fly_QuadPlane(viewerip=None, map=False): if map: options += ' --map' - sil = util.start_SIL('ArduPlane', model='quadplane', wipe=True, home=HOME_LOCATION, speedup=20, + sil = util.start_SIL('ArduPlane', model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10, defaults_file=os.path.join(testdir, 'quadplane.parm')) mavproxy = util.start_MAVProxy_SIL('QuadPlane', options=options) mavproxy.expect('Telemetry log: (\S+)') diff --git a/Tools/scripts/build_autotest.sh b/Tools/scripts/build_autotest.sh index a96f5cad0e..b102258f6f 100755 --- a/Tools/scripts/build_autotest.sh +++ b/Tools/scripts/build_autotest.sh @@ -40,7 +40,7 @@ lock_file() { if test -f "$lck" && kill -0 $pid 2> /dev/null; then LOCKAGE=$(($(date +%s) - $(stat -c '%Y' "build.lck"))) - test $LOCKAGE -gt 7200 && { + test $LOCKAGE -gt 30000 && { echo "old lock file $lck is valid for $pid with age $LOCKAGE seconds" } return 1 @@ -180,6 +180,6 @@ killall -9 JSBSim || /bin/true # raise core limit ulimit -c 10000000 -timelimit 12000 APM/Tools/autotest/autotest.py --timeout=11500 > buildlogs/autotest-output.txt 2>&1 +timelimit 22000 APM/Tools/autotest/autotest.py --timeout=20000 > buildlogs/autotest-output.txt 2>&1 ) >> build.log 2>&1 diff --git a/Tools/scripts/build_all_travis.sh b/Tools/scripts/build_ci.sh similarity index 85% rename from Tools/scripts/build_all_travis.sh rename to Tools/scripts/build_ci.sh index 614f75b1a5..d04f4f33f1 100755 --- a/Tools/scripts/build_all_travis.sh +++ b/Tools/scripts/build_ci.sh @@ -13,9 +13,9 @@ unset CXX CC export BUILDROOT=/tmp/travis.build.$$ rm -rf $BUILDROOT -# If TRAVIS_BUILD_TARGET is not set, default to all of them -if [ -z "$TRAVIS_BUILD_TARGET" ]; then - TRAVIS_BUILD_TARGET="sitl linux navio raspilot minlure bebop px4-v2 px4-v4" +# If CI_BUILD_TARGET is not set, default to all of them +if [ -z "$CI_BUILD_TARGET" ]; then + CI_BUILD_TARGET="sitl linux navio raspilot minlure bebop px4-v2 px4-v4" fi declare -A build_platforms @@ -45,8 +45,8 @@ waf=modules/waf/waf-light # get list of boards supported by the waf build for board in $($waf list_boards | head -n1); do waf_supported_boards[$board]=1; done -echo "Targets: $TRAVIS_BUILD_TARGET" -for t in $TRAVIS_BUILD_TARGET; do +echo "Targets: $CI_BUILD_TARGET" +for t in $CI_BUILD_TARGET; do echo "Starting make based build for target ${t}..." for v in ${!build_platforms[@]}; do if [[ ${build_platforms[$v]} != *$t* ]]; then @@ -69,6 +69,11 @@ for t in $TRAVIS_BUILD_TARGET; do $waf configure --board $t $waf clean $waf ${build_concurrency[$t]} build - [[ $t == linux ]] && $waf check + if [[ $t == linux ]]; then + $waf check + fi fi done + +echo build OK +exit 0 diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh new file mode 100755 index 0000000000..b4d3116feb --- /dev/null +++ b/Tools/scripts/configure-ci.sh @@ -0,0 +1,66 @@ +#!/bin/bash +# Install dependencies and configure the environment for CI build testing + +set -ex + +PKGS="build-essential gawk ccache genromfs libc6-i386 \ + python-argparse python-empy python-serial zlib1g-dev gcc-4.9 g++-4.9" + +ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3" +ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2" + +RPI_ROOT="master" +RPI_TARBALL="$RPI_ROOT.tar.gz" + +sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y +sudo apt-get -qq -y update +sudo apt-get -qq -y install $PKGS +sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 90 \ + --slave /usr/bin/g++ g++ /usr/bin/g++-4.9 + +pushd $HOME + +mkdir -p $HOME/opt +pushd $HOME/opt + +# PX4 toolchain +compiler=$ARM_ROOT +if [ ! -d "$HOME/opt/$compiler" ]; then + wget http://firmware.diydrones.com/Tools/PX4-tools/$ARM_TARBALL + tar -xf $ARM_TARBALL +fi + +# RPi/BBB toolchain +compiler="tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64" +if [ ! -d "$HOME/opt/$compiler" ]; then + wget http://firmware.diydrones.com/Tools/Travis/NavIO/$RPI_TARBALL + tar -xf $RPI_TARBALL +fi + +popd + +mkdir -p $HOME/bin + +# configure ccache +ln -s /usr/bin/ccache ~/bin/g++ +ln -s /usr/bin/ccache ~/bin/gcc +ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ +ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc +ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-g++ +ln -s /usr/bin/ccache ~/bin/arm-linux-gnueabihf-gcc + +exportline="export PATH=$HOME/bin:" +exportline="${exportline}:$HOME/opt/gcc-arm-none-eabi-4_9-2015q3/bin:" +exportline="${exportline}:$HOME/opt/tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64/bin:" +exportline="${exportline}:\$PATH" + +if grep -Fxq "$exportline" ~/.profile; then + echo nothing to do; +else + echo $exportline >> ~/.profile; +fi + +. ~/.profile + +popd + diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 0a18fb8705..2cfbf57abb 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -177,6 +177,9 @@ bool AP_Arming::ins_checks(bool report) if (ins.get_accel_count() > 1) { const Vector3f &prime_accel_vec = ins.get_accel(); for(uint8_t i=0; i 1) { const Vector3f &prime_gyro_vec = ins.get_gyro(); for(uint8_t i=0; isysid, msg->compid, MAV_RESULT_ACCEPTED); }else{ // send nack - mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, 1); + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR); } } diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 35b60c6082..a2c46aafaa 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -38,64 +38,151 @@ Plane::Plane(const char *home_str, const char *frame_str) : } /* - calculate lift in neutons + the following functions are from last_letter + https://github.com/Georacer/last_letter/blob/master/last_letter/src/aerodynamicsLib.cpp + many thanks to Georacer! */ -float Plane::calculate_lift(void) const +float Plane::liftCoeff(float alpha) const { - // simple lift equation from http://wright.nasa.gov/airplane/lifteq.html - const float max_angle = radians(30); - const float max_angle_delta = radians(10); - const float clift_at_max = coefficient.lift * 2 * M_PI_F * max_angle; - float Cl = coefficient.lift * 2 * M_PI_F * angle_of_attack; - if (fabsf(angle_of_attack) > max_angle+max_angle_delta) { - return 0; - } - if (angle_of_attack > max_angle) { - Cl = clift_at_max * (1-(angle_of_attack - max_angle)/max_angle_delta); - } else if (angle_of_attack < -max_angle) { - Cl = -clift_at_max * (1+(angle_of_attack - max_angle)/max_angle_delta); - } - float lift = 0.5 * Cl * air_density * sq(airspeed) * wing_area; - return lift; -} - - -/* - calculate induced drag in neutons - */ -float Plane::calculate_drag_induced(void) const -{ - float lift = calculate_lift(); + const float alpha0 = coefficient.alpha_stall; + const float M = coefficient.mcoeff; + const float c_lift_0 = coefficient.c_lift_0; + const float c_lift_a0 = coefficient.c_lift_a; - // simple induced drag from https://en.wikipedia.org/wiki/Lift-induced_drag - if (airspeed < 0.1) { - return 0; - } - float drag_i = coefficient.lift_drag * sq(lift) / (0.25 * sq(air_density) * sq(airspeed) * wing_area * M_PI_F * wing_efficiency * aspect_ratio); - return drag_i; + double sigmoid = ( 1+exp(-M*(alpha-alpha0))+exp(M*(alpha+alpha0)) ) / (1+exp(-M*(alpha-alpha0))) / (1+exp(M*(alpha+alpha0))); + double linear = (1.0-sigmoid) * (c_lift_0 + c_lift_a0*alpha); //Lift at small AoA + double flatPlate = sigmoid*(2*copysign(1,alpha)*pow(sin(alpha),2)*cos(alpha)); //Lift beyond stall + + float result = linear+flatPlate; + return result; } -/* - calculate form drag in neutons - */ -float Plane::calculate_drag_form(void) const +float Plane::dragCoeff(float alpha) const { - // simple form drag - float drag_f = 0.5 * air_density * sq(airspeed) * coefficient.drag; - return drag_f; + const float b = coefficient.b; + const float s = coefficient.s; + const float c_drag_p = coefficient.c_drag_p; + const float c_lift_0 = coefficient.c_lift_0; + const float c_lift_a0 = coefficient.c_lift_a; + const float oswald = coefficient.oswald; + + double AR = pow(b,2)/s; + double c_drag_a = c_drag_p + pow(c_lift_0+c_lift_a0*alpha,2)/(M_PI*oswald*AR); + + return c_drag_a; } -/* - calculate lift+drag in neutons in body frame - */ -Vector3f Plane::calculate_lift_drag(void) const +// Torque calculation function +Vector3f Plane::getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const { - if (velocity_ef.is_zero()) { - return Vector3f(0,0,0); - } - float lift = calculate_lift(); - float drag = calculate_drag_induced() + calculate_drag_form(); - return velocity_bf.normalized()*(-drag) + Vector3f(0, 0, -lift); + const float alpha = angle_of_attack; + const float s = coefficient.s; + const float c = coefficient.c; + const float b = coefficient.b; + const float c_l_0 = coefficient.c_l_0; + const float c_l_b = coefficient.c_l_b; + const float c_l_p = coefficient.c_l_p; + const float c_l_r = coefficient.c_l_r; + const float c_l_deltaa = coefficient.c_l_deltaa; + const float c_l_deltar = coefficient.c_l_deltar; + const float c_m_0 = coefficient.c_m_0; + const float c_m_a = coefficient.c_m_a; + const float c_m_q = coefficient.c_m_q; + const float c_m_deltae = coefficient.c_m_deltae; + const float c_n_0 = coefficient.c_n_0; + const float c_n_b = coefficient.c_n_b; + const float c_n_p = coefficient.c_n_p; + const float c_n_r = coefficient.c_n_r; + const float c_n_deltaa = coefficient.c_n_deltaa; + const float c_n_deltar = coefficient.c_n_deltar; + const Vector3f &CGOffset = coefficient.CGOffset; + + float rho = air_density; + + //read angular rates + double p = gyro.x; + double q = gyro.y; + double r = gyro.z; + + //calculate aerodynamic torque + double qbar = 1.0/2.0*rho*pow(airspeed,2)*s; //Calculate dynamic pressure + double la, na, ma; + if (is_zero(airspeed)) + { + la = 0; + ma = 0; + na = 0; + } + else + { + la = qbar*b*(c_l_0 + c_l_b*beta + c_l_p*b*p/(2*airspeed) + c_l_r*b*r/(2*airspeed) + c_l_deltaa*inputAileron + c_l_deltar*inputRudder); + ma = qbar*c*(c_m_0 + c_m_a*alpha + c_m_q*c*q/(2*airspeed) + c_m_deltae*inputElevator); + na = qbar*b*(c_n_0 + c_n_b*beta + c_n_p*b*p/(2*airspeed) + c_n_r*b*r/(2*airspeed) + c_n_deltaa*inputAileron + c_n_deltar*inputRudder); + } + + + // Add torque to to force misalignment with CG + // r x F, where r is the distance from CoG to CoL + la += CGOffset.y * force.z - CGOffset.z * force.y; + ma += -CGOffset.x * force.z + CGOffset.z * force.x; + na += -CGOffset.y * force.x + CGOffset.x * force.y; + + return Vector3f(la, ma, na); +} + +// Force calculation function from last_letter +Vector3f Plane::getForce(float inputAileron, float inputElevator, float inputRudder) const +{ + const float alpha = angle_of_attack; + const float c_drag_q = coefficient.c_drag_q; + const float c_lift_q = coefficient.c_lift_q; + const float s = coefficient.s; + const float c = coefficient.c; + const float b = coefficient.b; + const float c_drag_deltae = coefficient.c_drag_deltae; + const float c_lift_deltae = coefficient.c_lift_deltae; + const float c_y_0 = coefficient.c_y_0; + const float c_y_b = coefficient.c_y_b; + const float c_y_p = coefficient.c_y_p; + const float c_y_r = coefficient.c_y_r; + const float c_y_deltaa = coefficient.c_y_deltaa; + const float c_y_deltar = coefficient.c_y_deltar; + + float rho = air_density; + + //request lift and drag alpha-coefficients from the corresponding functions + double c_lift_a = liftCoeff(alpha); + double c_drag_a = dragCoeff(alpha); + + //convert coefficients to the body frame + double c_x_a = -c_drag_a*cos(alpha)+c_lift_a*sin(alpha); + double c_x_q = -c_drag_q*cos(alpha)+c_lift_q*sin(alpha); + double c_z_a = -c_drag_a*sin(alpha)-c_lift_a*cos(alpha); + double c_z_q = -c_drag_q*sin(alpha)-c_lift_q*cos(alpha); + + //read angular rates + double p = gyro.x; + double q = gyro.y; + double r = gyro.z; + + //calculate aerodynamic force + double qbar = 1.0/2.0*rho*pow(airspeed,2)*s; //Calculate dynamic pressure + double ax, ay, az; + if (is_zero(airspeed)) + { + ax = 0; + ay = 0; + az = 0; + } + else + { + ax = qbar*(c_x_a + c_x_q*c*q/(2*airspeed) - c_drag_deltae*cos(alpha)*fabs(inputElevator) + c_lift_deltae*sin(alpha)*inputElevator); + // split c_x_deltae to include "abs" term + ay = qbar*(c_y_0 + c_y_b*beta + c_y_p*b*p/(2*airspeed) + c_y_r*b*r/(2*airspeed) + c_y_deltaa*inputAileron + c_y_deltar*inputRudder); + az = qbar*(c_z_a + c_z_q*c*q/(2*airspeed) - c_drag_deltae*sin(alpha)*fabs(inputElevator) - c_lift_deltae*cos(alpha)*inputElevator); + // split c_z_deltae to include "abs" term + } + return Vector3f(ax, ay, az); } void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) @@ -104,45 +191,24 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel float elevator = (input.servos[1]-1500)/500.0f; float rudder = (input.servos[3]-1500)/500.0f; float throttle = constrain_float((input.servos[2]-1000)/1000.0f, 0, 1); - float speed_scaling = airspeed / cruise_airspeed; float thrust = throttle; - float roll_rate = aileron * speed_scaling; - float pitch_rate = elevator * speed_scaling; - float yaw_rate = rudder * speed_scaling; - - // rotational acceleration, in rad/s/s, in body frame - rot_accel.x = roll_rate * max_rates.x; - rot_accel.y = pitch_rate * max_rates.y; - rot_accel.z = yaw_rate * max_rates.z; - - // rotational air resistance - rot_accel.x -= gyro.x * radians(800.0) / terminal_rotation_rate.x; - rot_accel.y -= gyro.y * radians(800.0) / terminal_rotation_rate.y; - rot_accel.z -= gyro.z * radians(1200.0) / terminal_rotation_rate.z; - - // add torque of stabilisers - rot_accel.z += velocity_bf.y * speed_scaling * coefficient.vertical_stabiliser; - rot_accel.y -= velocity_bf.z * speed_scaling * coefficient.horizontal_stabiliser; // calculate angle of attack angle_of_attack = atan2f(velocity_bf.z, velocity_bf.x); beta = atan2f(velocity_bf.y,velocity_bf.x); - // add dihedral - rot_accel.x -= beta * airspeed * coefficient.dihedral; - + Vector3f force = getForce(aileron, elevator, rudder); + rot_accel = getTorque(aileron, elevator, rudder, force); + // velocity in body frame velocity_bf = dcm.transposed() * velocity_ef; - - // get lift and drag in body frame, in neutons - Vector3f lift_drag = calculate_lift_drag(); // scale thrust to newtons thrust *= thrust_scale; accel_body = Vector3f(thrust/mass, 0, 0); - accel_body += lift_drag/mass; + accel_body += force; // add some noise add_noise(thrust / thrust_scale); diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 6bf3e533eb..9b006df200 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/libraries/SITL/SIM_Plane.h @@ -39,37 +39,64 @@ public: } protected: - const float hover_throttle = 1.2f; - const float cruise_airspeed = 20; - const float cruise_pitch = radians(4); - const float wing_efficiency = 0.9; - const float wing_span = 2.0; - const float wing_chord = 0.15; - const float aspect_ratio = wing_span / wing_chord; - const float wing_area = wing_span * wing_chord; + const float hover_throttle = 0.7f; const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions float angle_of_attack; float beta; Vector3f velocity_bf; - // manually tweaked coefficients. Not even close to reality struct { - float drag = 0.005; - float lift = 2.0; - float lift_drag = 0.5; - float vertical_stabiliser = 0.1; - float horizontal_stabiliser = 2; - float dihedral = 0.1; + // from last_letter skywalker_2013/aerodynamics.yaml + // thanks to Georacer! + float s = 0.45; + float b = 1.88; + float c = 0.24; + float c_lift_0 = 0.56; + float c_lift_deltae = 0; + float c_lift_a = 6.9; + float c_lift_q = 0; + float mcoeff = 50; + float oswald = 0.9; + float alpha_stall = 0.4712; + float c_drag_q = 0; + float c_drag_deltae = 0.0; + float c_drag_p = 0.1; + float c_y_0 = 0; + float c_y_b = -0.98; + float c_y_p = 0; + float c_y_r = 0; + float c_y_deltaa = 0; + float c_y_deltar = -0.2; + float c_l_0 = 0; + float c_l_p = -1.0; + float c_l_b = -0.12; + float c_l_r = 0.14; + float c_l_deltaa = 0.25; + float c_l_deltar = -0.037; + float c_m_0 = 0.045; + float c_m_a = -0.7; + float c_m_q = -20; + float c_m_deltae = 1.0; + float c_n_0 = 0; + float c_n_b = 0.25; + float c_n_p = 0.022; + float c_n_r = -1; + float c_n_deltaa = 0.00; + float c_n_deltar = 0.1; + float deltaa_max = 0.3491; + float deltae_max = 0.3491; + float deltar_max = 0.3491; + // the X CoG offset should be -0.02, but that makes the plane too tail heavy + // in manual flight. Adjusted to -0.15 gives reasonable flight + Vector3f CGOffset{-0.15, 0, -0.05}; } coefficient; float thrust_scale; - Vector3f terminal_rotation_rate{radians(170), radians(200), radians(180)}; - Vector3f max_rates{radians(350), radians(250), radians(100)}; - float calculate_lift(void) const; - float calculate_drag_induced(void) const; - float calculate_drag_form(void) const; - Vector3f calculate_lift_drag(void) const; + float liftCoeff(float alpha) const; + float dragCoeff(float alpha) const; + Vector3f getForce(float inputAileron, float inputElevator, float inputRudder) const; + Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const; void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); };