aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into mergeDIYD

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-20 22:20:53 -08:00
commit de0a5a6b24
13 changed files with 317 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
.travis.yml
View File

@ -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"

24
ArduPlane/ArduPlane.cpp
View File

@ -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 ||

24
ArduPlane/quadplane.cpp
View File

@ -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);

4
README.md
View File

@ -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)

6
Tools/autotest/quadplane.py
View File

@ -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+)')

4
Tools/scripts/build_autotest.sh
View File

@ -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

17
Tools/scripts/build_all_travis.sh → Tools/scripts/build_ci.sh
View File

@ -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

66
Tools/scripts/configure-ci.sh Executable file
View File

@ -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

6
libraries/AP_Arming/AP_Arming.cpp
View File

@ -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<ins.get_accel_count(); i++) {
if (!ins.use_accel(i)) {
continue;
}
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
@ -206,6 +209,9 @@ bool AP_Arming::ins_checks(bool report)
if (ins.get_gyro_count() > 1) {
const Vector3f &prime_gyro_vec = ins.get_gyro();
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
if (!ins.use_gyro(i)) {
continue;
}
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;

2
libraries/GCS_MAVLink/GCS.h
View File

@ -227,7 +227,7 @@ private:
uint16_t waypoint_count;
uint32_t waypoint_timelast_receive; // milliseconds
uint32_t waypoint_timelast_request; // milliseconds
const uint16_t waypoint_receive_timeout; // milliseconds
const uint16_t waypoint_receive_timeout = 8000; // milliseconds
// saveable rate of each stream
AP_Int16 streamRates[NUM_STREAMS];

5
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -28,8 +28,7 @@ extern const AP_HAL::HAL& hal;
uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
uint8_t GCS_MAVLINK::mavlink_active = 0;
GCS_MAVLINK::GCS_MAVLINK() :
waypoint_receive_timeout(5000)
GCS_MAVLINK::GCS_MAVLINK()
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -361,7 +360,7 @@ void GCS_MAVLINK::handle_mission_clear_all(AP_Mission &mission, mavlink_message_
mavlink_msg_mission_ack_send(chan, msg->sysid, 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);
}
}

214
libraries/SITL/SIM_Plane.cpp
View File

@ -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);

69
libraries/SITL/SIM_Plane.h
View File

@ -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);
};