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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Rustom Jehangir 2016-07-05 19:01:21 -04:00
commit bd39f8162a
271 changed files with 5560 additions and 2373 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/CONTRIBUTING.md vendored
View File

@ -23,7 +23,7 @@ Please see our [wiki article](http://dev.ardupilot.org/wiki/submitting-patches-b
# Development Team
The ArduPilot project is open source and [maintained](MAINTAINERS.md) by a team of volunteers.
The ArduPilot project is open source and [maintained](https://github.com/ArduPilot/ardupilot#maintainers) by a team of volunteers.
To contribute, you can send a pull request on Github. You can also
join the [development discussion on Google

3
.gitignore vendored
View File

@ -62,6 +62,8 @@ ArduCopter/logs/
ArduCopter/terrain/*.DAT
ArduCopter/test.ArduCopter/
ArduCopter/test/*
ArduCopter/fence.txt
ArduCopter/ral.txt
ArduCopter/way.txt
ArduPlane/logs/
ArduPlane/test/*
@ -100,3 +102,4 @@ Thumbs.db
cscope.in.out
cscope.out
cscope.po.out
*.generated.h

1
APMrover2/GCS_Mavlink.cpp
View File

@ -596,6 +596,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
break;
case MSG_RETRY_DEFERRED:
case MSG_ADSB_VEHICLE:
case MSG_TERRAIN:
case MSG_OPTICAL_FLOW:
case MSG_GIMBAL_REPORT:

3
APMrover2/defines.h
View File

@ -6,8 +6,6 @@
#define TRUE 1
#define FALSE 0
#define ToRad(x) radians(x) // *pi/180
#define ToDeg(x) degrees(x) // *180/pi
#define DEBUG 0
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
@ -15,7 +13,6 @@
// active altitude sensor
// ----------------------
#define SONAR 0
#define BARO 1
// CH 7 control
enum ch7_option {

59
APMrover2/release-notes.txt
View File

@ -1,4 +1,61 @@
Release 3.0.0, 5 April 2015
Release 3.0.1, 17 June 2016
===========================
The ArduPilot development team is proud to announce the release of
version 3.0.1 of APM:Rover. This is a minor release with small but
important bug fix changes.
The two main motivations for this release
1. Fixing the arming for skid steering Rovers
2. Fix to the rover steering that should really improve steering of
all rovers.
Skid Steering Arming
--------------------
Fixed arming for Skid Steering rovers. You should now be able to arm
your skid steering rover using the steering stick. NOTE skid steering
Rovers - you will not be able to disarm. The reason for this is zero
throttle full left turn is a perfectly valid move for a skid steering
rover as it can turn on the spot. You don't want to be executing this
and have the rover disarm part way through your turn so we have
disabled disarming via the steering stick. You can still disarm from
the GCS. Thanks to Pierre Kancir for working on this.
Improved Steering Control
-------------------------
For historical reason's the steering controller was using the raw GPS
data for ground speed without any filtering. If you have every graphed
this data you will see on a rover its very spiky and all over the
place. This spiky'ness was feeding into the lateral accel demand and
causing inaccuracies/jitters. Now we using the EKF GPS filtered data
which is much smoother and accurate and the steering control has
greatly improved.
Improved Cornering
------------------
Previously when corning we didn't take into account any "lean or tilt"
in the rover - we assumed the turns were always flat. We have changed
this to now take into account any lean so turning should be more
accurate. Boat users should really benefit from this too.
MAVLink2 support has been added
-------------------------------
See this post by Tridge -
http://discuss.ardupilot.org/t/mavlink2-is-in-ardupilot-master/9188/1
The other changes in this release are:
- setting your sonar_trigger_cm to 0 will now log the sonar data but
not use it for vehicle avoidance.
- the throttle PID is now being logged
- range finder input is now being captured (thanks to Allan Matthew)
- added LOG_DISARMED parameter
- merge upstream PX4Firmware changes
- numerous waf build improvements
- numerous EKF2 improvements
Release 3.0.0, 5 April 2016
===========================
The ArduPilot development team is proud to announce the release of
version 3.0.0 of APM:Rover. This is a major release with a lot of

4
APMrover2/version.h
View File

@ -2,8 +2,8 @@
#include "ap_version.h"
#define THISFIRMWARE "ArduRover v3.0.1beta1"
#define FIRMWARE_VERSION 3,0,1,FIRMWARE_VERSION_TYPE_BETA
#define THISFIRMWARE "ArduRover v3.0.1"
#define FIRMWARE_VERSION 3,0,1,FIRMWARE_VERSION_TYPE_OFFICIAL
#ifndef GIT_VERSION
#define FIRMWARE_STRING THISFIRMWARE

7
AntennaTracker/GCS_Mavlink.cpp
View File

@ -129,7 +129,7 @@ void Tracker::send_waypoint_request(mavlink_channel_t chan)
void Tracker::send_nav_controller_output(mavlink_channel_t chan)
{
float alt_diff = (g.alt_source == 0) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
float alt_diff = (g.alt_source == ALT_SOURCE_BARO) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
mavlink_msg_nav_controller_output_send(
chan,
@ -265,6 +265,7 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
case MSG_EXTENDED_STATUS1:
case MSG_EXTENDED_STATUS2:
case MSG_RETRY_DEFERRED:
case MSG_ADSB_VEHICLE:
case MSG_CURRENT_WAYPOINT:
case MSG_VFR_HUD:
case MSG_SYSTEM_TIME:
@ -508,7 +509,7 @@ void Tracker::mavlink_check_target(const mavlink_message_t* msg)
msg->sysid,
msg->compid,
MAV_DATA_STREAM_POSITION,
1, // 1hz
g.mavlink_update_rate,
1); // start streaming
}
// request air pressure
@ -518,7 +519,7 @@ void Tracker::mavlink_check_target(const mavlink_message_t* msg)
msg->sysid,
msg->compid,
MAV_DATA_STREAM_RAW_SENSORS,
1, // 1hz
g.mavlink_update_rate,
1); // start streaming
}
}

2
AntennaTracker/GCS_Mavlink.h
View File

@ -14,7 +14,7 @@ protected:
// telem_delay is not used by Tracker but is pure virtual, thus
// this implementaiton. it probably *should* be used by Tracker,
// as currently Tracker may brick XBees
uint32_t telem_delay() const override { return 0; };
uint32_t telem_delay() const override { return 0; }
private:

32
AntennaTracker/Log.cpp
View File

@ -36,22 +36,48 @@ struct PACKED log_Vehicle_Baro {
// Write a vehicle baro packet
void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
{
struct log_Vehicle_Baro pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_BAR_MSG),
time_us : AP_HAL::micros64(),
press : pressure,
alt_diff : altitude
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Vehicle_Pos {
LOG_PACKET_HEADER;
uint64_t time_us;
int32_t vehicle_lat;
int32_t vehicle_lng;
int32_t vehicle_alt;
float vehicle_vel_x;
float vehicle_vel_y;
float vehicle_vel_z;
};
// Write a vehicle pos packet
void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel)
{
struct log_Vehicle_Pos pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
time_us : AP_HAL::micros64(),
vehicle_lat : lat,
vehicle_lng : lng,
vehicle_alt : alt,
vehicle_vel_x : vel.x,
vehicle_vel_y : vel.y,
vehicle_vel_z : vel.z,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
const struct LogStructure Tracker::log_structure[] = {
LOG_COMMON_STRUCTURES,
{LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
"VBAR", "Qff", "TimeUS,Press,AltDiff" },
"VBAR", "Qff", "TimeUS,Press,AltDiff" },
{LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
"VPOS", "QLLefff", "TimeUS,Lat,Lng,Alt,VelX,VelY,VelZ" }
};
void Tracker::Log_Write_Vehicle_Startup_Messages()

13
AntennaTracker/Parameters.cpp
View File

@ -221,6 +221,15 @@ const AP_Param::Info Tracker::var_info[] = {
// @User: Standard
GSCALAR(alt_source, "ALT_SOURCE", 0),
// @Param: MAV_UPDATE_RATE
// @DisplayName: Mavlink Update Rate
// @Description: The rate at which Mavlink updates position and baro data
// @Units: Hz
// @Increment: 1
// @Range: 1 10
// @User: Standard
GSCALAR(mavlink_update_rate, "MAV_UPDATE_RATE", 1),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_
@ -328,7 +337,7 @@ const AP_Param::Info Tracker::var_info[] = {
// @Range: 0.001 0.1
// @Increment: 0.001
// @User: Standard
GGROUP(pidPitch2Srv, "PITCH2SRV_", PID),
GGROUP(pidPitch2Srv, "PITCH2SRV_", AC_PID),
// @Param: YAW2SRV_P
// @DisplayName: Yaw axis controller P gain
@ -358,7 +367,7 @@ const AP_Param::Info Tracker::var_info[] = {
// @Range: 0.001 0.1
// @Increment: 0.001
// @User: Standard
GGROUP(pidYaw2Srv, "YAW2SRV_", PID),
GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID),
// @Param: CMD_TOTAL
// @DisplayName: Number of loaded mission items

18
AntennaTracker/Parameters.h
View File

@ -63,8 +63,8 @@ public:
k_param_scheduler,
k_param_ins,
k_param_sitl,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
k_param_pidPitch_old, // deprecated
k_param_pidYaw_old, // deprecated
k_param_gcs2, // stream rates for uartD
k_param_serial2_baud, // deprecated
@ -100,12 +100,15 @@ public:
k_param_serial_manager, // serial manager library
k_param_servo_yaw_type,
k_param_alt_source,
k_param_mavlink_update_rate,
//
// 200 : Radio settings
//
k_param_channel_yaw = 200,
k_param_channel_pitch,
k_param_pidPitch2Srv,
k_param_pidYaw2Srv,
//
// 220: Waypoint data
@ -138,6 +141,7 @@ public:
AP_Int8 servo_pitch_type;
AP_Int8 servo_yaw_type;
AP_Int8 alt_source;
AP_Int8 mavlink_update_rate;
AP_Float onoff_yaw_rate;
AP_Float onoff_pitch_rate;
AP_Float onoff_yaw_mintime;
@ -154,13 +158,13 @@ public:
AP_Int32 log_bitmask;
// PID controllers
PID pidPitch2Srv;
PID pidYaw2Srv;
// AC_PID controllers
AC_PID pidPitch2Srv;
AC_PID pidYaw2Srv;
Parameters() :
pidPitch2Srv(0.2, 0, 0.05f, 4000.0f),
pidYaw2Srv (0.2, 0, 0.05f, 4000.0f)
pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f),
pidYaw2Srv (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f)
{}
};

12
AntennaTracker/ReleaseNotes.txt
View File

@ -1,5 +1,17 @@
Antenna Tracker Release Notes:
------------------------------------------------------------------
AntennaTracker 0.8.0 22-Jun-2016
Changes from 0.7.8
1) Added PITCH2SRV_FILT, YAW2SRV_FILT added to smooth input to controllers. Lower values lead to more delay but smoother motion.
2) Estimate vehicle's altitude between telemetry updates when using ALT_SOURCE = GPS
3) Bug fix to vehicle position estimate (was using vehicle's heading instead of 3D velocity vector)
4) Added MAV_UPDATE_RATE parameter to allow more easily setting the rate the vehicle sends position data
------------------------------------------------------------------
AntennaTracker 0.7.8 10-Jun-2016
Changes from 0.7.7
1) Bug fix to VBAR dataflash logging
2) VPOS dataflash logging message captures vehicle position
------------------------------------------------------------------
AntennaTracker 0.7.7 31-May-2016
Changes from 0.7.6
1) SERVO_TYPE parameter split into SERVO_PITCH_TYPE, SERVO_YAW_TYPE to allow different servo types for each axis

16
AntennaTracker/Tracker.h
View File

@ -46,7 +46,7 @@
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <DataFlash/DataFlash.h>
#include <PID/PID.h>
#include <AC_PID/AC_PID.h>
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
@ -147,10 +147,9 @@ private:
bool location_valid; // true if we have a valid location for the vehicle
Location location; // lat, long in degrees * 10^7; alt in meters * 100
Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
uint32_t last_update_us; // last position update in micxroseconds
uint32_t last_update_us; // last position update in microseconds
uint32_t last_update_ms; // last position update in milliseconds
float heading; // last known direction vehicle is moving
float ground_speed; // vehicle's last known ground speed in m/s
Vector3f vel; // the vehicle's velocity in m/s
} vehicle;
// Navigation controller state
@ -168,14 +167,6 @@ private:
bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode
} nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
// Servo state
struct {
bool yaw_lower : 1; // true if yaw servo has been limited from moving to a lower position (i.e. position or rate limited)
bool yaw_upper : 1; // true if yaw servo has been limited from moving to a higher position (i.e. position or rate limited)
bool pitch_lower : 1; // true if pitch servo has been limited from moving to a lower position (i.e. position or rate limited)
bool pitch_upper : 1; // true if pitch servo has been limited from moving to a higher position (i.e. position or rate limited)
} servo_limit = {true, true, true, true};
// setup the var_info table
AP_Param param_loader{var_info};
@ -255,6 +246,7 @@ private:
void compass_cal_update();
void Log_Write_Attitude();
void Log_Write_Baro(void);
void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt, const Vector3f& vel);
void Log_Write_Vehicle_Baro(float pressure, float altitude);
void Log_Write_Vehicle_Startup_Messages();
void start_logging();

6
AntennaTracker/defines.h
View File

@ -24,6 +24,11 @@ enum ServoType {
SERVO_TYPE_CR=2
};
enum AltSource {
ALT_SOURCE_BARO=0,
ALT_SOURCE_GPS=1
};
// Logging parameters
#define MASK_LOG_ATTITUDE (1<<0)
#define MASK_LOG_GPS (1<<1)
@ -35,3 +40,4 @@ enum ServoType {
// Logging messages
#define LOG_V_BAR_MSG 0x04
#define LOG_V_POS_MSG 0x05

2
AntennaTracker/make.inc
View File

@ -15,7 +15,7 @@ LIBRARIES += GCS_MAVLink
LIBRARIES += AP_SerialManager
LIBRARIES += AP_Declination
LIBRARIES += DataFlash
LIBRARIES += PID
LIBRARIES += AC_PID
LIBRARIES += AP_Scheduler
LIBRARIES += AP_NavEKF
LIBRARIES += AP_NavEKF2

35
AntennaTracker/servos.cpp
View File

@ -78,11 +78,8 @@ void Tracker::update_pitch_position_servo(float pitch)
// PITCH2SRV_IMAX 4000.000000
// calculate new servo position
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err);
// initialise limit flags
servo_limit.pitch_lower = false;
servo_limit.pitch_upper = false;
g.pidPitch2Srv.set_input_filter_all(angle_err);
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
// rate limit pitch servo
if (g.pitch_slew_time > 0.02f) {
@ -92,11 +89,9 @@ void Tracker::update_pitch_position_servo(float pitch)
}
if (new_servo_out <= channel_pitch.get_servo_out() - max_change) {
new_servo_out = channel_pitch.get_servo_out() - max_change;
servo_limit.pitch_lower = true;
}
if (new_servo_out >= channel_pitch.get_servo_out() + max_change) {
new_servo_out = channel_pitch.get_servo_out() + max_change;
servo_limit.pitch_upper = true;
}
}
channel_pitch.set_servo_out(new_servo_out);
@ -104,11 +99,11 @@ void Tracker::update_pitch_position_servo(float pitch)
// position limit pitch servo
if (channel_pitch.get_servo_out() <= -pitch_limit_cd) {
channel_pitch.set_servo_out(-pitch_limit_cd);
servo_limit.pitch_lower = true;
g.pidPitch2Srv.reset_I();
}
if (channel_pitch.get_servo_out() >= pitch_limit_cd) {
channel_pitch.set_servo_out(pitch_limit_cd);
servo_limit.pitch_upper = true;
g.pidPitch2Srv.reset_I();
}
}
@ -146,7 +141,8 @@ void Tracker::update_pitch_cr_servo(float pitch)
{
float ahrs_pitch = degrees(ahrs.pitch);
float err_cd = (pitch - ahrs_pitch) * 100.0f;
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid(err_cd));
g.pidPitch2Srv.set_input_filter_all(err_cd);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
}
/**
@ -256,17 +252,13 @@ void Tracker::update_yaw_position_servo(float yaw)
}
slew_dir = new_slew_dir;
// initialise limit flags
servo_limit.yaw_lower = false;
servo_limit.yaw_upper = false;
int16_t new_servo_out;
if (slew_dir != 0) {
new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I();
} else {
float servo_change = g.pidYaw2Srv.get_pid(angle_err);
g.pidYaw2Srv.set_input_filter_all(angle_err);
float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000);
new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000);
}
@ -279,23 +271,21 @@ void Tracker::update_yaw_position_servo(float yaw)
}
if (new_servo_out <= channel_yaw.get_servo_out() - max_change) {
new_servo_out = channel_yaw.get_servo_out() - max_change;
servo_limit.yaw_lower = true;
}
if (new_servo_out >= channel_yaw.get_servo_out() + max_change) {
new_servo_out = channel_yaw.get_servo_out() + max_change;
servo_limit.yaw_upper = true;
}
}
channel_yaw.set_servo_out(new_servo_out);
// position limit pitch servo
// position limit yaw servo
if (channel_yaw.get_servo_out() <= -yaw_limit_cd) {
channel_yaw.set_servo_out(-yaw_limit_cd);
servo_limit.yaw_lower = true;
g.pidYaw2Srv.reset_I();
}
if (channel_yaw.get_servo_out() >= yaw_limit_cd) {
channel_yaw.set_servo_out(yaw_limit_cd);
servo_limit.yaw_upper = true;
g.pidYaw2Srv.reset_I();
}
}
@ -335,5 +325,6 @@ void Tracker::update_yaw_cr_servo(float yaw)
float yaw_cd = wrap_180_cd(yaw*100.0f);
float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd));
g.pidYaw2Srv.set_input_filter_all(err_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());
}

21
AntennaTracker/tracking.cpp
View File

@ -15,7 +15,10 @@ void Tracker::update_vehicle_pos_estimate()
if (dt < TRACKING_TIMEOUT_SEC) {
// project the vehicle position to take account of lost radio packets
vehicle.location_estimate = vehicle.location;
location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
float north_offset = vehicle.vel.x * dt;
float east_offset = vehicle.vel.y * dt;
location_offset(vehicle.location_estimate, north_offset, east_offset);
vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt;
// set valid_location flag
vehicle.location_valid = true;
} else {
@ -58,12 +61,12 @@ void Tracker::update_bearing_and_distance()
nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
// calculate altitude difference to vehicle using gps
nav_status.alt_difference_gps = (vehicle.location.alt - current_loc.alt) / 100.0f;
nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
if (g.alt_source == 0) {
if (g.alt_source == ALT_SOURCE_BARO) {
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
} else {
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
@ -124,14 +127,12 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
vehicle.location.lat = msg.lat;
vehicle.location.lng = msg.lon;
vehicle.location.alt = msg.alt/10;
vehicle.heading = msg.hdg * 0.01f;
vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f;
vehicle.last_update_us = AP_HAL::micros();
vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
vehicle.last_update_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis();
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, 1);
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
}
}
@ -158,8 +159,8 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
}
}
// log altitude difference
Log_Write_Vehicle_Baro(local_pressure, alt_diff);
// log vehicle baro data
Log_Write_Vehicle_Baro(aircraft_pressure, alt_diff);
}
/**

4
AntennaTracker/version.h
View File

@ -2,8 +2,8 @@
#include "ap_version.h"
#define THISFIRMWARE "AntennaTracker V0.7.7"
#define FIRMWARE_VERSION 0,7,7,FIRMWARE_VERSION_TYPE_DEV
#define THISFIRMWARE "AntennaTracker V0.8.0"
#define FIRMWARE_VERSION 0,8,0,FIRMWARE_VERSION_TYPE_DEV
#ifndef GIT_VERSION
#define FIRMWARE_STRING THISFIRMWARE

2
AntennaTracker/wscript
View File

@ -7,7 +7,7 @@ def build(bld):
name=vehicle + '_libs',
vehicle=vehicle,
libraries=bld.ap_common_vehicle_libraries() + [
'PID',
'AC_PID',
],
use='mavlink',
)

9
ArduCopter/ArduCopter.cpp
View File

@ -97,7 +97,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_thr_average, 100, 90),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
@ -394,6 +394,9 @@ void Copter::ten_hz_logging_loop()
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();
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
@ -476,9 +479,7 @@ void Copter::one_hz_loop()
motors.set_frame_orientation(g.frame_orientation);
// set all throttle channel settings
motors.set_throttle_range(g.throttle_min, channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
// set hover throttle
motors.set_hover_throttle(g.throttle_mid);
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
}

43
ArduCopter/Attitude.cpp
View File

@ -2,7 +2,7 @@
#include "Copter.h"
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float Copter::get_smoothing_gain()
{
@ -57,6 +57,7 @@ void Copter::check_ekf_yaw_reset()
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
ekfYawReset_ms = new_ekfYawReset_ms;
Log_Write_Event(DATA_EKF_YAW_RESET);
}
}
@ -94,31 +95,35 @@ float Copter::get_look_ahead_yaw()
* throttle control
****************************************************************/
// update_thr_average - update estimated throttle required to hover (if necessary)
// should be called at 100hz
void Copter::update_thr_average()
// update estimated throttle required to hover (if necessary)
// called at 100hz
void Copter::update_throttle_hover()
{
// ensure throttle_average has been initialised
if( is_zero(throttle_average) ) {
throttle_average = 0.5f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
}
#if FRAME_CONFIG != HELI_FRAME
// if not armed or landed exit
if (!motors.armed() || ap.land_complete) {
return;
}
// do not update in manual throttle modes or Drift
if (mode_has_manual_throttle(control_mode) || (control_mode == DRIFT)) {
return;
}
// do not update while climbing or descending
if (!is_zero(pos_control.get_desired_velocity().z)) {
return;
}
// get throttle output
float throttle = motors.get_throttle();
// calc average throttle if we are in a level hover
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_average = throttle_average * 0.99f + throttle * 0.01f;
// update position controller
pos_control.set_throttle_hover(throttle_average);
// Can we set the time constant automatically
motors.update_throttle_hover(0.01f);
}
#endif
}
// set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared
@ -139,9 +144,9 @@ float Copter::get_pilot_desired_throttle(int16_t throttle_control)
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
float thr_mid = MAX(0,g.throttle_mid-g.throttle_min) / (float)(1000-g.throttle_min);
float thr_mid = constrain_float(motors.get_throttle_hover(), 0.1f, 0.9f);
// check throttle is above, below or in the deadband
if (throttle_control < mid_stick) {
@ -199,9 +204,7 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
float Copter::get_non_takeoff_throttle()
{
// ensure mid throttle is set within a reasonable range
g.throttle_mid = constrain_int16(g.throttle_mid,g.throttle_min+50,700);
return MAX(0,g.throttle_mid-g.throttle_min) / ((float)(1000-g.throttle_min) * 2.0f);
return MAX(0,motors.get_throttle_hover()/2.0f);
}
float Copter::get_takeoff_trigger_throttle()
@ -285,7 +288,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
void Copter::set_accel_throttle_I_from_pilot_throttle(float pilot_throttle)
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_accel_z.set_integrator((pilot_throttle-throttle_average) * 1000.0f);
g.pid_accel_z.set_integrator((pilot_throttle-motors.get_throttle_hover()) * 1000.0f);
}
// updates position controller's maximum altitude using fence and EKF limits

4
ArduCopter/Copter.cpp
View File

@ -51,7 +51,6 @@ Copter::Copter(void) :
super_simple_cos_yaw(1.0),
super_simple_sin_yaw(0.0f),
initial_armed_bearing(0),
throttle_average(0.0f),
desired_climb_rate(0),
loiter_time_max(0),
loiter_time(0),
@ -77,6 +76,7 @@ Copter::Copter(void) :
pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
g.p_pos_xy, g.pi_vel_xy),
avoid(ahrs, inertial_nav, fence),
wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
circle_nav(inertial_nav, ahrs, pos_control),
pmTest1(0),
@ -92,7 +92,7 @@ Copter::Copter(void) :
camera_mount(ahrs, current_loc),
#endif
#if AC_FENCE == ENABLED
fence(inertial_nav),
fence(ahrs, inertial_nav),
#endif
#if AC_RALLY == ENABLED
rally(ahrs),

9
ArduCopter/Copter.h
View File

@ -76,6 +76,7 @@
#include <AC_WPNav/AC_Circle.h> // circle navigation library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence/AC_Fence.h> // Arducopter Fence library
#include <AC_Avoidance/AC_Avoid.h> // Arducopter stop at fence library
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Notify/AP_Notify.h> // Notify library
@ -268,6 +269,8 @@ private:
uint32_t start_ms;
} takeoff_state;
uint32_t precland_last_update_ms;
RCMapper rcmap;
// board specific config
@ -373,7 +376,6 @@ private:
int32_t initial_armed_bearing;
// Throttle variables
float throttle_average; // estimated throttle required to hover
int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only
// Loiter control
@ -459,6 +461,7 @@ private:
AC_AttitudeControl_Multi attitude_control;
#endif
AC_PosControl pos_control;
AC_Avoid avoid;
AC_WPNav wp_nav;
AC_Circle circle_nav;
@ -620,7 +623,7 @@ private:
void check_ekf_yaw_reset();
float get_roi_yaw();
float get_look_ahead_yaw();
void update_thr_average();
void update_throttle_hover();
void set_throttle_takeoff();
float get_pilot_desired_throttle(int16_t throttle_control);
float get_pilot_desired_climb_rate(float throttle_control);
@ -664,7 +667,6 @@ private:
void Log_Write_Performance();
void Log_Write_Attitude();
void Log_Write_MotBatt();
void Log_Write_Startup();
void Log_Write_Event(uint8_t id);
void Log_Write_Data(uint8_t id, int32_t value);
void Log_Write_Data(uint8_t id, uint32_t value);
@ -911,6 +913,7 @@ private:
void pre_arm_rc_checks();
bool pre_arm_gps_checks(bool display_failure);
bool pre_arm_ekf_attitude_check();
bool pre_arm_rallypoint_check();
bool pre_arm_terrain_check(bool display_failure);
bool arm_checks(bool display_failure, bool arming_from_gcs);
void init_disarm_motors();

30
ArduCopter/GCS_Mavlink.cpp
View File

@ -740,6 +740,11 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
case MSG_MAG_CAL_REPORT:
copter.compass.send_mag_cal_report(chan);
break;
case MSG_ADSB_VEHICLE:
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
copter.adsb.send_adsb_vehicle(chan);
break;
}
return true;
@ -827,7 +832,16 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0),
AP_GROUPEND
// @Param: ADSB
// @DisplayName: ADSB stream rate to ground station
// @Description: ADSB stream rate to ground station
// @Units: Hz
// @Range: 0 50
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5),
AP_GROUPEND
};
void
@ -933,6 +947,12 @@ GCS_MAVLINK_Copter::data_stream_send(void)
send_message(MSG_VIBRATION);
send_message(MSG_RPM);
}
if (copter.gcs_out_of_time) return;
if (stream_trigger(STREAM_ADSB)) {
send_message(MSG_ADSB_VEHICLE);
}
}
@ -1924,6 +1944,14 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
#endif
#if AC_FENCE == ENABLED
// send or receive fence points with GCS
case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160
case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
copter.fence.handle_msg(chan, msg);
break;
#endif // AC_FENCE == ENABLED
#if CAMERA == ENABLED
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202

12
ArduCopter/Log.cpp
View File

@ -302,13 +302,14 @@ struct PACKED log_Control_Tuning {
float throttle_in;
float angle_boost;
float throttle_out;
float throttle_hover;
float desired_alt;
float inav_alt;
int32_t baro_alt;
int16_t desired_rangefinder_alt;
int16_t rangefinder_alt;
float terr_alt;
int16_t desired_climb_rate;
int16_t target_climb_rate;
int16_t climb_rate;
};
@ -327,13 +328,14 @@ void Copter::Log_Write_Control_Tuning()
throttle_in : attitude_control.get_throttle_in(),
angle_boost : attitude_control.angle_boost(),
throttle_out : motors.get_throttle(),
throttle_hover : motors.get_throttle_hover(),
desired_alt : pos_control.get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : baro_alt,
desired_rangefinder_alt : (int16_t)target_rangefinder_alt,
rangefinder_alt : rangefinder_state.alt_cm,
terr_alt : terr_alt,
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
target_climb_rate : (int16_t)pos_control.get_vel_target_z(),
climb_rate : climb_rate
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -727,7 +729,7 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" },
"CTUN", "Qffffffeccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
@ -776,6 +778,7 @@ void Copter::Log_Write_Vehicle_Startup_Messages()
// only 200(?) bytes are guaranteed by DataFlash
DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING);
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
DataFlash.Log_Write_Rally(rally);
}
@ -832,7 +835,6 @@ void Copter::Log_Write_Control_Tuning() {}
void Copter::Log_Write_Performance() {}
void Copter::Log_Write_Attitude(void) {}
void Copter::Log_Write_MotBatt() {}
void Copter::Log_Write_Startup() {}
void Copter::Log_Write_Event(uint8_t id) {}
void Copter::Log_Write_Data(uint8_t id, int32_t value) {}
void Copter::Log_Write_Data(uint8_t id, uint32_t value) {}
@ -844,7 +846,7 @@ void Copter::Log_Write_Baro(void) {}
void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
void Copter::Log_Write_Home_And_Origin() {}
void Copter::Log_Sensor_Health() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {};
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
#if FRAME_CONFIG == HELI_FRAME
void Copter::Log_Write_Heli() {}

41
ArduCopter/Parameters.cpp
View File

@ -278,15 +278,6 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT),
// @Param: THR_MIN
// @DisplayName: Throttle Minimum
// @Description: The minimum throttle that will be sent to the motors to keep them spinning
// @Units: Percent*10
// @Range: 0 300
// @Increment: 1
// @User: Standard
GSCALAR(throttle_min, "THR_MIN", THR_MIN_DEFAULT),
// @Param: FS_THR_ENABLE
// @DisplayName: Throttle Failsafe Enable
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
@ -303,15 +294,6 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT),
// @Param: THR_MID
// @DisplayName: Throttle Mid Position
// @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover
// @User: Standard
// @Range: 300 700
// @Units: Percent*10
// @Increment: 10
GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT),
// @Param: THR_DZ
// @DisplayName: Throttle deadzone
// @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes
@ -768,7 +750,7 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Heli),
#else
// @Group: ATC_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Multi),
#endif
@ -843,6 +825,10 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(fence, "FENCE_", AC_Fence),
#endif
// @Group: AVOID_
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
GOBJECT(avoid, "AVOID_", AC_Avoid),
#if AC_RALLY == ENABLED
// @Group: RALLY_
// @Path: ../libraries/AP_Rally/AP_Rally.cpp
@ -855,18 +841,12 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(motors, "H_", AP_MotorsHeli_Single),
#elif FRAME_CONFIG == SINGLE_FRAME
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsSingle.cpp
GOBJECT(motors, "MOT_", AP_MotorsSingle),
#elif FRAME_CONFIG == COAX_FRAME
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsCoax.cpp
GOBJECT(motors, "MOT_", AP_MotorsCoax),
#elif FRAME_CONFIG == TRI_FRAME
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsTri.cpp
GOBJECT(motors, "MOT_", AP_MotorsTri),
#else
@ -963,7 +943,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: TERRAIN_FOLLOW
// @DisplayName: Terrain Following use control
// @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain.
// @Values: 0:Do Not Use in RTL and Land 1:Use in RTL and Land
// @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land
// @User: Standard
GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
@ -1067,6 +1047,10 @@ void Copter::convert_pid_parameters(void)
{ Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" },
{ Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }
};
AP_Param::ConversionInfo throttle_conversion_info[] = {
{ Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" },
{ Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" }
};
// gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees
// and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500
@ -1094,4 +1078,9 @@ void Copter::convert_pid_parameters(void)
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&angle_and_filt_conversion_info[i], 1.0f);
}
// convert throttle parameters (multicopter only)
table_size = ARRAY_SIZE(throttle_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&throttle_conversion_info[i], 0.001f);
}
}

7
ArduCopter/Parameters.h
View File

@ -181,6 +181,7 @@ public:
k_param_fs_crash_check,
k_param_throw_motor_start,
k_param_terrain_follow, // 94
k_param_avoid,
// 97: RSSI
k_param_rssi = 97,
@ -280,7 +281,7 @@ public:
k_param_rc_8,
k_param_rc_10,
k_param_rc_11,
k_param_throttle_min,
k_param_throttle_min, // remove
k_param_throttle_max, // remove
k_param_failsafe_throttle,
k_param_throttle_fs_action, // remove
@ -292,7 +293,7 @@ public:
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid,
k_param_throttle_mid, // remove
k_param_failsafe_gps_enabled, // remove
k_param_rc_9,
k_param_rc_12,
@ -414,10 +415,8 @@ public:
// Throttle
//
AP_Int16 throttle_min;
AP_Int8 failsafe_throttle;
AP_Int16 failsafe_throttle_value;
AP_Int16 throttle_mid;
AP_Int16 throttle_deadzone;
// Flight modes

35
ArduCopter/arming_checks.cpp
View File

@ -174,6 +174,14 @@ bool Copter::pre_arm_checks(bool display_failure)
}
#endif
// check rally points
if (!pre_arm_rallypoint_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: rallypoints outside fence");
}
return false;
}
// check INS
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
@ -491,6 +499,25 @@ bool Copter::pre_arm_ekf_attitude_check()
return filt_status.flags.attitude;
}
// check rally points are within fences
bool Copter::pre_arm_rallypoint_check()
{
#if AC_RALLY == ENABLED && AC_FENCE == ENABLED
for (uint8_t i=0; i<rally.get_rally_total(); i++) {
RallyLocation rally_loc;
if (rally.get_rally_point_with_index(i, rally_loc)) {
Location_Class rally_point(rally.rally_location_to_location(rally_loc));
if (!fence.check_destination_within_fence(rally_point)) {
return false;
}
}
}
return true;
#else
return true;
#endif
}
// check we have required terrain data
bool Copter::pre_arm_terrain_check(bool display_failure)
{
@ -650,6 +677,14 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
}
#endif
// check rally points
if (!pre_arm_rallypoint_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: rallypoints outside fence");
}
return false;
}
// check lean angle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) {

2
ArduCopter/capabilities.cpp
View File

@ -11,5 +11,7 @@ void Copter::init_capabilities(void)
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
#endif
}

8
ArduCopter/config.h
View File

@ -547,14 +547,6 @@
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THR_MID_DEFAULT
# define THR_MID_DEFAULT 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position
#endif
#ifndef THR_MIN_DEFAULT
# define THR_MIN_DEFAULT 130 // minimum throttle sent to the motors when armed and pilot throttle above zero
#endif
#define THR_MAX 1000 // maximum throttle input and output sent to the motors
#ifndef THR_DZ_DEFAULT
# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter

16
ArduCopter/control_althold.cpp
View File

@ -84,7 +84,7 @@ void Copter::althold_run()
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
@ -92,7 +92,7 @@ void Copter::althold_run()
#else
// Multicopters do not stabilize roll/pitch/yaw when motor are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
#endif
break;
@ -102,13 +102,13 @@ void Copter::althold_run()
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case AltHold_Takeoff:
@ -129,7 +129,7 @@ void Copter::althold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -143,7 +143,7 @@ void Copter::althold_run()
attitude_control.set_yaw_target_to_current_heading();
#endif
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
// set motors to spin-when-armed if throttle at zero, otherwise full range
if (ap.throttle_zero) {
@ -151,13 +151,13 @@ void Copter::althold_run()
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case AltHold_Flying:
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {

37
ArduCopter/control_auto.cpp
View File

@ -150,7 +150,7 @@ void Copter::auto_takeoff_run()
wp_nav.shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -179,7 +179,7 @@ void Copter::auto_takeoff_run()
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
@ -226,7 +226,7 @@ void Copter::auto_wp_run()
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -259,10 +259,10 @@ void Copter::auto_wp_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
}
@ -298,7 +298,7 @@ void Copter::auto_spline_run()
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -331,10 +331,10 @@ void Copter::auto_spline_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
@ -375,7 +375,7 @@ void Copter::auto_land_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -421,6 +421,17 @@ void Copter::auto_land_run()
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -433,7 +444,7 @@ void Copter::auto_land_run()
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// auto_rtl_start - initialises RTL in AUTO flight mode
@ -528,7 +539,7 @@ void Copter::auto_circle_run()
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
}
#if NAV_GUIDED == ENABLED
@ -587,7 +598,7 @@ void Copter::auto_loiter_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -610,7 +621,7 @@ void Copter::auto_loiter_run()
failsafe_terrain_set_status(wp_nav.update_wpnav());
pos_control.update_z_controller();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter

28
ArduCopter/control_autotune.cpp
View File

@ -215,7 +215,7 @@ void Copter::autotune_stop()
autotune_load_orig_gains();
// re-enable angle-to-rate request limits
attitude_control.limit_angle_to_rate_request(true);
attitude_control.use_ff_and_input_shaping(true);
// log off event and send message to ground station
autotune_update_gcs(AUTOTUNE_MESSAGE_STOPPED);
@ -271,7 +271,7 @@ void Copter::autotune_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -304,7 +304,7 @@ void Copter::autotune_run()
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
}else{
// check if pilot is overriding the controls
if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || target_climb_rate != 0) {
@ -312,7 +312,7 @@ void Copter::autotune_run()
autotune_state.pilot_override = true;
// set gains to their original values
autotune_load_orig_gains();
attitude_control.limit_angle_to_rate_request(true);
attitude_control.use_ff_and_input_shaping(true);
}
// reset pilot override time
autotune_override_time = millis();
@ -332,7 +332,7 @@ void Copter::autotune_run()
// if pilot override call attitude controller
if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
}else{
// somehow get attitude requests from autotuning
autotune_attitude_control();
@ -357,10 +357,10 @@ void Copter::autotune_attitude_control()
case AUTOTUNE_STEP_WAITING_FOR_LEVEL:
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
// re-enable rate limits
attitude_control.limit_angle_to_rate_request(true);
attitude_control.use_ff_and_input_shaping(true);
// hold level attitude
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true);
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true, get_smoothing_gain());
// hold the copter level for 0.5 seconds before we begin a twitch
// reset counter if we are no longer level
@ -432,29 +432,29 @@ void Copter::autotune_attitude_control()
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
// disable rate limits
attitude_control.limit_angle_to_rate_request(false);
attitude_control.use_ff_and_input_shaping(false);
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
// Testing increasing stabilize P gain so will set lean angle target
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
// request roll to 20deg
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f, get_smoothing_gain());
break;
case AUTOTUNE_AXIS_PITCH:
// request pitch to 20deg
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, get_smoothing_gain());
break;
case AUTOTUNE_AXIS_YAW:
// request pitch to 20deg
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false);
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false, get_smoothing_gain());
break;
}
} else {
// Testing rate P and D gains so will set body-frame rate targets.
// Rate controller will use existing body-frame rates and convert to motor outputs
// for all axes except the one we override here.
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f, get_smoothing_gain());
switch (autotune_state.axis) {
case AUTOTUNE_AXIS_ROLL:
// override body-frame roll rate
@ -531,7 +531,7 @@ void Copter::autotune_attitude_control()
case AUTOTUNE_STEP_UPDATE_GAINS:
// re-enable rate limits
attitude_control.limit_angle_to_rate_request(true);
attitude_control.use_ff_and_input_shaping(true);
// log the latest gains
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
@ -730,7 +730,7 @@ void Copter::autotune_attitude_control()
autotune_state.positive_direction = !autotune_state.positive_direction;
if (autotune_state.axis == AUTOTUNE_AXIS_YAW) {
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false);
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
}
// set gains to their intra-test values (which are very close to the original gains)

6
ArduCopter/control_brake.cpp
View File

@ -42,14 +42,14 @@ void Copter::brake_run()
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-motors.get_throttle_hover());
return;
}
@ -70,7 +70,7 @@ void Copter::brake_run()
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop

6
ArduCopter/control_circle.cpp
View File

@ -46,7 +46,7 @@ void Copter::circle_run()
// To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -85,9 +85,9 @@ void Copter::circle_run()
// call attitude controller
if (circle_pilot_yaw_override) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
}
// adjust climb rate using rangefinder

2
ArduCopter/control_drift.cpp
View File

@ -98,7 +98,7 @@ void Copter::drift_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle with angle boost
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);

2
ArduCopter/control_flip.cpp
View File

@ -177,7 +177,7 @@ void Copter::flip_run()
case Flip_Recover:
// use originally captured earth-frame angle targets to recover
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
// increase throttle to gain any lost altitude
throttle_out += FLIP_THR_INC;

42
ArduCopter/control_guided.cpp
View File

@ -177,7 +177,8 @@ bool Copter::guided_set_destination(const Vector3f& destination)
#if AC_FENCE == ENABLED
// reject destination if outside the fence
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
Location_Class dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
@ -205,13 +206,10 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc)
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
int32_t alt_target_cm;
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
@ -326,7 +324,7 @@ void Copter::guided_takeoff_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -353,7 +351,7 @@ void Copter::guided_takeoff_run()
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// guided_pos_control_run - runs the guided position controller
@ -364,7 +362,7 @@ void Copter::guided_pos_control_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -396,10 +394,10 @@ void Copter::guided_pos_control_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
@ -413,7 +411,7 @@ void Copter::guided_vel_control_run()
pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -448,10 +446,10 @@ void Copter::guided_vel_control_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
@ -466,7 +464,7 @@ void Copter::guided_posvel_control_run()
pos_control.set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -522,10 +520,10 @@ void Copter::guided_posvel_control_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
@ -538,7 +536,7 @@ void Copter::guided_angle_control_run()
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -564,7 +562,7 @@ void Copter::guided_angle_control_run()
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up());
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
@ -578,7 +576,7 @@ void Copter::guided_angle_control_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);

16
ArduCopter/control_land.cpp
View File

@ -59,7 +59,7 @@ void Copter::land_gps_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -123,8 +123,12 @@ void Copter::land_gps_run()
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active) {
wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif
@ -132,7 +136,7 @@ void Copter::land_gps_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
@ -183,7 +187,7 @@ void Copter::land_nogps_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -209,7 +213,7 @@ void Copter::land_nogps_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;

16
ArduCopter/control_loiter.cpp
View File

@ -97,7 +97,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
@ -106,7 +106,7 @@ void Copter::loiter_run()
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
#endif
break;
@ -116,13 +116,13 @@ void Copter::loiter_run()
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case Loiter_Takeoff:
@ -145,7 +145,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
@ -157,7 +157,7 @@ void Copter::loiter_run()
wp_nav.init_loiter_target();
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
// if throttle zero reset attitude and exit immediately
@ -166,7 +166,7 @@ void Copter::loiter_run()
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
break;
case Loiter_Flying:
@ -178,7 +178,7 @@ void Copter::loiter_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {

6
ArduCopter/control_poshold.cpp
View File

@ -145,7 +145,7 @@ void Copter::poshold_run()
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -193,7 +193,7 @@ void Copter::poshold_run()
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}else{
// convert pilot input to lean angles
@ -517,7 +517,7 @@ void Copter::poshold_run()
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
// update attitude controller targets
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (rangefinder_alt_ok()) {

20
ArduCopter/control_rtl.cpp
View File

@ -137,7 +137,7 @@ void Copter::rtl_climb_return_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -171,10 +171,10 @@ void Copter::rtl_climb_return_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
// check if we've completed this stage of RTL
@ -204,7 +204,7 @@ void Copter::rtl_loiterathome_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -238,10 +238,10 @@ void Copter::rtl_loiterathome_run()
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
// check if we've completed this stage of RTL
@ -285,7 +285,7 @@ void Copter::rtl_descent_run()
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -334,7 +334,7 @@ void Copter::rtl_descent_run()
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
@ -366,7 +366,7 @@ void Copter::rtl_land_run()
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -439,7 +439,7 @@ void Copter::rtl_land_run()
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;

4
ArduCopter/control_sport.cpp
View File

@ -36,7 +36,7 @@ void Copter::sport_run()
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
return;
}
@ -99,7 +99,7 @@ void Copter::sport_run()
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
}else{
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

2
ArduCopter/control_stabilize.cpp
View File

@ -50,7 +50,7 @@ void Copter::stabilize_run()
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop

6
ArduCopter/control_throw.cpp
View File

@ -139,7 +139,7 @@ void Copter::throw_run()
case Throw_Uprighting:
// demand a level roll/pitch attitude with zero yaw rate
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control.set_throttle_out(500, false, g.throttle_filt);
@ -149,7 +149,7 @@ void Copter::throw_run()
case Throw_HgtStabilise:
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
// call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
@ -163,7 +163,7 @@ void Copter::throw_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
// call height controller
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);

16
ArduCopter/crash_check.cpp
View File

@ -4,7 +4,7 @@
// Code to detect a crash main ArduCopter code
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
#define CRASH_CHECK_ANGLE_DEVIATION_CD 3000.0f // 30 degrees beyond angle max is signal we are inverted
#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond angle max is signal we are inverted
#define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
// crash_check - disarms motors if a crash has been detected
@ -33,8 +33,8 @@ void Copter::crash_check()
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
const float angle_error = attitude_control.get_att_error_angle_deg();
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
crash_counter = 0;
return;
}
@ -43,7 +43,7 @@ void Copter::crash_check()
crash_counter++;
// check if crashing for 2 seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) {
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
@ -99,14 +99,14 @@ void Copter::parachute_check()
}
// check for angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) {
const float angle_error = attitude_control.get_att_error_angle_deg();
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
control_loss_count = 0;
return;
}
// increment counter
if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) {
control_loss_count++;
}
@ -122,7 +122,7 @@ void Copter::parachute_check()
// To-Do: add check that the vehicle is actually falling
// check if loss of control for at least 1 second
} else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
} else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) {
// reset control loss counter
control_loss_count = 0;
// log an error in the dataflash

1
ArduCopter/defines.h
View File

@ -350,6 +350,7 @@ enum ThrowModeState {
#define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only
#define DATA_EKF_ALT_RESET 60
#define DATA_LAND_CANCELLED_BY_PILOT 61
#define DATA_EKF_YAW_RESET 62
// Centi-degrees to radians
#define DEGX100 5729.57795f

4
ArduCopter/heli.cpp
View File

@ -87,10 +87,10 @@ void Copter::update_heli_control_dynamics(void)
// if we are not landed and motor power is demanded, increment slew scalar
hover_roll_trim_scalar_slew++;
}
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, MAIN_LOOP_RATE);
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, scheduler.get_loop_rate_hz());
// set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/MAIN_LOOP_RATE));
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/scheduler.get_loop_rate_hz()));
}
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing

2
ArduCopter/heli_control_stabilize.cpp
View File

@ -60,7 +60,7 @@ void Copter::heli_stabilize_run()
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);

29
ArduCopter/land_detector.cpp
View File

@ -2,6 +2,11 @@
#include "Copter.h"
// Code to detect a crash main ArduCopter code
#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
// counter to verify landings
static uint32_t land_detector_count = 0;
@ -57,7 +62,7 @@ void Copter::update_land_detector()
bool motor_at_lower_limit = motors.limit.throttle_lower;
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min();
bool motor_at_lower_limit = motors.limit.throttle_lower && attitude_control.is_throttle_mix_min();
#endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
@ -65,7 +70,7 @@ void Copter::update_land_detector()
if (motor_at_lower_limit && accel_stationary) {
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) {
land_detector_count++;
} else {
set_land_complete(true);
@ -76,7 +81,7 @@ void Copter::update_land_detector()
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE));
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
}
// set land_complete flag and disarm motors if disarm-on-land is configured
@ -125,40 +130,40 @@ void Copter::update_throttle_thr_mix()
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
attitude_control.set_throttle_mix_min();
return;
}
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
if(channel_throttle->get_control_in() <= 0) {
motors.set_throttle_mix_min();
attitude_control.set_throttle_mix_min();
} else {
motors.set_throttle_mix_mid();
attitude_control.set_throttle_mix_mid();
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f);
bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
// check for large external disturbance - angle error over 30 degrees
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f);
const float angle_error = attitude_control.get_att_error_angle_deg();
bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
// check for large acceleration - falling or high turbulence
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > 3.0f);
bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
// check for requested decent
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
motors.set_throttle_mix_max();
attitude_control.set_throttle_mix_max();
} else {
motors.set_throttle_mix_min();
attitude_control.set_throttle_mix_min();
}
}
#endif

1
ArduCopter/make.inc
View File

@ -43,6 +43,7 @@ LIBRARIES += AC_WPNav
LIBRARIES += AC_Circle
LIBRARIES += AP_Declination
LIBRARIES += AC_Fence
LIBRARIES += AC_Avoidance
LIBRARIES += AP_Scheduler
LIBRARIES += AP_RCMapper
LIBRARIES += AP_Notify

6
ArduCopter/radio.cpp
View File

@ -32,7 +32,7 @@ void Copter::init_rc_in()
channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
channel_yaw->set_angle(4500);
channel_throttle->set_range(0, THR_MAX);
channel_throttle->set_range(0, 1000);
channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
@ -56,10 +56,10 @@ void Copter::init_rc_out()
{
motors.set_update_rate(g.rc_speed);
motors.set_frame_orientation(g.frame_orientation);
motors.set_loop_rate(scheduler.get_loop_rate_hz());
motors.Init(); // motor initialisation
#if FRAME_CONFIG != HELI_FRAME
motors.set_throttle_range(g.throttle_min, channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
motors.set_hover_throttle(g.throttle_mid);
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
for(uint8_t i = 0; i < 5; i++) {

1
ArduCopter/system.cpp
View File

@ -200,6 +200,7 @@ void Copter::init_ardupilot()
Location_Class::set_terrain(&terrain);
wp_nav.set_terrain(&terrain);
#endif
wp_nav.set_avoidance(&avoid);
pos_control.set_dt(MAIN_LOOP_SECONDS);

1
ArduCopter/wscript
View File

@ -11,6 +11,7 @@ def build(bld):
'AC_AttitudeControl',
'AC_InputManager',
'AC_Fence',
'AC_Avoidance',
'AC_PID',
'AC_PrecLand',
'AC_Sprayer',

14
ArduPlane/ArduPlane.cpp
View File

@ -164,7 +164,7 @@ void Plane::ahrs_update()
}
// calculate a scaled roll limit based on current pitch
roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
roll_limit_cd = aparm.roll_limit_cd * cosf(ahrs.pitch);
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
// updated the summed gyro used for ground steering and
@ -491,6 +491,9 @@ void Plane::update_GPS_10Hz(void)
// update wind estimate
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
// lost 3D fix, start again
ground_start_count = 5;
}
calc_gndspeed_undershoot();
@ -737,13 +740,14 @@ void Plane::update_flight_mode(void)
case QLAND:
case QRTL: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
nav_roll_cd = channel_roll->norm_input() * roll_limit;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max);
} else {
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
break;

73
ArduPlane/Attitude.cpp
View File

@ -38,7 +38,7 @@ float Plane::get_speed_scaler(void)
*/
bool Plane::stick_mixing_enabled(void)
{
if (auto_throttle_mode) {
if (auto_throttle_mode && auto_navigation_mode) {
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
geofence_stickmixing() &&
@ -429,7 +429,16 @@ void Plane::calc_throttle()
return;
}
channel_throttle->set_servo_out(SpdHgt_Controller->get_throttle_demand());
int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand();
// Received an external msg that guides throttle in the last 3 seconds?
if (control_mode == GUIDED &&
plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
commanded_throttle = plane.guided_state.forced_throttle;
}
channel_throttle->set_servo_out(commanded_throttle);
}
/*****************************************
@ -445,12 +454,23 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
if (control_mode == STABILIZE && rudder_input != 0) {
disable_integrator = true;
}
steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll
steering_control.rudder += channel_roll->get_servo_out() * g.kff_rudder_mix;
steering_control.rudder += rudder_input;
steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500);
int16_t commanded_rudder;
// Received an external msg that guides yaw in the last 3 seconds?
if (control_mode == GUIDED &&
plane.guided_state.last_forced_rpy_ms.z > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
commanded_rudder = plane.guided_state.forced_rpy_cd.z;
} else {
commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll
commanded_rudder += channel_roll->get_servo_out() * g.kff_rudder_mix;
commanded_rudder += rudder_input;
}
steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
}
/*
@ -519,8 +539,16 @@ void Plane::calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand();
// Received an external msg that guides roll in the last 3 seconds?
if (control_mode == GUIDED &&
plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
commanded_pitch = plane.guided_state.forced_rpy_cd.y;
}
nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}
@ -529,7 +557,16 @@ void Plane::calc_nav_pitch()
*/
void Plane::calc_nav_roll()
{
nav_roll_cd = constrain_int32(nav_controller->nav_roll_cd(), -roll_limit_cd, roll_limit_cd);
int32_t commanded_roll = nav_controller->nav_roll_cd();
// Received an external msg that guides roll in the last 3 seconds?
if (control_mode == GUIDED &&
plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
commanded_roll = plane.guided_state.forced_rpy_cd.x;
}
nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
update_load_factor();
}
@ -1305,22 +1342,6 @@ bool Plane::allow_reverse_thrust(void)
return allow;
}
void Plane::demo_servos(uint8_t i)
{
while(i > 0) {
gcs_send_text(MAV_SEVERITY_INFO,"Demo servos");
demoing_servos = true;
servo_write(1, 1400);
hal.scheduler->delay(400);
servo_write(1, 1600);
hal.scheduler->delay(200);
servo_write(1, 1500);
demoing_servos = false;
hal.scheduler->delay(400);
i--;
}
}
/*
adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make
keeping up good airspeed in FBWA mode easier, as the plane will

90
ArduPlane/GCS_Mavlink.cpp
View File

@ -873,6 +873,11 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
CHECK_PAYLOAD_SIZE(MAG_CAL_REPORT);
plane.compass.send_mag_cal_report(chan);
break;
case MSG_ADSB_VEHICLE:
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
plane.adsb.send_adsb_vehicle(chan);
break;
}
return true;
}
@ -962,6 +967,15 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10),
// @Param: ADSB
// @DisplayName: ADSB stream rate to ground station
// @Description: ADSB stream rate to ground station
// @Units: Hz
// @Range: 0 50
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5),
AP_GROUPEND
};
@ -1081,6 +1095,12 @@ GCS_MAVLINK_Plane::data_stream_send(void)
send_message(MSG_GIMBAL_REPORT);
send_message(MSG_VIBRATION);
}
if (plane.gcs_out_of_time) return;
if (stream_trigger(STREAM_ADSB)) {
send_message(MSG_ADSB_VEHICLE);
}
}
@ -2083,6 +2103,76 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
plane.DataFlash.remote_log_block_status_msg(chan, msg);
break;
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
{
// Only allow companion computer (or other external controller) to
// control attitude in GUIDED mode. We DON'T want external control
// in e.g., RTL, CICLE. Specifying a single mode for companion
// computer control is more safe (even more so when using
// FENCE_ACTION = 4 for geofence failures).
if (plane.control_mode != GUIDED) { // don't screw up failsafes
break;
}
mavlink_set_attitude_target_t att_target;
mavlink_msg_set_attitude_target_decode(msg, &att_target);
// Mappings: If any of these bits are set, the corresponding input should be ignored.
// NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted
// bit 1: body roll rate
// bit 2: body pitch rate
// bit 3: body yaw rate
// bit 4: unknown
// bit 5: unknown
// bit 6: reserved
// bit 7: throttle
// bit 8: attitude
// if not setting all Quaternion values, use _rate flags to indicate which fields.
// Extract the Euler roll angle from the Quaternion.
Quaternion q(att_target.q[0], att_target.q[1],
att_target.q[2], att_target.q[3]);
// NOTE: att_target.type_mask is inverted for easier interpretation
att_target.type_mask = att_target.type_mask ^ 0xFF;
uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy
uint32_t now = AP_HAL::millis();
if ((attitude_mask & 0b10000001) || // partial, including roll
(attitude_mask == 0b10000000)) { // all angles
plane.guided_state.forced_rpy_cd.x = degrees(q.get_euler_roll()) * 100.0f;
// Update timer for external roll to the nav control
plane.guided_state.last_forced_rpy_ms.x = now;
}
if ((attitude_mask & 0b10000010) || // partial, including pitch
(attitude_mask == 0b10000000)) { // all angles
plane.guided_state.forced_rpy_cd.y = degrees(q.get_euler_pitch()) * 100.0f;
// Update timer for external pitch to the nav control
plane.guided_state.last_forced_rpy_ms.y = now;
}
if ((attitude_mask & 0b10000100) || // partial, including yaw
(attitude_mask == 0b10000000)) { // all angles
plane.guided_state.forced_rpy_cd.z = degrees(q.get_euler_yaw()) * 100.0f;
// Update timer for external yaw to the nav control
plane.guided_state.last_forced_rpy_ms.z = now;
}
if (att_target.type_mask & 0b01000000) { // throttle
plane.guided_state.forced_throttle = att_target.thrust * 100.0f;
// Update timer for external throttle
plane.guided_state.last_forced_throttle_ms = now;
}
break;
}
case MAVLINK_MSG_ID_SET_HOME_POSITION:
{
mavlink_set_home_position_t packet;

1
ArduPlane/Log.cpp
View File

@ -520,6 +520,7 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
// only 200(?) bytes are guaranteed by DataFlash
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
DataFlash.Log_Write_Mode(control_mode);
DataFlash.Log_Write_Rally(rally);
}
// start a new log

2
ArduPlane/Parameters.cpp
View File

@ -736,7 +736,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Range: 0 9000
// @Increment: 1
// @User: Standard
GSCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE),
ASCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE),
// @Param: LIM_PITCH_MAX
// @DisplayName: Maximum Pitch Angle

1
ArduPlane/Parameters.h
View File

@ -449,7 +449,6 @@ public:
// Navigational maneuvering limits
//
AP_Int16 roll_limit_cd;
AP_Int16 alt_offset;
AP_Int16 acro_roll_rate;
AP_Int16 acro_pitch_rate;

27
ArduPlane/Plane.h
View File

@ -526,6 +526,17 @@ private:
bool vtol_loiter:1;
} auto_state;
struct {
// roll pitch yaw commanded from external controller in centidegrees
Vector3l forced_rpy_cd;
// last time we heard from the external controller
Vector3l last_forced_rpy_ms;
// throttle commanded from external controller in percent
float forced_throttle;
uint32_t last_forced_throttle_ms;
} guided_state;
struct {
// on hard landings, only check once after directly a landing so you
// don't trigger a crash when picking up the aircraft
@ -549,11 +560,15 @@ private:
// true if we are in an auto-throttle mode, which means
// we need to run the speed/height controller
bool auto_throttle_mode;
bool auto_throttle_mode:1;
// true if we are in an auto-navigation mode, which controls whether control input is ignored
// with STICK_MIXING=0
bool auto_navigation_mode:1;
// this controls throttle suppression in auto modes
bool throttle_suppressed;
bool throttle_suppressed:1;
// reduce throttle to eliminate battery over-current
int8_t throttle_watt_limit_max;
int8_t throttle_watt_limit_min; // for reverse thrust
@ -754,7 +769,7 @@ private:
bool gcs_out_of_time = false;
// time that rudder arming has been running
uint32_t rudder_arm_timer;
uint32_t rudder_arm_timer = 0;
// support for quadcopter-plane
QuadPlane quadplane{ahrs};
@ -769,7 +784,6 @@ private:
int32_t last_mixer_crc = -1;
#endif // CONFIG_HAL_BOARD
void demo_servos(uint8_t i);
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
void send_heartbeat(mavlink_channel_t chan);
@ -918,7 +932,8 @@ private:
bool setup_failsafe_mixing(void);
void set_control_channels(void);
void init_rc_in();
void init_rc_out();
void init_rc_out_main();
void init_rc_out_aux();
void rudder_arm_disarm_check();
void read_radio();
void control_failsafe(uint16_t pwm);

8
ArduPlane/altitude.cpp
View File

@ -139,13 +139,17 @@ int32_t Plane::relative_altitude_abs_cm(void)
*/
float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
{
if (use_rangefinder_if_available && rangefinder_state.in_range) {
#if RANGEFINDER_ENABLED == ENABLED
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
}
#endif
#if AP_TERRAIN_AVAILABLE
float altitude;
if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(altitude, true)) {
if (target_altitude.terrain_following &&
terrain.status() == AP_Terrain::TerrainStatusOK &&
terrain.height_above_terrain(altitude, true)) {
return altitude;
}
#endif

4
ArduPlane/arming_checks.cpp
View File

@ -30,9 +30,9 @@ bool AP_Arming_Plane::pre_arm_checks(bool report)
// Check airspeed sensor
ret &= AP_Arming::airspeed_checks(report);
if (plane.g.roll_limit_cd < 300) {
if (plane.aparm.roll_limit_cd < 300) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: LIM_ROLL_CD too small (%u)", plane.g.roll_limit_cd);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: LIM_ROLL_CD too small (%u)", plane.aparm.roll_limit_cd);
}
ret = false;
}

1
ArduPlane/capabilities.cpp
View File

@ -9,5 +9,6 @@ void Plane::init_capabilities(void)
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_COMMAND_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
}

2
ArduPlane/defines.h
View File

@ -6,8 +6,6 @@
#define TRUE 1
#define FALSE 0
#define ToRad(x) radians(x) // *pi/180
#define ToDeg(x) degrees(x) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius

2
ArduPlane/make.inc
View File

@ -54,4 +54,6 @@ LIBRARIES += AC_AttitudeControl
LIBRARIES += AC_PID
LIBRARIES += AP_InertialNav
LIBRARIES += AC_WPNav
LIBRARIES += AC_Fence
LIBRARIES += AC_Avoidance
LIBRARIES += AP_Tuning

2
ArduPlane/navigation.cpp
View File

@ -185,7 +185,7 @@ void Plane::update_loiter(uint16_t radius)
} else if (loiter.start_time_ms == 0 &&
control_mode == AUTO &&
!auto_state.no_crosstrack &&
get_distance(current_loc, next_WP_loc) > radius*2) {
get_distance(current_loc, next_WP_loc) > radius*3) {
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
// navigate to it like in auto-mode for normal crosstrack behavior
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);

2
ArduPlane/parachute.cpp
View File

@ -40,7 +40,7 @@ bool Plane::parachute_manual_release()
}
if (parachute.alt_min() > 0 && relative_ground_altitude(false) < parachute.alt_min() &&
auto_state.started_flying_in_auto_ms > 0 && auto_state.takeoff_complete) {
auto_state.last_flying_ms > 0) {
// Allow manual ground tests by only checking if flying too low if we've taken off
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Parachute: Too low");
return false;

150
ArduPlane/quadplane.cpp
View File

@ -199,14 +199,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
AP_GROUPINFO("LAND_FINAL_ALT", 27, QuadPlane, land_final_alt, 6),
// @Param: THR_MID
// @DisplayName: Throttle Mid Position
// @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover
// @User: Standard
// @Range: 300 700
// @Units: Percent*10
// @Increment: 10
AP_GROUPINFO("THR_MID", 28, QuadPlane, throttle_mid, 500),
// 28 was used by THR_MID
// @Param: TRAN_PIT_MAX
// @DisplayName: Transition max pitch
@ -304,14 +297,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
AP_GROUPINFO("GUIDED_MODE", 40, QuadPlane, guided_mode, 0),
// @Param: THR_MIN
// @DisplayName: Throttle Minimum
// @Description: The minimum throttle that will be sent to the motors to keep them spinning
// @Units: Percent*10
// @Range: 0 300
// @Increment: 1
// @User: Standard
AP_GROUPINFO("THR_MIN", 41, QuadPlane, throttle_min, 100),
// 41 was used by THR_MIN
// @Param: ESC_CAL
// @DisplayName: ESC Calibration
@ -319,6 +305,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Values: 0:Disabled,1:ThrottleInput,2:FullInput
// @User: Standard
AP_GROUPINFO("ESC_CAL", 42, QuadPlane, esc_calibration, 0),
// @Param: VFWD_ALT
// @DisplayName: Forward velocity alt cutoff
// @Description: Controls altitude to disable forward velocity assist when below this relative altitude. This is useful to keep the forward velocity propeller from hitting the ground. Rangefinder height data is incorporated when available.
// @Range: 0 10
// @Increment: 0.25
// @User: Standard
AP_GROUPINFO("VFWD_ALT", 43, QuadPlane, vel_forward_alt_cutoff, 0),
AP_GROUPEND
};
@ -434,8 +428,7 @@ bool QuadPlane::setup(void)
motors->set_frame_orientation(frame_type);
motors->Init();
motors->set_throttle_range(throttle_min, thr_min_pwm, thr_max_pwm);
motors->set_hover_throttle(throttle_mid);
motors->set_throttle_range(thr_min_pwm, thr_max_pwm);
motors->set_update_rate(rc_speed);
motors->set_interlock(true);
pid_accel_z.set_dt(loop_delta_t);
@ -522,7 +515,7 @@ void QuadPlane::init_stabilize(void)
void QuadPlane::hold_stabilize(float throttle_in)
{
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_desired_yaw_rate_cds(),
smoothing_gain);
@ -578,7 +571,7 @@ void QuadPlane::hold_hover(float target_climb_rate)
pos_control->set_accel_z(pilot_accel_z);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_desired_yaw_rate_cds(),
smoothing_gain);
@ -624,7 +617,7 @@ void QuadPlane::init_land(void)
init_loiter();
throttle_wait = false;
poscontrol.state = QPOS_LAND_DESCEND;
motors_lower_limit_start_ms = 0;
landing_detect.lower_limit_start_ms = 0;
}
@ -643,24 +636,25 @@ bool QuadPlane::is_flying(void)
// crude landing detector to prevent tipover
bool QuadPlane::should_relax(void)
{
bool motor_at_lower_limit = motors->limit.throttle_lower && motors->is_throttle_mix_min();
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
if (motors->get_throttle() < 0.01) {
motor_at_lower_limit = true;
}
if (!motor_at_lower_limit) {
motors_lower_limit_start_ms = 0;
landing_detect.lower_limit_start_ms = 0;
}
if (motor_at_lower_limit && motors_lower_limit_start_ms == 0) {
motors_lower_limit_start_ms = millis();
if (motor_at_lower_limit && landing_detect.lower_limit_start_ms == 0) {
landing_detect.lower_limit_start_ms = millis();
}
bool relax_loiter = motors_lower_limit_start_ms != 0 && (millis() - motors_lower_limit_start_ms) > 1000;
bool relax_loiter = landing_detect.lower_limit_start_ms != 0 &&
(millis() - landing_detect.lower_limit_start_ms) > 1000;
return relax_loiter;
}
// see if we are flying in vtol
bool QuadPlane::is_flying_vtol(void)
{
if (in_vtol_mode() && millis() - motors_lower_limit_start_ms > 5000) {
if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) {
return true;
}
return false;
@ -718,10 +712,11 @@ void QuadPlane::control_loiter()
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
get_desired_yaw_rate_cds());
get_desired_yaw_rate_cds(),
4.0f);
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll();
@ -880,6 +875,9 @@ void QuadPlane::update_transition(void)
plane.channel_throttle->get_control_in()>0 ||
plane.is_flying())) {
// the quad should provide some assistance to the plane
if (transition_state != TRANSITION_AIRSPEED_WAIT) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
}
transition_state = TRANSITION_AIRSPEED_WAIT;
transition_start_ms = millis();
assisted_flight = true;
@ -929,7 +927,8 @@ void QuadPlane::update_transition(void)
transition_state = TRANSITION_DONE;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done");
}
float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms;
float trans_time_ms = (float)transition_time_ms.get();
float throttle_scaled = last_throttle * (trans_time_ms - (millis() - transition_start_ms)) / trans_time_ms;
if (throttle_scaled < 0) {
throttle_scaled = 0;
}
@ -1187,7 +1186,7 @@ void QuadPlane::vtol_position_controller(void)
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds(),
smoothing_gain);
@ -1216,6 +1215,15 @@ void QuadPlane::vtol_position_controller(void)
// max_speed will control how fast we will fly. It will always decrease
poscontrol.max_speed = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01);
poscontrol.speed_scale = poscontrol.max_speed / MAX(distance, 1);
// start with low integrator. The alt_hold controller will
// add hover throttle to initial integrator. By starting
// without it we end up with a smoother startup when
// transitioning from fixed wing flight
float aspeed;
if (ahrs.airspeed_estimate(&aspeed) && aspeed > 6) {
pid_accel_z.set_integrator((-motors->get_throttle_hover())*1000.0f);
}
}
// run fixed wing navigation
@ -1274,7 +1282,7 @@ void QuadPlane::vtol_position_controller(void)
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds(),
smoothing_gain);
@ -1307,7 +1315,7 @@ void QuadPlane::vtol_position_controller(void)
plane.nav_pitch_cd = wp_nav->get_pitch();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds(),
smoothing_gain);
@ -1341,7 +1349,7 @@ void QuadPlane::vtol_position_controller(void)
break;
case QPOS_LAND_DESCEND: {
float height_above_ground = (plane.current_loc.alt - loc.alt)*0.01;
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
pos_control->set_alt_target_from_climb_rate(-landing_descent_rate_cms(height_above_ground),
plane.G_Dt, true);
break;
@ -1405,7 +1413,7 @@ void QuadPlane::takeoff_controller(void)
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds(),
smoothing_gain);
@ -1435,7 +1443,7 @@ void QuadPlane::waypoint_controller(void)
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
wp_nav->get_yaw(),
true);
true, 4.0f);
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
@ -1552,7 +1560,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
wp_nav->init_loiter_target();
throttle_wait = false;
motors_lower_limit_start_ms = 0;
landing_detect.lower_limit_start_ms = 0;
Location origin = inertial_nav.get_origin();
Vector2f diff2d;
Vector3f target;
@ -1583,17 +1591,49 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
return true;
}
/*
check if a landing is complete
*/
void QuadPlane::check_land_complete(void)
{
if (poscontrol.state == QPOS_LAND_FINAL &&
(motors_lower_limit_start_ms != 0 &&
millis() - motors_lower_limit_start_ms > 5000)) {
plane.disarm_motors();
poscontrol.state = QPOS_LAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
// reload target airspeed which could have been modified by the mission
plane.g.airspeed_cruise_cm.load();
if (poscontrol.state != QPOS_LAND_FINAL) {
// only apply to final landing phase
return;
}
uint32_t now = AP_HAL::millis();
bool might_be_landed = (landing_detect.lower_limit_start_ms != 0 &&
now - landing_detect.lower_limit_start_ms > 1000);
if (!might_be_landed) {
landing_detect.land_start_ms = 0;
return;
}
float height = inertial_nav.get_altitude()*0.01f;
if (landing_detect.land_start_ms == 0) {
landing_detect.land_start_ms = now;
landing_detect.vpos_start_m = height;
}
// we only consider the vehicle landed when the motors have been
// at minimum for 5s and the vertical position estimate has not
// changed by more than 20cm for 4s
if (fabsf(height - landing_detect.vpos_start_m > 0.2)) {
// height has changed, call off landing detection
landing_detect.land_start_ms = 0;
return;
}
if ((now - landing_detect.land_start_ms) < 4000 ||
(now - landing_detect.lower_limit_start_ms) < 5000) {
// not landed yet
return;
}
landing_detect.land_start_ms = 0;
// motors have been at zero for 5s, and we have had less than 0.3m
// change in altitude for last 4s. We are landed.
plane.disarm_motors();
poscontrol.state = QPOS_LAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
// reload target airspeed which could have been modified by the mission
plane.g.airspeed_cruise_cm.load();
}
/*
@ -1616,8 +1656,8 @@ bool QuadPlane::verify_vtol_land(void)
}
// at land_final_alt begin final landing
if (poscontrol.state == QPOS_LAND_DESCEND &&
plane.current_loc.alt < plane.next_WP_loc.alt+land_final_alt*100) {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (poscontrol.state == QPOS_LAND_DESCEND && height_above_ground < land_final_alt) {
poscontrol.state = QPOS_LAND_FINAL;
pos_control->set_alt_target(inertial_nav.get_altitude());
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started");
@ -1690,6 +1730,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
if (!plane.ahrs.get_velocity_NED(vel_ned)) {
// we don't know our velocity? EKF must be pretty sick
vel_forward.last_pct = 0;
vel_forward.integrator = 0;
return 0;
}
Vector3f vel_error_body = ahrs.get_rotation_body_to_ned().transposed() * ((desired_velocity_cms*0.01f) - vel_ned);
@ -1714,10 +1755,19 @@ int8_t QuadPlane::forward_throttle_pct(void)
// integrator as throttle percentage (-100 to 100)
vel_forward.integrator += fwd_vel_error * deltat * vel_forward.gain * 100;
// constrain to throttle range. This allows for reverse throttle if configured
vel_forward.integrator = constrain_float(vel_forward.integrator, MIN(0,plane.aparm.throttle_min), plane.aparm.throttle_max);
// inhibit reverse throttle and allow petrol engines with min > 0
int8_t fwd_throttle_min = (plane.aparm.throttle_min <= 0) ? 0 : plane.aparm.throttle_min;
vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_max);
vel_forward.last_pct = vel_forward.integrator;
// If we are below alt_cutoff then scale down the effect until it turns off at alt_cutoff and decay the integrator
float alt_cutoff = MAX(0,vel_forward_alt_cutoff);
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
height_above_ground, alt_cutoff, alt_cutoff+2);
if (vel_forward.last_pct == 0) {
// if the percent is 0 then decay the integrator
vel_forward.integrator *= 0.95f;
}
return vel_forward.last_pct;
}

13
ArduPlane/quadplane.h
View File

@ -6,6 +6,8 @@
#include <AP_InertialNav/AP_InertialNav.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_WPNav/AC_WPNav.h>
#include <AC_Fence/AC_Fence.h>
#include <AC_Avoidance/AC_Avoid.h>
/*
frame types for quadplane build. Most case be set with
@ -193,8 +195,6 @@ private:
// min and max PWM for throttle
AP_Int16 thr_min_pwm;
AP_Int16 thr_max_pwm;
AP_Int16 throttle_mid;
AP_Int16 throttle_min;
// speed below which quad assistance is given
AP_Float assist_speed;
@ -210,6 +210,7 @@ private:
// alt to switch to QLAND_FINAL
AP_Float land_final_alt;
AP_Float vel_forward_alt_cutoff;
AP_Int8 enable;
AP_Int8 transition_pitch_max;
@ -263,8 +264,12 @@ private:
// true when quad is assisting a fixed wing mode
bool assisted_flight;
// time when motors reached lower limit
uint32_t motors_lower_limit_start_ms;
struct {
// time when motors reached lower limit
uint32_t lower_limit_start_ms;
uint32_t land_start_ms;
float vpos_start_m;
} landing_detect;
// time we last set the loiter target
uint32_t last_loiter_ms;

39
ArduPlane/radio.cpp
View File

@ -55,24 +55,42 @@ void Plane::init_rc_in()
}
/*
initialise RC output channels
initialise RC output for main channels. This is done early to allow
for BRD_SAFETYENABLE=0 and early servo control
*/
void Plane::init_rc_out()
void Plane::init_rc_out_main()
{
channel_roll->enable_out();
channel_pitch->enable_out();
// setup failsafe for bottom 4 channels. We don't do all channels
// yet as some may be for VTOL motors in a quadplane
RC_Channel::setup_failsafe_trim_mask(0x000F);
/*
change throttle trim to minimum throttle. This prevents a
configuration error where the user sets CH3_TRIM incorrectly and
the motor may start on power up
*/
channel_throttle->set_radio_trim(throttle_min());
channel_roll->enable_out();
channel_pitch->enable_out();
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
}
channel_rudder->enable_out();
// setup PX4 to output the min throttle when safety off if arming
// is setup for min on disarm
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
}
}
/*
initialise RC output channels for aux channels
*/
void Plane::init_rc_out_aux()
{
update_aux();
RC_Channel_aux::enable_aux_servos();
@ -81,12 +99,6 @@ void Plane::init_rc_out()
// setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all();
// setup PX4 to output the min throttle when safety off if arming
// is setup for min on disarm
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
}
}
/*
@ -219,8 +231,11 @@ void Plane::read_radio()
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else {
} else if (stick_mixing_enabled()) {
rudder_input = channel_rudder->get_control_in();
} else {
// no stick mixing
rudder_input = 0;
}
// check for transmitter tuning changes

94
ArduPlane/release-notes.txt
View File

@ -1,3 +1,97 @@
Release 3.6.0, 6th June 2016
----------------------------
The ArduPilot development team is proud to announce the release of
version 3.6.0 of APM:Plane. This is a major update so please read the
notes carefully.
The biggest changes in this release are:
- major update to PX4Firmware code
- major update to QuadPlane code
- addition of MAVLink2 support
The updated PX4Firmware tree greatly improves support for the new
Pixracer boards as well as improving scheduling performance and UAVCAN
support.
The QuadPlane changes are very extensive in this release. A lot of new
features have been added, including:
- improved automatic weathervaning
- greatly improved support for mixed fixed wing and VTOL missions
- automatic RTL with VTOL land
- VTOL GUIDED mode support
- greatly improved transition code
- new tuning system for VTOL motors
- extensive upgrade to logging system for much better flight analysis
The new QuadPlane features are documented at:
http://ardupilot.org/plane/docs/quadplane-support.html
There is also a prototype implementation supporting tiltrotors and
tiltwings, but so far it has only been flown in simulations and it
should be considered very experimental.
Detailed changes include:
- added motortest for all quad motors in sequence
- merge upstream PX4Firmware changes
- new AC_AttitudeControl library from copter for quadplane
- modified default gains for quadplanes
- new velocity controller for initial quadplane landing
- smooth out final descent for VTOL landing
- changed default loop rate for quadplanes to 300Hz
- support up to 16 output channels (two via SBUS output only)
- fixed bug with landing flare for high values of LAND_FLARE_SEC
- improved crash detection logic
- added in-flight transmitter tuning
- fix handling of SET_HOME_POSITION
- added Q_VFWD_GAIN for forward motor in VTOL modes
- added Q_WVANE_GAIN for active weathervaning
- log the number of lost log messages
- Move position update to 50hz loop rather then the 10hz
- Suppress throttle when parachute release initiated, not after release.
- support Y6 frame class in quadplane
- log L1 xtrack error integrator and remove extra yaw logging
- limit roll before calculating load factor
- simplify landing flare logic
- smooth-out the end of takeoff pitch by reducing takeoff pitch min via TKOFF_PLIM_SEC
- added support for DO_VTOL_TRANSITION as a mission item
- fixed is_flying() for VTOL flight
- added Q_ENABLE=2 for starting AUTO in VTOL
- reload airspeed after VTOL landing
- lower default VTOL ANGLE_MAX to 30 degrees
- Change mode to RTL on end of mission rather then staying in auto
- implemented QRTL for quadplane RTL
- added Q_RTL_MODE parameter for QRTL after RTL approach
- reduced the rate of EKF and attitude logging to 25Hz
- added CHUTE_DELAY_MS parameter
- allow remapping of any input channel to any output channel
- numerous waf build improvements
- support fast timer capture for camera trigger feedback
- numerous improvements for Pixracer support
- added more general tiltrotor support to SITL
- only save learned compass offsets when disarmed
- support MISSION_ITEM_INT for more accurate waypoint positions
- change parachute deployment altitude to above ground not home
- added AP_Tuning system for QuadPlane tuning
- added initial support for tiltrotors and tiltwings
- added LOG_REPLAY and LOG_DISARMED parameters
- added Q_GUIDED_MODE parameter
- major update to QuadPlane documentation
- added MAVLink2 support
- fixed origin vs home altitude discrepancy
- improved Lidar based landing glide slope
- fixed throttle failsafe with THR_PASS_STAB=1
- prevent EKF blocking during baro and airspeed cal
- allow for ground testing of parachutes with CHUTE_MINALT=0
- fixed elevator stick mixing for above 50% input
- added QuadPlane ESC calibration
Release 3.6.0beta1, 30th April 2016
-----------------------------------

38
ArduPlane/system.cpp
View File

@ -99,6 +99,9 @@ void Plane::init_ardupilot()
}
#endif
set_control_channels();
init_rc_out_main();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet
@ -120,8 +123,6 @@ void Plane::init_ardupilot()
// allow servo set on all channels except first 4
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
@ -230,9 +231,9 @@ void Plane::init_ardupilot()
startup_ground();
// don't initialise rc output until after quadplane is setup as
// don't initialise aux rc output until after quadplane is setup as
// that can change initial values of channels
init_rc_out();
init_rc_out_aux();
// choose the nav controller
set_nav_controller();
@ -264,13 +265,6 @@ void Plane::startup_ground(void)
delay(GROUND_START_DELAY * 1000);
#endif
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
demo_servos(1);
}
//INS ground start
//------------------------
//
@ -289,12 +283,6 @@ void Plane::startup_ground(void)
// initialise mission library
mission.init();
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
demo_servos(3);
}
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = millis();
@ -348,6 +336,10 @@ void Plane::set_mode(enum FlightMode mode)
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.last_forced_rpy_ms.zero();
guided_state.last_forced_throttle_ms = 0;
// always reset this because we don't know who called set_mode. In evasion
// behavior you should set this flag after set_mode so you know the evasion
// logic is controlling the mode. This allows manual override of the mode
@ -385,6 +377,7 @@ void Plane::set_mode(enum FlightMode mode)
{
case INITIALISING:
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
case MANUAL:
@ -392,21 +385,25 @@ void Plane::set_mode(enum FlightMode mode)
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
auto_navigation_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
auto_navigation_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
auto_navigation_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
@ -414,17 +411,20 @@ void Plane::set_mode(enum FlightMode mode)
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
auto_navigation_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
auto_navigation_mode = true;
if (quadplane.available() && quadplane.enable == 2) {
auto_state.vtol_mode = true;
} else {
@ -437,17 +437,20 @@ void Plane::set_mode(enum FlightMode mode)
case RTL:
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
break;
case LOITER:
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
auto_navigation_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
@ -462,6 +465,7 @@ void Plane::set_mode(enum FlightMode mode)
case QLOITER:
case QLAND:
case QRTL:
auto_navigation_mode = false;
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {

28
ArduPlane/tuning.cpp
View File

@ -277,21 +277,45 @@ float AP_Tuning_Plane::controller_error(uint8_t parm)
if (!plane.quadplane.available()) {
return 0;
}
// in general a good tune will have rmsP significantly greater
// than rmsD. Otherwise it is too easy to push D too high while
// tuning a quadplane and end up with D dominating
const float max_P_D_ratio = 3.0f;
switch(parm) {
// special handling of dual-parameters
case TUNING_RATE_ROLL_PI:
case TUNING_RATE_ROLL_P:
case TUNING_RATE_ROLL_I:
case TUNING_RATE_ROLL_D:
return plane.quadplane.attitude_control->control_monitor_rms_output_roll();
case TUNING_RATE_ROLL_D: {
// special case for D term to keep it well below P
float rms_P = plane.quadplane.attitude_control->control_monitor_rms_output_roll_P();
float rms_D = plane.quadplane.attitude_control->control_monitor_rms_output_roll_D();
if (rms_P < rms_D * max_P_D_ratio) {
return max_P_D_ratio;
}
return rms_P+rms_D;
}
case TUNING_RATE_PITCH_PI:
case TUNING_RATE_PITCH_P:
case TUNING_RATE_PITCH_I:
case TUNING_RATE_PITCH_D:
return plane.quadplane.attitude_control->control_monitor_rms_output_pitch();
case TUNING_RATE_PITCH_D: {
// special case for D term to keep it well below P
float rms_P = plane.quadplane.attitude_control->control_monitor_rms_output_pitch_P();
float rms_D = plane.quadplane.attitude_control->control_monitor_rms_output_pitch_D();
if (rms_P < rms_D * max_P_D_ratio) {
return max_P_D_ratio;
}
return rms_P+rms_D;
}
case TUNING_RATE_YAW_PI:
case TUNING_RATE_YAW_P:
case TUNING_RATE_YAW_I:

4
ArduPlane/version.h
View File

@ -2,8 +2,8 @@
#include "ap_version.h"
#define THISFIRMWARE "ArduPlane V3.6.0beta1"
#define FIRMWARE_VERSION 3,6,0,FIRMWARE_VERSION_TYPE_BETA
#define THISFIRMWARE "ArduPlane V3.6.0"
#define FIRMWARE_VERSION 3,6,0,FIRMWARE_VERSION_TYPE_OFFICIAL
#ifndef GIT_VERSION
#define FIRMWARE_STRING THISFIRMWARE

11
ArduPlane/wscript
View File

@ -26,7 +26,9 @@ def build(bld):
'AC_WPNav',
'AC_AttitudeControl',
'AP_Motors',
'AC_PID'
'AC_PID',
'AC_Fence',
'AC_Avoidance'
],
use='mavlink',
)
@ -36,3 +38,10 @@ def build(bld):
program_groups=['bin', 'plane'],
use=vehicle + '_libs',
)
bld.ap_program(
program_name='arduplane-tri',
program_groups=['bin', 'plane'],
use=vehicle + '_libs',
defines=['FRAME_CONFIG=TRI_FRAME'],
)

23
README-WAF.md → BUILD.md
View File

@ -63,8 +63,10 @@ waf list
## Program groups ##
Program groups are used to represent a class of programs. They can be used to
build all programs of a certain class without having to specify each program. A
program can belong to more than one group.
build all programs of a certain class without having to specify each program.
It's possible for two groups to overlap, except when both groups are main
groups. In other words, a program can belong to more than one group, but only
to one main group.
There's a special group, called "all", that comprises all programs.
@ -134,6 +136,9 @@ waf bin
# Build all examples
waf examples
# Build arducopter binaries
waf copter
```
## Building a specific program ##
@ -149,6 +154,20 @@ waf --targets bin/arducopter-quad
waf --targets tests/test_vectors
```
## Uploading ##
There's a build option `--upload` that can be used to tell the build that it
must upload the program(s) addressed by `--targets` arguments. The
implementation is board-specific and not all boards may have that implemented.
Example:
```bash
# PX4 supports uploading the program through a USB connection
waf configure --board px4-v2
# Build arducopter-quad and upload it to my board
waf --targets bin/arducopter-quad --upload
```
## Checking ##
The command `check` builds all programs and then run the relevant tests. In

2
Tools/Replay/Replay.cpp
View File

@ -399,9 +399,7 @@ bool Replay::find_log_info(struct log_information &info)
if (streq(type, "PARM") && streq(reader.last_parm_name, "SCHED_LOOP_RATE")) {
// get rate directly from parameters
info.update_rate = reader.last_parm_value;
return true;
}
if (strlen(clock_source) == 0) {
// If you want to add a clock source, also add it to
// handle_msg and handle_log_format_msg, above. Note that

17
Tools/Replay/wscript
View File

@ -7,7 +7,18 @@ def build(bld):
if not isinstance(bld.get_board(), boards.linux):
return
bld.ap_program(
use='ap',
program_groups='tools',
vehicle = bld.path.name
bld.ap_stlib(
name=vehicle + '_libs',
vehicle=vehicle,
libraries=bld.ap_common_vehicle_libraries() + [
'AP_InertialNav',
],
use='mavlink',
)
bld.ap_program(
program_groups='tools',
use=vehicle + '_libs',
)

4
Tools/ardupilotwaf/boards.py
View File

@ -232,6 +232,10 @@ class linux(Board):
'AP_HAL_Linux',
]
# We always want to use PRI format macros
cfg.define('__STDC_FORMAT_MACROS', 1)
class minlure(linux):
def configure_env(self, cfg, env):
super(minlure, self).configure_env(cfg, env)

15
Tools/ardupilotwaf/cmake.py
View File

@ -173,6 +173,9 @@ cmake_configure_task.run = _cmake_configure_task_run
class cmake_build_task(Task.Task):
run_str = '${CMAKE} --build ${CMAKE_BLD_DIR} --target ${CMAKE_TARGET}'
color = 'BLUE'
# the cmake-generated build system is responsible of managing its own
# dependencies
always_run = True
def exec_command(self, cmd, **kw):
kw['stdout'] = sys.stdout
@ -211,10 +214,6 @@ def _cmake_build_task_post_run(self):
return self.original_post_run()
cmake_build_task.post_run = _cmake_build_task_post_run
# the cmake-generated build system is responsible of managing its own
# dependencies
cmake_build_task = Task.always_run(cmake_build_task)
class CMakeConfig(object):
'''
CMake configuration. This object shouldn't be instantiated directly. Use
@ -253,9 +252,8 @@ class CMakeConfig(object):
if self._config_task and self._config_task.cmake_config_sig == sig:
return self._config_task
# NOTE: we'll probably need to use the full class name in waf 1.9
self._config_task = taskgen.create_task('cmake_configure')
self._config_task.cwd = self.bldnode.abspath()
self._config_task = taskgen.create_task('cmake_configure_task')
self._config_task.cwd = self.bldnode
self._config_task.cmake = self
self._config_task.cmake_config_sig = sig
@ -349,8 +347,7 @@ def cmake_build(bld, cmake_config, cmake_target, **kw):
def create_cmake_build_task(self, cmake_config, cmake_target):
cmake = get_cmake(cmake_config)
# NOTE: we'll probably need to use the full class name in waf 1.9
tsk = self.create_task('cmake_build')
tsk = self.create_task('cmake_build_task')
tsk.cmake = cmake
tsk.cmake_target = cmake_target
tsk.output_patterns = []

8
Tools/ardupilotwaf/cxx_checks.py
View File

@ -151,6 +151,10 @@ def check_librt(cfg, env):
@conf
def check_lttng(cfg, env):
if cfg.options.disable_lttng:
cfg.msg("Checking for 'lttng-ust':", 'disabled', color='YELLOW')
return False
cfg.check_cfg(package='lttng-ust', mandatory=False, global_define=True,
args=['--libs', '--cflags'])
env.LIB += cfg.env['LIB_LTTNG-UST']
@ -158,6 +162,10 @@ def check_lttng(cfg, env):
@conf
def check_libiio(cfg, env):
if cfg.options.disable_libiio:
cfg.msg("Checking for 'libiio':", 'disabled', color='YELLOW')
return False
cfg.check_cfg(package='libiio', mandatory=False, global_define=True,
args=['--libs', '--cflags'])
env.LIB += cfg.env['LIB_LIBIIO']

4
Tools/autotest/apmrover2.py
View File

@ -79,7 +79,7 @@ def drive_mission(mavproxy, mav, filename):
return True
def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False):
def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''drive APMrover2 in SIL
you can pass viewerip as an IP address to optionally send fg and
@ -111,7 +111,7 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False):
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model='rover', home=home, speedup=10, valgrind=valgrind)
sil = util.start_SIL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)

27
Tools/autotest/arducopter.py
View File

@ -23,7 +23,7 @@ homeloc = None
num_wp = 0
speedup_default = 5
def hover(mavproxy, mav, hover_throttle=1450):
def hover(mavproxy, mav, hover_throttle=1500):
mavproxy.send('rc 3 %u\n' % hover_throttle)
return True
@ -158,7 +158,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
save_wp(mavproxy, mav)
# switch back to stabilize mode
mavproxy.send('rc 3 1430\n')
mavproxy.send('rc 3 1500\n')
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
@ -254,6 +254,9 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
return False
mavproxy.send('rc 4 1500\n')
# raise throttle slightly to avoid hitting the ground
mavproxy.send('rc 3 1600\n')
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
@ -393,8 +396,9 @@ def fly_fence_test(mavproxy, mav, timeout=180):
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# enable fence
# enable fence, disable avoidance
mavproxy.send('param set FENCE_ENABLE 1\n')
mavproxy.send('param set AVOID_ENABLE 0\n')
# first east
print("turn east")
@ -432,8 +436,11 @@ def fly_fence_test(mavproxy, mav, timeout=180):
wait_mode(mav, 'STABILIZE')
print("Reached home OK")
return True
# disable fence
# disable fence, enable avoidance
mavproxy.send('param set FENCE_ENABLE 0\n')
mavproxy.send('param set AVOID_ENABLE 1\n')
# reduce throttle
mavproxy.send('rc 3 1000\n')
# switch mode to stabilize
@ -647,7 +654,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1430\n')
mavproxy.send('rc 3 1500\n')
# fly south 50m
print("# Flying south %u meters" % side)
@ -712,7 +719,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
# switch to stabilize mode
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1430\n')
mavproxy.send('rc 3 1500\n')
# start copter yawing slowly
mavproxy.send('rc 4 1550\n')
@ -907,7 +914,7 @@ def setup_rc(mavproxy):
# zero throttle
mavproxy.send('rc 3 1000\n')
def fly_ArduCopter(binary, viewerip=None, map=False, valgrind=False):
def fly_ArduCopter(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL
you can pass viewerip as an IP address to optionally send fg and
@ -931,7 +938,7 @@ def fly_ArduCopter(binary, viewerip=None, map=False, valgrind=False):
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind)
sil = util.start_SIL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
@ -1258,7 +1265,7 @@ def fly_ArduCopter(binary, viewerip=None, map=False, valgrind=False):
return True
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False):
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
global homeloc
@ -1279,7 +1286,7 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False):
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind)
sil = util.start_SIL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip

4
Tools/autotest/arduplane.py
View File

@ -426,7 +426,7 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non
return True
def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False):
def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly ArduPlane in SIL
you can pass viewerip as an IP address to optionally send fg and
@ -461,7 +461,7 @@ def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False):
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model='jsbsim', home=HOME_LOCATION, speedup=10, valgrind=valgrind)
sil = util.start_SIL(binary, model='jsbsim', home=HOME_LOCATION, speedup=10, valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)

17
Tools/autotest/autotest.py
View File

@ -144,6 +144,7 @@ parser.add_option("--map", action='store_true', default=False, help='show map')
parser.add_option("--experimental", default=False, action='store_true', help='enable experimental tests')
parser.add_option("--timeout", default=3000, type='int', help='maximum runtime in seconds')
parser.add_option("--valgrind", default=False, action='store_true', help='run ArduPilot binaries under valgrind')
parser.add_option("--gdb", default=False, action='store_true', help='run ArduPilot binaries under gdb')
parser.add_option("--debug", default=False, action='store_true', help='make built binaries debug binaries')
parser.add_option("-j", default=None, type='int', help='build CPUs')
@ -265,19 +266,19 @@ def run_step(step):
return get_default_params('APMrover2', binary)
if step == 'fly.ArduCopter':
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind)
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.CopterAVC':
return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind)
return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.ArduPlane':
return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind)
return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.QuadPlane':
return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind)
return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'drive.APMrover2':
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind)
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'build.All':
return build_all()
@ -482,9 +483,15 @@ if len(args) > 0:
# allow a wildcard list of steps
matched = []
for a in args:
arg_matched = False
for s in steps:
if fnmatch.fnmatch(s.lower(), a.lower()):
matched.append(s)
arg_matched = True
if not arg_matched:
print("No steps matched argument ({})".format(a))
sys.exit(1)
steps = matched
try:

1
Tools/autotest/copter_params.parm
View File

@ -73,6 +73,7 @@ INS_ACC3SCAL_X 1.000
INS_ACC3SCAL_Y 1.000
INS_ACC3SCAL_Z 1.000
MOT_THST_EXPO 0.5
MOT_THST_HOVER 0.36
# flightmodes
# switch 1 Circle
# switch 2 LAND

1
Tools/autotest/locations.txt
View File

@ -23,3 +23,4 @@ KSFO=37.619373,-122.376637,5.3,118
NFSC=53.637561,9.929800,13,345
Rotherham=53.275131,-1.19404,21,360
LeeField=32.42553,-84.79109,75,260
Sterling=38.982480,-77.439567,88,90

10
Tools/autotest/pysim/util.py
View File

@ -190,7 +190,15 @@ class SIL(pexpect.spawn):
pexpect_autoclose(self)
# give time for parameters to properly setup
time.sleep(3)
self.expect('Waiting for connection',timeout=300)
if gdb:
# if we run GDB we do so in an xterm. "Waiting for
# connection" is never going to appear on xterm's output.
#... so let's give it another magic second.
time.sleep(1)
# TODO: have a SITL-compiled ardupilot able to have its
# console on an output fd.
else:
self.expect('Waiting for connection',timeout=300)
def valgrind_log_filepath(self):

84
Tools/autotest/quadplane-tilttri.parm Normal file
View File

@ -0,0 +1,84 @@
AHRS_EKF_TYPE 2
ARMING_RUDDER 2
ARSPD_ENABLE 1
ARSPD_FBW_MAX 35
ARSPD_FBW_MIN 6
ARSPD_USE 1
AUTOTUNE_LEVEL 8
COMPASS_OFS2_X -0.420265
COMPASS_OFS2_Y -0.726942
COMPASS_OFS2_Z 6.665476
COMPASS_OFS3_X 0
COMPASS_OFS3_Y 0
COMPASS_OFS3_Z 0
COMPASS_OFS_X -0.453570
COMPASS_OFS_Y -0.585846
COMPASS_OFS_Z 6.815743
EK2_ENABLE 1
EK2_IMU_MASK 3
EKF_ENABLE 1
FBWB_CLIMB_RATE 10
FLTMODE1 10
FLTMODE2 11
FLTMODE3 12
FLTMODE4 5
FLTMODE5 19
FLTMODE6 5
FLTMODE_CH 5
INS_ACC2OFFS_X 0.001000
INS_ACC2OFFS_Y 0.001000
INS_ACC2OFFS_Z 0.001000
INS_ACC2SCAL_X 1.001000
INS_ACC2SCAL_Y 1.001000
INS_ACC2SCAL_Z 1.001000
INS_ACCOFFS_X 0.001000
INS_ACCOFFS_Y 0.001000
INS_ACCOFFS_Z 0.001000
INS_ACCSCAL_X 1.001000
INS_ACCSCAL_Y 1.001000
INS_ACCSCAL_Z 1.001000
INS_GYR_CAL 0
KFF_RDDRMIX 0.500000
LIM_PITCH_MAX 3000
LIM_PITCH_MIN -3000
LIM_ROLL_CD 6500
NAVL1_PERIOD 14
PTCH2SRV_D 0.309954
PTCH2SRV_I 0.425000
PTCH2SRV_IMAX 4000
PTCH2SRV_P 3.646518
PTCH2SRV_RLL 1
Q_ANGLE_MAX 4500
Q_ASSIST_SPEED 6
Q_ENABLE 1
RALLY_INCL_HOME 0
RALLY_LIMIT_KM 5
RALLY_TOTAL 0
RC1_DZ 30
RC1_MAX 1886
RC1_MIN 1087
RC1_REV 1
RC1_TRIM 1500
RC2_REV 1
RC3_MAX 2000
RC3_MIN 1000
RC4_REV 1
RLL2SRV_D 0.141009
RLL2SRV_I 0.425000
RLL2SRV_IMAX 4000
RLL2SRV_P 1.600000
SCHED_LOOP_RATE 300
THR_MAX 100
TRIM_ARSPD_CM 2500
LOG_BITMASK 65534
Q_TILT_MASK 3
Q_VFWD_GAIN 0.1
RC12_FUNCTION 41
RC12_MIN 1000
RC12_MAX 2000
RTL_RADIUS 80
Q_RTL_MODE 1
ALT_HOLD_RTL 2000

4
Tools/autotest/quadplane.py
View File

@ -40,7 +40,7 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
return True
def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False):
def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly QuadPlane in SIL
you can pass viewerip as an IP address to optionally send fg and
@ -55,7 +55,7 @@ def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False):
options += ' --map'
sil = util.start_SIL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
defaults_file=os.path.join(testdir, 'quadplane.parm'), valgrind=valgrind)
defaults_file=os.path.join(testdir, 'quadplane.parm'), valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('QuadPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)

101
Tools/autotest/sim_vehicle.py
View File

@ -88,37 +88,59 @@ def under_cygwin():
return os.path.exists("/usr/bin/cygstart")
def kill_tasks_cygwin(victims):
'''shell out to ps -ea to find processes to kill'''
for victim in list(victims):
pids = cygwin_pidof(victim)
# progress("pids for (%s): %s" % (victim,",".join([ str(p) for p in pids])))
for apid in pids:
os.kill(apid, signal.SIGKILL)
def kill_tasks():
'''clean up stray processes by name. This is a somewhat shotgun approach'''
victim_names = set([
'JSBSim',
'lt-JSBSim',
'ArduPlane.elf',
'ArduCopter.elf',
'APMrover2.elf',
'AntennaTracker.elf',
'JSBSIm.exe',
'MAVProxy.exe',
'runsim.py',
'AntennaTracker.elf',
])
if under_cygwin():
return kill_tasks_cygwin(list(victim_names))
def kill_tasks_psutil(victims):
'''use the psutil module to kill tasks by name. Sadly, this module is not available on Windows, but when it is we should be able to *just* use this routine'''
import psutil
for proc in psutil.process_iter():
if proc.status == psutil.STATUS_ZOMBIE:
continue
if proc.name in victim_names:
if proc.name in victims:
proc.kill()
def kill_tasks_pkill(victims):
'''shell out to pkill(1) to kill processed by name'''
for victim in victims: # pkill takes a single pattern, so iterate
cmd = ["pkill"]
cmd.append(victim)
run_cmd_blocking("pkill", cmd, quiet=True)
class BobException(Exception):
pass
def kill_tasks():
'''clean up stray processes by name. This is a somewhat shotgun approach'''
progress("Killing tasks")
try:
victim_names = [
'JSBSim',
'lt-JSBSim',
'ArduPlane.elf',
'ArduCopter.elf',
'APMrover2.elf',
'AntennaTracker.elf',
'JSBSIm.exe',
'MAVProxy.exe',
'runsim.py',
'AntennaTracker.elf',
]
if under_cygwin():
return kill_tasks_cygwin(victim_names)
try:
kill_tasks_psutil(victim_names)
except ImportError as e:
kill_tasks_pkill(victim_names)
except Exception as e:
progress("kill_tasks failed: {}".format(str(e)))
# clean up processes at exit:
atexit.register(kill_tasks)
@ -202,6 +224,7 @@ group_sim.add_option("-t", "--tracker-location", default='CMAC_PILOTSBOX', type=
group_sim.add_option("-w", "--wipe-eeprom", action='store_true', default=False, help='wipe EEPROM and reload parameters')
group_sim.add_option("-m", "--mavproxy-args", default=None, type='string', help='additional arguments to pass to mavproxy.py')
group_sim.add_option("", "--strace", action='store_true', default=False, help="strace the ArduPilot binary")
group_sim.add_option("", "--model", type='string', default=None, help='Override simulation model to use')
parser.add_option_group(group_sim)
@ -254,6 +277,18 @@ default_frame_for_vehicle = {
"AntennaTracker": "tracker"
}
if not default_frame_for_vehicle.has_key(opts.vehicle):
# try in parent directories, useful for having config in subdirectories
cwd = os.getcwd()
while cwd:
bname = os.path.basename(cwd)
if not bname:
break
if bname in default_frame_for_vehicle:
opts.vehicle = bname
break
cwd = os.path.dirname(cwd)
# try to validate vehicle
if not default_frame_for_vehicle.has_key(opts.vehicle):
progress("** Is (%s) really your vehicle type? Try -v VEHICLETYPE if not, or be in the e.g. ArduCopter subdirectory" % (opts.vehicle,))
@ -347,9 +382,15 @@ _options_for_frame = {
},
# PLANE
"quadplane-tilttri" : {
"build_target" : "sitl-tri",
"make_target" : "sitl-tri",
"waf_target" : "bin/arduplane-tri",
"default_params_filename": "quadplane-tilttri.parm",
},
"quadplane-tri" : {
"make_target" : "sitl-tri",
"waf_target" : "bin/arduplane-tri",
"default_params_filename": "quadplane-tri.parm",
},
"quadplane": {
"waf_target": "bin/arduplane",
"default_params_filename": "quadplane.parm",
@ -422,6 +463,16 @@ def options_for_frame(frame, vehicle, opts):
if not ret.has_key("model"):
ret["model"] = frame
if not ret.has_key("sitl-port"):
ret["sitl-port"] = True
if opts.model is not None:
ret["model"] = opts.model
if (ret["model"].find("xplane") != -1 or
ret["model"].find("flightaxis") != -1):
ret["sitl-port"] = False
if not ret.has_key("make_target"):
ret["make_target"] = "sitl"
@ -524,8 +575,9 @@ def progress_cmd(what, cmd):
shell_text = "%s" % (" ".join([ '"%s"' % x for x in cmd ]))
progress(shell_text)
def run_cmd_blocking(what, cmd, **kw):
progress_cmd(what, cmd)
def run_cmd_blocking(what, cmd, quiet=False, **kw):
if not quiet:
progress_cmd(what, cmd)
p = subprocess.Popen(cmd, **kw)
return os.waitpid(p.pid,0)
@ -612,7 +664,10 @@ def start_mavproxy(opts, stuff):
if opts.hil:
cmd.extend(["--load-module", "HIL"])
else:
cmd.extend(["--master", mavlink_port, "--sitl", simout_port])
cmd.extend(["--master", mavlink_port])
if stuff["sitl-port"]:
cmd.extend(["--sitl", simout_port])
# If running inside of a vagrant guest, then we probably want to forward our mavlink out to the containing host OS
if getpass.getuser() == "vagrant":
cmd.extend(["--out", "10.0.2.2:14550"])

4
Tools/autotest/sim_vehicle.sh
View File

@ -354,6 +354,10 @@ case $FRAME in
MODEL="$FRAME"
SITLRCIN=0
;;
xplane*)
MODEL="$FRAME"
SITLRCIN=0
;;
*-heli)
BUILD_TARGET="sitl-heli"
MODEL="$FRAME"

2
Tools/scripts/install-prereqs-ubuntu.sh
View File

@ -6,7 +6,7 @@ BASE_PKGS="gawk make git arduino-core curl"
PYTHON_PKGS="pymavlink MAVProxy droneapi catkin_pkg"
PX4_PKGS="python-serial python-argparse openocd flex bison libncurses5-dev \
autoconf texinfo build-essential libftdi-dev libtool zlib1g-dev \
zip genromfs python-empy"
zip genromfs python-empy libc6-i386"
BEBOP_PKGS="g++-arm-linux-gnueabihf"
SITL_PKGS="g++ python-pip python-setuptools python-matplotlib python-serial python-scipy python-opencv python-numpy python-pyparsing ccache realpath"
ASSUME_YES=false

2
Tools/vagrant/initvagrant.sh
View File

@ -53,7 +53,7 @@ ln -fs /vagrant/Tools/vagrant/screenrc /home/vagrant/.screenrc
# build JSB sim
pushd /tmp
rm -rf jsbsim
git clone git://github.com/tridge/jsbsim.git
git clone https://github.com/tridge/jsbsim.git
sudo apt-get install -y libtool automake autoconf libexpat1-dev
cd jsbsim
./autogen.sh

778
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -87,320 +87,440 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// @User: Standard
AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
// @Param: ANG_LIM_TC
// @DisplayName: Angle Limit (to maintain altitude) Time Constant
// @Description: Angle Limit (to maintain altitude) Time Constant
// @Range: 0.5 10.0
// @User: Advanced
AP_GROUPINFO("ANG_LIM_TC", 16, AC_AttitudeControl, _angle_limit_tc, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT),
AP_GROUPEND
};
void AC_AttitudeControl::relax_bf_rate_controller()
// Set output throttle and disable stabilization
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (reset_attitude_control) {
relax_attitude_controllers();
}
_motors.set_throttle(throttle_in);
_angle_boost = 0.0f;
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// TODO add _ahrs.get_quaternion()
_attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
_attitude_target_ang_vel = _ahrs.get_gyro();
_attitude_target_euler_angle = Vector3f(_ahrs.roll, _ahrs.pitch, _ahrs.yaw);
// Set reference angular velocity used in angular velocity controller equal
// to the input angular velocity and reset the angular velocity integrators.
// This zeros the output of the angular velocity controller.
_ang_vel_target_rads = _ahrs.get_gyro();
_rate_target_ang_vel = _ahrs.get_gyro();
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
// Write euler derivatives derived from vehicle angular velocity to
// _att_target_euler_rate_rads. This resets the state of the input shapers.
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
// The attitude controller works around the concept of the desired attitude, target attitude
// and measured attitude. The desired attitude is the attitude input into the attitude controller
// that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
// to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed
// directly into the rate controllers. The angular error between the measured attitude and the target attitude is
// fed into the angle controller and the output of the angle controller summed at the input of the rate controllers.
// By feeding the target angular velocity directly into the rate controllers the measured and target attitudes
// remain very close together.
//
// All input functions below follow the same procedure
// 1. define the desired attitude the aircraft should attempt to achieve using the input variables
// 2. using the desired attitude and input variables, define the target angular velocity so that it should
// move the target attitude towards the desired attitude
// 3. if _rate_bf_ff_enabled & _use_ff_and_input_shaping are not being used then make the target attitude
// and target angular velocities equal to the desired attitude and desired angular velocities.
// 4. ensure _attitude_target_quat, _attitude_target_euler_angle, _attitude_target_euler_rate and
// _attitude_target_ang_vel have been defined. This ensures input modes can be changed without discontinuity.
// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
{
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + radians(yaw_shift_cd*0.01f));
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_ang_vel.x = input_shaping_angle(attitude_error_angle.x, smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x);
_attitude_target_ang_vel.y = input_shaping_angle(attitude_error_angle.y, smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y);
_attitude_target_ang_vel.z = input_shaping_angle(attitude_error_angle.z, smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_attitude_target_quat = attitude_desired_quat;
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// Sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
euler_roll_angle += get_roll_trim_rad();
if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis
// angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss());
_attitude_target_euler_rate.x = input_shaping_angle(euler_roll_angle-_attitude_target_euler_angle.x, smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x);
_attitude_target_euler_rate.y = input_shaping_angle(euler_pitch_angle-_attitude_target_euler_angle.y, smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y);
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
// When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the
// input value and the feedforward rate is zeroed.
_att_target_euler_rad.x = euler_roll_angle_rad;
_att_target_euler_rate_rads.x = 0;
}
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) {
// When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis
// angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss());
// Acceleration is limited directly to smooth the beginning of the curve.
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
} else {
_att_target_euler_rad.y = euler_pitch_angle_rad;
_att_target_euler_rate_rads.y = 0;
}
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
_attitude_target_euler_angle.z += euler_yaw_rate*_dt;
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
if (_rate_bf_ff_enabled) {
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
} else {
euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads);
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd*0.01f);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
euler_roll_angle += get_roll_trim_rad();
// Set roll/pitch attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// Zero the roll and pitch feed-forward rate.
_att_target_euler_rate_rads.x = 0;
_att_target_euler_rate_rads.y = 0;
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(euler_roll_angle-_attitude_target_euler_angle.x, smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x);
_attitude_target_euler_rate.y = input_shaping_angle(euler_pitch_angle-_attitude_target_euler_angle.y, smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y);
_attitude_target_euler_rate.z = input_shaping_angle(euler_yaw_angle-_attitude_target_euler_angle.z, smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
if (get_accel_yaw_max_radss() > 0.0f) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
// The output rate is used to update the attitude target euler angles and is fed forward into the rate controller.
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
// When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate
// is fed forward into the rate controller.
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), -get_slew_yaw_rads()*_dt, get_slew_yaw_rads()*_dt);
// Update attitude target from constrained angle error
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
_attitude_target_euler_angle.z = euler_yaw_angle;
}
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
}
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle_rad += get_roll_trim_rad();
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = euler_yaw_angle_rad;
// If slew_yaw is enabled, constrain yaw target within get_slew_yaw_rads() of _ahrs.yaw
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -get_slew_yaw_rads(), get_slew_yaw_rads());
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
}
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, Vector3f(0.0f,0.0f,0.0f));
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate_rads = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate_rads = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f);
float euler_roll_rate = radians(euler_roll_rate_cds*0.01f);
float euler_pitch_rate = radians(euler_pitch_rate_cds*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
// Compute acceleration-limited euler roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_euler_rate_rads.x += constrain_float(euler_roll_rate_rads - _att_target_euler_rate_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.x = input_shaping_ang_vel(_attitude_target_euler_rate.x, euler_roll_rate, euler_accel.x);
_attitude_target_euler_rate.y = input_shaping_ang_vel(_attitude_target_euler_rate.y, euler_pitch_rate, euler_accel.y);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {
_att_target_euler_rate_rads.x = euler_roll_rate_rads;
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + euler_roll_rate*_dt);
_attitude_target_euler_angle.y = constrain_float(_attitude_target_euler_angle.y + euler_pitch_rate*_dt, radians(-85.0f), radians(85.0f));
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate*_dt);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
}
// Compute acceleration-limited euler pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_euler_rate_rads.y += constrain_float(euler_pitch_rate_rads - _att_target_euler_rate_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.y = euler_pitch_rate_rads;
}
// Compute acceleration-limited euler yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_euler_rate_rads.z = euler_yaw_rate_rads;
}
// Update the attitude target from the computed euler rates
update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD);
update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD);
// Apply tilt limit
_att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad());
// Convert euler angle derivatives of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads);
// Call attitude controller
attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
float roll_rate_rads = radians(roll_rate_bf_cds*0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds*0.01f);
// Compute acceleration-limited body-frame roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
_att_target_ang_vel_rads.x += constrain_float(roll_rate_bf_rads - _att_target_ang_vel_rads.x, -rate_change_limit_rads, rate_change_limit_rads);
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// Compute acceleration-limited euler rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss());
_attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss());
_attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss());
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_att_target_ang_vel_rads.x = roll_rate_bf_rads;
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Compute acceleration-limited body-frame pitch rate
if (get_accel_pitch_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt;
_att_target_ang_vel_rads.y += constrain_float(pitch_rate_bf_rads - _att_target_ang_vel_rads.y, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.y = pitch_rate_bf_rads;
}
// Compute acceleration-limited body-frame yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
_att_target_ang_vel_rads.z += constrain_float(yaw_rate_bf_rads - _att_target_ang_vel_rads.z, -rate_change_limit_rads, rate_change_limit_rads);
} else {
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
}
// Compute quaternion target attitude
Quaternion att_target_quat;
att_target_quat.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
// Rotate quaternion target attitude using computed rate
att_target_quat.rotate(_att_target_ang_vel_rads*_dt);
att_target_quat.normalize();
// Call attitude controller
attitude_controller_run_quat(att_target_quat, _att_target_ang_vel_rads);
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
{
// Call attitude controller
attitude_controller_run_quat(att_target_quat, att_target_ang_vel_rads);
// Keep euler derivative updated
ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _ang_vel_target_rads, _att_target_euler_rate_rads);
}
void AC_AttitudeControl::attitude_controller_run_euler(const Vector3f& att_target_euler_rad, const Vector3f& att_target_ang_vel_rads)
{
// Compute quaternion target attitude
Quaternion att_target_quat;
att_target_quat.from_euler(att_target_euler_rad.x, att_target_euler_rad.y, att_target_euler_rad.z);
// Call quaternion attitude controller
attitude_controller_run_quat(att_target_quat, att_target_ang_vel_rads);
attitude_controller_run_quat();
}
void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads)
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
// Update euler attitude target and angular velocity target
att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
_att_target_ang_vel_rads = att_target_ang_vel_rads;
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion att_vehicle_quat;
att_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Compute attitude error
(att_vehicle_quat.inverse()*att_target_quat).to_axis_angle(_att_error_rot_vec_rad);
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
// Compute the angular velocity target from the attitude error
update_ang_vel_target_from_att_error();
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;
// Add the angular velocity feedforward, rotated into vehicle frame
Matrix3f Trv;
get_rotation_reference_to_vehicle(Trv);
_ang_vel_target_rads += Trv * _att_target_ang_vel_rads;
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;
// Correct the thrust vector and smoothly add feedforward and yaw input
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;
_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;
} else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
}
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// rotate target and normalize
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
}
}
void AC_AttitudeControl::rate_controller_run()
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
control_monitor_update();
Matrix3f att_to_rot_matrix; // earth frame to target frame
att_to_quat.rotation_matrix(att_to_rot_matrix);
Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
Matrix3f att_from_rot_matrix; // earth frame to target frame
att_from_quat.rotation_matrix(att_from_rot_matrix);
Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
// the dot product is used to calculate the angle between the target and desired thrust vectors
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross.length();
if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
thrust_vec_cross = Vector3f(0,0,1);
thrust_vec_dot = 0.0f;
}else{
thrust_vec_cross /= thrust_vector_length;
}
Quaternion thrust_vec_correction_quat;
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
// calculate the remaining rotation required after thrust vector is rotated
Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;
Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
heading_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z;
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat;
}
}
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel)
{
error_angle = wrap_PI(error_angle);
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
float ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max);
// Acceleration is limited directly to smooth the beginning of the curve.
float delta_ang_vel = accel_max * _dt;
return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
}
// limits the acceleration and deceleration of a velocity request
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max)
{
if (accel_max > 0.0f) {
float delta_ang_vel = accel_max * _dt;
target_ang_vel += constrain_float(desired_ang_vel - target_ang_vel, -delta_ang_vel, delta_ang_vel);
} else {
target_ang_vel = desired_ang_vel;
}
return target_ang_vel;
}
// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
Vector3f rot_accel;
if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) {
rot_accel.x = euler_accel.x;
rot_accel.y = euler_accel.y;
rot_accel.z = euler_accel.z;
} else {
rot_accel.x = euler_accel.x;
rot_accel.y = MIN(euler_accel.y/cos_phi, euler_accel.z/sin_phi);
rot_accel.z = MIN(MIN(euler_accel.x/sin_theta, euler_accel.y/sin_phi), euler_accel.z/cos_phi);
}
return rot_accel;
}
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
float yaw_shift = radians(yaw_shift_cd*0.01f);
Quaternion _attitude_target_update_quat;
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{
float sin_theta = sinf(euler_rad.y);
@ -413,6 +533,8 @@ void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const
ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
}
// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads)
{
float sin_theta = sinf(euler_rad.y);
@ -431,87 +553,35 @@ bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const
return true;
}
void AC_AttitudeControl::update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.x - _ahrs.roll), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.x = angle_error + _ahrs.roll;
// Increment the attitude target
_att_target_euler_rad.x += euler_roll_rate_rads * _dt;
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x);
}
void AC_AttitudeControl::update_att_target_pitch(float euler_pitch_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.y - _ahrs.pitch), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.y = angle_error + _ahrs.pitch;
// Increment the attitude target
_att_target_euler_rad.y += euler_pitch_rate_rads * _dt;
_att_target_euler_rad.y = wrap_PI(_att_target_euler_rad.y);
}
void AC_AttitudeControl::update_att_target_yaw(float euler_yaw_rate_rads, float overshoot_max_rad)
{
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(_att_target_euler_rad.z - _ahrs.yaw), -overshoot_max_rad, overshoot_max_rad);
// Update attitude target from constrained angle error
_att_target_euler_rad.z = angle_error + _ahrs.yaw;
// Increment the attitude target
_att_target_euler_rad.z += euler_yaw_rate_rads * _dt;
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z);
}
void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors()
{
// Integrate the angular velocity error into the attitude error
_att_error_rot_vec_rad += (_att_target_ang_vel_rads - _ahrs.get_gyro()) * _dt;
// Constrain attitude error
_att_error_rot_vec_rad.x = constrain_float(_att_error_rot_vec_rad.x, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
_att_error_rot_vec_rad.y = constrain_float(_att_error_rot_vec_rad.y, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
_att_error_rot_vec_rad.z = constrain_float(_att_error_rot_vec_rad.z, -AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD);
}
void AC_AttitudeControl::update_ang_vel_target_from_att_error()
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad)
{
Vector3f rate_target_ang_vel;
// Compute the roll angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) {
_ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x;
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
}
// Compute the pitch angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) {
_ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y;
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
}
// Compute the yaw angular velocity demand from the roll angle error
if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) {
_ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
if (_use_ff_and_input_shaping) {
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS));
}else{
_ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z;
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
}
// Add feedforward term that attempts to ensure that the copter yaws about the reference
// Z axis, rather than the vehicle body Z axis.
// NOTE: This is a small-angle approximation.
_ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z;
_ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z;
return rate_target_ang_vel;
}
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
// Run the roll angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_roll(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().x;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -534,7 +604,8 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
// Run the pitch angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_pitch(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().y;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -557,7 +628,8 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
// Run the yaw angular velocity PID controller and return the output
float AC_AttitudeControl::rate_target_to_motor_yaw(float rate_target_rads)
{
float current_rate_rads = _ahrs.get_gyro().z;
float rate_error_rads = rate_target_rads - current_rate_rads;
@ -580,6 +652,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
return constrain_float(output, -1.0f, 1.0f);
}
// Enable or disable body-frame feed forward
void AC_AttitudeControl::accel_limiting(bool enable_limits)
{
if (enable_limits) {
@ -600,33 +673,14 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
}
}
void AC_AttitudeControl::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl::get_althold_lean_angle_max() const
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost) {
_motors.set_throttle(get_boosted_throttle(throttle_in));
}else{
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
}
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (reset_attitude_control) {
relax_bf_rate_controller();
set_yaw_target_to_current_heading();
}
_motors.set_throttle(throttle_in);
_angle_boost = 0.0f;
// convert to centi-degrees for public interface
return ToDeg(_althold_lean_angle_max) * 100.0f;
}
// Proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim)
{
if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {
@ -644,43 +698,35 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
}
}
void AC_AttitudeControl::get_rotation_vehicle_to_ned(Matrix3f& m)
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
float AC_AttitudeControl::stopping_point(float first_ord_mag, float p, float second_ord_lim)
{
m = _ahrs.get_rotation_body_to_ned();
}
void AC_AttitudeControl::get_rotation_ned_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_reference_to_ned(Matrix3f& m)
{
m.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
}
void AC_AttitudeControl::get_rotation_ned_to_reference(Matrix3f& m)
{
get_rotation_reference_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_vehicle_to_reference(Matrix3f& m)
{
Matrix3f Tvn;
Matrix3f Tnr;
get_rotation_vehicle_to_ned(Tvn);
get_rotation_ned_to_reference(Tnr);
m = Tnr * Tvn;
}
void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_reference(m);
m = m.transposed();
if (second_ord_lim > 0.0f && !is_zero(second_ord_lim) && is_zero(p)) {
return (first_ord_mag*first_ord_mag)/(2.0f*second_ord_lim);
} else if ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && !is_zero(p)) {
return first_ord_mag/p;
} else if ((second_ord_lim < 0.0f || is_zero(second_ord_lim)) && is_zero(p)) {
return 0.0f;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
float linear_velocity = second_ord_lim/p;
if (fabsf(first_ord_mag) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
return first_ord_mag/p;
} else {
float linear_dist = second_ord_lim/sq(p);
float overshoot = (linear_dist/2.0f) + sq(first_ord_mag)/(2.0f*second_ord_lim);
if (first_ord_mag > 0){
return overshoot;
} else {
return -overshoot;
}
}
}
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = get_rate_roll_pid().get_filt_alpha();
@ -688,6 +734,7 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid().kD())/_dt + get_rate_roll_pid().kP());
}
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = get_rate_pitch_pid().get_filt_alpha();
@ -695,6 +742,7 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP());
}
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = get_rate_yaw_pid().get_filt_alpha();

225
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -13,9 +13,6 @@
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h>
// TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centidegrees
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
#define AC_ATTITUDE_CONTROL_ANGLE_P 6.0f // default angle P gain for roll, pitch and yaw
#else
@ -25,8 +22,8 @@
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(360.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 1000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec.
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(120.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 6000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec.
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 0.0f
@ -38,17 +35,21 @@
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis)
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis)
#define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD radians(300.0f) // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
#define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX_RAD radians(300.0f) // earth-frame rate stabilize controller's maximum overshoot angle (never limited)
#define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD radians(10.0f) // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX_RAD radians(30.0f) // earth-frame rate stabilize controller's maximum overshoot angle
#define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited
#define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate
#define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
#define AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ 1.0f // filter (in hz) of throttle filter used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix
#define AC_ATTITUDE_CONTROL_MID_DEFAULT 0.5f // manual throttle mix
#define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
class AC_AttitudeControl {
public:
@ -61,8 +62,9 @@ public:
_p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
_dt(dt),
_angle_boost(0),
_att_ctrl_use_accel_limit(true),
_throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ),
_use_ff_and_input_shaping(true),
_throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
_ahrs(ahrs),
_aparm(aparm),
_motors(motors)
@ -108,35 +110,32 @@ public:
// Sets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; _accel_yaw_max.save(); }
// Ensure body-frame rate controller has zero errors to relax rate controller output
void relax_bf_rate_controller();
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers();
// Sets yaw target to vehicle heading
void set_yaw_target_to_current_heading() { _att_target_euler_rad.z = _ahrs.yaw; }
void set_yaw_target_to_current_heading() { _attitude_target_euler_angle.z = _ahrs.yaw; }
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void shift_ef_yaw_target(float yaw_shift_cd);
// Command a Quaternion attitude with feedforward and smoothing
void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain);
// Command an euler roll, pitch and yaw angle
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
// Command an euler roll, pitch, and yaw rate
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
// Command an angular velocity
// Command an angular velocity with angular velocity feedforward and smoothing
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Command a quaternion attitude and a body-frame angular velocity
void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads);
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run();
virtual void rate_controller_run() = 0;
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);
@ -146,24 +145,23 @@ public:
bool ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads);
// Configures whether the attitude controller should limit the rate demand to constrain angular acceleration
void limit_angle_to_rate_request(bool limit_request) { _att_ctrl_use_accel_limit = limit_request; }
void use_ff_and_input_shaping(bool use_shaping) { _use_ff_and_input_shaping = use_shaping; }
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
// attitude controller's reference attitude.
Vector3f get_att_target_euler_cd() const { return _att_target_euler_rad*degrees(100.0f); }
// attitude controller's target attitude.
Vector3f get_att_target_euler_cd() const { return _attitude_target_euler_angle*degrees(100.0f); }
// Return a rotation vector in centidegrees representing the rotation from vehicle body frame to the
// attitude controller's reference attitude.
Vector3f get_att_error_rot_vec_cd() const { return _att_error_rot_vec_rad*degrees(100.0f); }
// Return the angle between the target thrust vector and the current thrust vector.
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
// Set x-axis angular velocity reference in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _ang_vel_target_rads.x = radians(rate_cds*0.01f); }
// Set x-axis angular velocity in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _rate_target_ang_vel.x = radians(rate_cds*0.01f); }
// Set y-axis angular velocity reference in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _ang_vel_target_rads.y = radians(rate_cds*0.01f); }
// Set y-axis angular velocity in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _rate_target_ang_vel.y = radians(rate_cds*0.01f); }
// Set z-axis angular velocity reference in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _ang_vel_target_rads.z = radians(rate_cds*0.01f); }
// Set z-axis angular velocity in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _rate_target_ang_vel.z = radians(rate_cds*0.01f); }
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_roll();
@ -183,8 +181,8 @@ public:
// Return yaw step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw()/_p_angle_yaw.kP(); }
// Return reference angular velocity used in the angular velocity controller
Vector3f rate_bf_targets() const { return _ang_vel_target_rads*degrees(100.0f); }
// Return angular velocity in radians used in the angular velocity controller
Vector3f rate_bf_targets() const { return _rate_target_ang_vel; }
// Enable or disable body-frame feed forward
void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; }
@ -198,8 +196,11 @@ public:
// Enable or disable body-frame feed forward
void accel_limiting(bool enable_or_disable);
// Update Alt_Hold angle maximum
virtual void update_althold_lean_angle_max(float throttle_in) = 0;
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff);
virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
// Set output throttle and disable stabilization
void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff);
@ -211,7 +212,7 @@ public:
float angle_boost() const { return _angle_boost; }
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
virtual float get_althold_lean_angle_max() const = 0;
float get_althold_lean_angle_max() const;
// Return configured tilt angle limit in centidegrees
float lean_angle_max() const { return _aparm.angle_max; }
@ -219,60 +220,43 @@ public:
// Proportional controller with piecewise sqrt sections to constrain second derivative
static float sqrt_controller(float error, float p, float second_ord_lim);
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
static float stopping_point(float first_ord_mag, float p, float second_ord_lim);
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel);
// limits the acceleration and deceleration of a velocity request
float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max);
// translates body frame acceleration limits to the euler axis
Vector3f euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel);
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot);
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();
protected:
// Retrieve a rotation matrix from the vehicle body frame to NED earth frame
void get_rotation_vehicle_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to the vehicle body frame
void get_rotation_ned_to_vehicle(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to NED earth frame
void get_rotation_reference_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to reference (setpoint) body frame
void get_rotation_ned_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from vehicle body frame to reference (setpoint) body frame
void get_rotation_vehicle_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to vehicle body frame
void get_rotation_reference_to_vehicle(Matrix3f& m);
// Command an euler attitude and a body-frame angular velocity
void attitude_controller_run_euler(const Vector3f& att_target_euler_rad, const Vector3f& att_target_ang_vel_rads);
// Command a quaternion attitude and a body-frame angular velocity
void attitude_controller_run_quat(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads);
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_roll(float euler_roll_rate_rads, float overshoot_max_rad);
// Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative
void update_att_target_pitch(float euler_pitch_rate_rads, float overshoot_max_rad);
// Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative
void update_att_target_yaw(float euler_yaw_rate_rads, float overshoot_max_rad);
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();
// Update _ang_vel_target_rads using _att_error_rot_vec_rad
void update_ang_vel_target_from_att_error();
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad);
// Run the roll angular velocity PID controller and return the output
float rate_bf_to_motor_roll(float rate_target_rads);
float rate_target_to_motor_roll(float rate_target_rads);
// Run the pitch angular velocity PID controller and return the output
float rate_bf_to_motor_pitch(float rate_target_rads);
float rate_target_to_motor_pitch(float rate_target_rads);
// Run the yaw angular velocity PID controller and return the output
virtual float rate_bf_to_motor_yaw(float rate_target_rads);
// Compute a throttle value that is adjusted for the tilt angle of the vehicle
virtual float get_boosted_throttle(float throttle_in) = 0;
virtual float rate_target_to_motor_yaw(float rate_target_rads);
// Return angle in radians to be added to roll angle. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
@ -316,64 +300,75 @@ protected:
AC_P _p_angle_pitch;
AC_P _p_angle_yaw;
// Angle limit time constant (to maintain altitude)
AP_Float _angle_limit_tc;
// Intersampling period in seconds
float _dt;
// This represents a 321-intrinsic rotation from NED frame to the reference (setpoint)
// attitude used in the attitude controller, in radians. Formerly _angle_ef_target.
Vector3f _att_target_euler_rad;
// This represents a 321-intrinsic rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller, in radians.
Vector3f _attitude_target_euler_angle;
// This represents an euler axis-angle rotation vector from the vehicle’s
// estimated attitude to the reference (setpoint) attitude used in the attitude
// controller, in radians in the vehicle body frame of reference. Formerly
// _angle_bf_error.
Vector3f _att_error_rot_vec_rad;
// This represents the angular velocity of the reference (setpoint) attitude used in
// This represents the angular velocity of the target (setpoint) attitude used in
// the attitude controller as 321-intrinsic euler angle derivatives, in radians per
// second. Formerly _rate_ef_desired.
Vector3f _att_target_euler_rate_rads;
// second.
Vector3f _attitude_target_euler_rate;
// This represents the angular velocity of the reference (setpoint) attitude used in
// This represents a quaternion rotation from NED frame to the target (setpoint)
// attitude used in the attitude controller.
Quaternion _attitude_target_quat;
// This represents the angular velocity of the target (setpoint) attitude used in
// the attitude controller as an angular velocity vector, in radians per second in
// the reference attitude frame. Formerly _rate_bf_desired.
Vector3f _att_target_ang_vel_rads;
// the target attitude frame.
Vector3f _attitude_target_ang_vel;
// This represents the reference (setpoint) angular velocity used in the angular
// velocity controller, in radians per second. Formerly _rate_bf_target.
Vector3f _ang_vel_target_rads;
// This represents the angular velocity in radians per second, used in the angular
// velocity controller.
Vector3f _rate_target_ang_vel;
// The angle between the target thrust vector and the current thrust vector.
float _thrust_error_angle;
// throttle provided as input to attitude controller. This does not include angle boost.
// Used only for logging.
float _throttle_in = 0.0f;
// This represents the throttle increase applied for tilt compensation.
// Used only for logging.
float _angle_boost;
// Specifies whether the attitude controller should use the acceleration limit
bool _att_ctrl_use_accel_limit;
// Specifies whether the attitude controller should use the input shaping and feedforward
bool _use_ff_and_input_shaping;
// Filtered throttle input - used to limit lean angle when throttle is saturated
LowPassFilterFloat _throttle_in_filt;
// Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
float _althold_lean_angle_max = 0.0f;
// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix_desired;
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_rpy_mix;
// References to external libraries
const AP_AHRS& _ahrs;
const AP_Vehicle::MultiCopter &_aparm;
AP_Motors& _motors;
private:
protected:
/*
state of control monitoring
*/
struct {
float rms_roll;
float rms_pitch;
float rms_roll_P;
float rms_roll_D;
float rms_pitch_P;
float rms_pitch_D;
float rms_yaw;
} _control_monitor;
// update state in ControlMonitor
void control_monitor_filter_pid(const DataFlash_Class::PID_Info &pid_info, float &rms);
void control_monitor_filter_pid(float value, float &rms_P);
void control_monitor_update(void);
public:
@ -382,6 +377,10 @@ public:
// return current RMS controller filter for each axis
float control_monitor_rms_output_roll(void) const;
float control_monitor_rms_output_roll_P(void) const;
float control_monitor_rms_output_roll_D(void) const;
float control_monitor_rms_output_pitch_P(void) const;
float control_monitor_rms_output_pitch_D(void) const;
float control_monitor_rms_output_pitch(void) const;
float control_monitor_rms_output_yaw(void) const;
};

80
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -150,17 +150,17 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_att_target_ang_vel_rads.x = _ahrs.get_gyro().x;
_att_target_ang_vel_rads.y = _ahrs.get_gyro().y;
_attitude_target_ang_vel.x = _ahrs.get_gyro().x;
_attitude_target_ang_vel.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _att_target_ang_vel_rads.z;
float rate_change_rads = yaw_rate_bf_rads - _attitude_target_ang_vel.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_att_target_ang_vel_rads.z += rate_change_rads;
_attitude_target_ang_vel.z += rate_change_rads;
} else {
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
_attitude_target_ang_vel.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
@ -173,32 +173,44 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_rate(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_att_target_euler_rad.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_att_target_euler_rad.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_att_target_euler_rad.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
_attitude_target_euler_angle.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_attitude_target_euler_angle.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_attitude_target_euler_angle.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_att_target_euler_rad.y > M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
if (_attitude_target_euler_angle.y > M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
if (_att_target_euler_rad.y < -M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(-M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
if (_attitude_target_euler_angle.y < -M_PI/2.0f) {
_attitude_target_euler_angle.x = wrap_PI(_attitude_target_euler_angle.x + M_PI);
_attitude_target_euler_angle.y = wrap_PI(-M_PI - _attitude_target_euler_angle.x);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
update_ang_vel_target_from_att_error();
_rate_target_ang_vel = update_ang_vel_target_from_att_error(_att_error_rot_vec_rad);
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_ang_vel_target_rads.x = _att_target_ang_vel_rads.x;
_ang_vel_target_rads.y = _att_target_ang_vel_rads.y;
_rate_target_ang_vel.x = _attitude_target_ang_vel.x;
_rate_target_ang_vel.y = _attitude_target_ang_vel.y;
// add desired target to yaw
_ang_vel_target_rads.z += _att_target_ang_vel_rads.z;
_rate_target_ang_vel.z += _attitude_target_ang_vel.z;
_thrust_error_angle = norm(_att_error_rot_vec_rad.x, _att_error_rot_vec_rad.y);
}
void AC_AttitudeControl_Heli::integrate_bf_rate_error_to_angle_errors()
{
// Integrate the angular velocity error into the attitude error
_att_error_rot_vec_rad += (_attitude_target_ang_vel - _ahrs.get_gyro()) * _dt;
// Constrain attitude error
_att_error_rot_vec_rad.x = constrain_float(_att_error_rot_vec_rad.x, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
_att_error_rot_vec_rad.y = constrain_float(_att_error_rot_vec_rad.y, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
_att_error_rot_vec_rad.z = constrain_float(_att_error_rot_vec_rad.z, -AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD, AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD);
}
// subclass non-passthrough too, for external gyro, no flybar
@ -223,23 +235,20 @@ void AC_AttitudeControl_Heli::rate_controller_run()
_motors.set_roll(_passthrough_roll/4500.0f);
_motors.set_pitch(_passthrough_pitch/4500.0f);
} else {
rate_bf_to_motor_roll_pitch(_ang_vel_target_rads.x, _ang_vel_target_rads.y);
rate_bf_to_motor_roll_pitch(_rate_target_ang_vel.x, _rate_target_ang_vel.y);
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw/4500.0f);
} else {
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
_motors.set_yaw(rate_target_to_motor_yaw(_rate_target_ang_vel.z));
}
}
// get lean angle max for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const
// Update Alt_Hold angle maximum
void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
{
// calc maximum tilt angle based on throttle
float ret = acosf(constrain_float(_throttle_in_filt.get()/0.9f, 0.0f, 1.0f));
// TEMP: convert to centi-degrees for public interface
return degrees(ret) * 100.0f;
float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f));
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max);
}
//
@ -349,7 +358,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
}
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)
{
float pd,i,vff; // used to capture pid values for logging
float current_rate_rads; // this iteration's rate
@ -404,11 +413,12 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
// throttle functions
//
// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in)
void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
// no angle boost for trad helis
_throttle_in = throttle_in;
update_althold_lean_angle_max(throttle_in);
_motors.set_throttle_filter_cutoff(filter_cutoff);
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
return throttle_in;
}

23
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -27,6 +27,7 @@
#define AC_ATTITUDE_HELI_RATE_RP_FF_FILTER 10.0f
#define AC_ATTITUDE_HELI_RATE_Y_VFF_FILTER 10.0f
#define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT 300
#define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD ToRad(30.0f)
class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public:
@ -68,6 +69,9 @@ public:
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds);
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();
// subclass non-passthrough too, for external gyro, no flybar
void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
@ -75,9 +79,8 @@ public:
// should be called at 100hz or more
virtual void rate_controller_run();
// get lean angle max for pilot input that prioritises altitude hold over lean angle
// NOTE: returns centi-degrees
float get_althold_lean_angle_max() const;
// Update Alt_Hold angle maximum
void update_althold_lean_angle_max(float throttle_in) override;
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage
void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; }
@ -95,6 +98,9 @@ public:
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
void set_hover_roll_trim_scalar(float scalar) {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);}
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
// user settable parameters
static const struct AP_Param::GroupInfo var_info[];
@ -117,14 +123,11 @@ private:
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/sec) for roll, pitch and yaw
// outputs are sent directly to motor class
void rate_bf_to_motor_roll_pitch(float rate_roll_target_rads, float rate_pitch_target_rads);
float rate_bf_to_motor_yaw(float rate_yaw_rads);
float rate_target_to_motor_yaw(float rate_yaw_rads);
//
// throttle methods
//
// calculate total body frame throttle required to produce the given earth frame throttle
float get_boosted_throttle(float throttle_in);
// pass through for roll and pitch
int16_t _passthrough_roll;
@ -139,6 +142,12 @@ private:
// internal variables
float _hover_roll_trim_scalar = 0; // scalar used to suppress Hover Roll Trim
// This represents an euler axis-angle rotation vector from the vehicles
// estimated attitude to the reference (setpoint) attitude used in the attitude
// controller, in radians in the vehicle body frame of reference.
Vector3f _att_error_rot_vec_rad;
// parameters
AP_Int8 _piro_comp_enabled; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _hover_roll_trim; // Angle in centi-degrees used to counter tail rotor thrust in hover

75
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -120,6 +120,20 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Units: Hz
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum
// @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
// @Range: 0.1 0.25
// @User: Advanced
AP_GROUPINFO("THR_MIX_MIN", 4, AC_AttitudeControl_Multi, _thr_mix_min, AC_ATTITUDE_CONTROL_MIN_DEFAULT),
// @Param: THR_MIX_MAX
// @DisplayName: Throttle Mix Maximum
// @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
// @Range: 0.5 0.9
// @User: Advanced
AP_GROUPINFO("THR_MIX_MAX", 5, AC_AttitudeControl_Multi, _thr_mix_max, AC_ATTITUDE_CONTROL_MAX_DEFAULT),
AP_GROUPEND
};
@ -133,23 +147,41 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehic
AP_Param::setup_object_defaults(this, var_info);
}
// get lean angle max for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
// Update Alt_Hold angle maximum
void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
{
// calc maximum tilt angle based on throttle
float thr_max = _motors_multi.get_throttle_thrust_max();
// divide by zero check
if (is_zero(thr_max)) {
return 0.0f;
_althold_lean_angle_max = 0.0f;
return;
}
return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f;
float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
_althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+_angle_limit_tc))*(althold_lean_angle_max-_althold_lean_angle_max);
}
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
update_althold_lean_angle_max(throttle_in);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost) {
// Apply angle boost
throttle_in = get_throttle_boosted(throttle_in);
}else{
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
_motors.set_throttle(throttle_in);
_motors.set_throttle_avg_max(get_throttle_avg_max(MAX(throttle_in, _throttle_in)));
}
// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)
float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
{
if (!_angle_boost_enabled) {
_angle_boost = 0;
@ -166,3 +198,36 @@ float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)
_angle_boost = constrain_float(throttle_out - throttle_in,-1.0f,1.0f);
return throttle_out;
}
// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float AC_AttitudeControl_Multi::get_throttle_avg_max(float throttle_in)
{
return MAX(throttle_in, throttle_in*MAX(0.0f,1.0f-_throttle_rpy_mix)+_motors.get_throttle_hover()*_throttle_rpy_mix);
}
// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
{
// slew _throttle_rpy_mix to _throttle_rpy_mix_desired
if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
_throttle_rpy_mix += MIN(2.0f*_dt, _throttle_rpy_mix_desired-_throttle_rpy_mix);
} else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
_throttle_rpy_mix -= MIN(0.5f*_dt, _throttle_rpy_mix-_throttle_rpy_mix_desired);
}
_throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f);
}
void AC_AttitudeControl_Multi::rate_controller_run()
{
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_motors.set_roll(rate_target_to_motor_roll(_rate_target_ang_vel.x));
_motors.set_pitch(rate_target_to_motor_pitch(_rate_target_ang_vel.y));
_motors.set_yaw(rate_target_to_motor_yaw(_rate_target_ang_vel.z));
control_monitor_update();
}

Some files were not shown because too many files have changed in this diff Show More