forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into fw_autoland_att_tecs
This commit is contained in:
Thomas Gubler
commit
74a7d9693d
|
|
@ -67,8 +67,7 @@ pwm rate -c 1234 -r 400
|
|||
# Set disarmed, min and max PWM signals
|
||||
#
|
||||
pwm disarmed -c 1234 -p 900
|
||||
pwm min -c 1234 -p 1200
|
||||
pwm max -c 1234 -p 1800
|
||||
pwm min -c 1234 -p 1050
|
||||
|
||||
#
|
||||
# Start common for all multirotors apps
|
||||
|
|
|
|||
|
|
@ -0,0 +1,160 @@
|
|||
#
|
||||
# Makefile for the px4fmu_default configuration
|
||||
#
|
||||
|
||||
#
|
||||
# Use the configuration's ROMFS.
|
||||
#
|
||||
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
|
||||
ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
|
||||
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
MODULES += drivers/device
|
||||
MODULES += drivers/stm32
|
||||
MODULES += drivers/stm32/adc
|
||||
MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/px4io
|
||||
MODULES += drivers/px4fmu
|
||||
MODULES += drivers/boards/px4fmu-v1
|
||||
MODULES += drivers/ardrone_interface
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/bma180
|
||||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/mb12xx
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/eeprom
|
||||
MODULES += systemcmds/ramtron
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/boardinfo
|
||||
MODULES += systemcmds/i2c
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/preflight_check
|
||||
MODULES += systemcmds/pwm
|
||||
MODULES += systemcmds/esc_calib
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/top
|
||||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/config
|
||||
MODULES += systemcmds/nshterm
|
||||
|
||||
#
|
||||
# General system control
|
||||
#
|
||||
MODULES += modules/commander
|
||||
MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/mavlink_onboard
|
||||
MODULES += modules/gpio_led
|
||||
|
||||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
#
|
||||
#MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/att_pos_estimator_ekf
|
||||
#MODULES += modules/position_estimator_inav
|
||||
MODULES += examples/flow_position_estimator
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
|
||||
#MODULES += modules/fw_pos_control_l1
|
||||
#MODULES += modules/fw_att_control
|
||||
#MODULES += modules/multirotor_att_control
|
||||
#MODULES += modules/multirotor_pos_control
|
||||
#MODULES += examples/flow_position_control
|
||||
#MODULES += examples/flow_speed_control
|
||||
MODULES += modules/fixedwing_backside
|
||||
|
||||
#
|
||||
# Logging
|
||||
#
|
||||
MODULES += modules/sdlog2
|
||||
|
||||
#
|
||||
# Unit tests
|
||||
#
|
||||
#MODULES += modules/unit_test
|
||||
#MODULES += modules/commander/commander_tests
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/controllib
|
||||
MODULES += modules/uORB
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
MODULES += lib/mathlib/math/filter
|
||||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
#MODULES += examples/math_demo
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/hello_sky
|
||||
#MODULES += examples/px4_simple_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/daemon
|
||||
#MODULES += examples/px4_daemon_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
||||
#MODULES += examples/px4_mavlink_debug
|
||||
|
||||
# Tutorial code from
|
||||
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
|
||||
#MODULES += examples/fixedwing_control
|
||||
|
||||
# Hardware test
|
||||
#MODULES += examples/hwtest
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
# In general, these should move to modules over time.
|
||||
#
|
||||
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
|
||||
# to make the table a bit more readable.
|
||||
#
|
||||
define _B
|
||||
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
|
||||
endef
|
||||
|
||||
# command priority stack entrypoint
|
||||
BUILTIN_COMMANDS := \
|
||||
$(call _B, sercon, , 2048, sercon_main ) \
|
||||
$(call _B, serdis, , 2048, serdis_main ) \
|
||||
$(call _B, sysinfo, , 2048, sysinfo_main )
|
||||
|
|
@ -41,6 +41,7 @@
|
|||
|
||||
#include "KalmanNav.hpp"
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
// constants
|
||||
// Titterton pg. 52
|
||||
|
|
@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
|||
// attitude representations
|
||||
C_nb(),
|
||||
q(),
|
||||
_accel_sub(-1),
|
||||
_gyro_sub(-1),
|
||||
_mag_sub(-1),
|
||||
// subscriptions
|
||||
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
|
||||
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
|
||||
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||
// publications
|
||||
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
|
||||
_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
|
||||
_att(&getPublications(), ORB_ID(vehicle_attitude)),
|
||||
// timestamps
|
||||
_pubTimeStamp(hrt_absolute_time()),
|
||||
|
|
@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
|||
phi(0), theta(0), psi(0),
|
||||
vN(0), vE(0), vD(0),
|
||||
lat(0), lon(0), alt(0),
|
||||
lat0(0), lon0(0), alt0(0),
|
||||
// parameters for ground station
|
||||
_vGyro(this, "V_GYRO"),
|
||||
_vAccel(this, "V_ACCEL"),
|
||||
|
|
@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
|||
lon = 0.0f;
|
||||
alt = 0.0f;
|
||||
|
||||
// gyro, accel and mag subscriptions
|
||||
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
|
||||
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
|
||||
|
||||
struct accel_report accel;
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
|
||||
|
||||
struct mag_report mag;
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
|
||||
|
||||
// initialize rotation quaternion with a single raw sensor measurement
|
||||
q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
|
||||
_sensors.update();
|
||||
q = init(
|
||||
_sensors.accelerometer_m_s2[0],
|
||||
_sensors.accelerometer_m_s2[1],
|
||||
_sensors.accelerometer_m_s2[2],
|
||||
_sensors.magnetometer_ga[0],
|
||||
_sensors.magnetometer_ga[1],
|
||||
_sensors.magnetometer_ga[2]);
|
||||
|
||||
// initialize dcm
|
||||
C_nb = Dcm(q);
|
||||
|
|
@ -252,11 +248,21 @@ void KalmanNav::update()
|
|||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
// set reference position for
|
||||
// local position
|
||||
lat0 = lat;
|
||||
lon0 = lon;
|
||||
alt0 = alt;
|
||||
// XXX map_projection has internal global
|
||||
// states that multiple things could change,
|
||||
// should make map_projection take reference
|
||||
// lat/lon and not have init
|
||||
map_projection_init(lat0, lon0);
|
||||
_positionInitialized = true;
|
||||
warnx("initialized EKF state with GPS\n");
|
||||
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
|
||||
double(vN), double(vE), double(vD),
|
||||
lat, lon, alt);
|
||||
lat, lon, double(alt));
|
||||
}
|
||||
|
||||
// prediction step
|
||||
|
|
@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
|
|||
{
|
||||
using namespace math;
|
||||
|
||||
// position publication
|
||||
// global position publication
|
||||
_pos.timestamp = _pubTimeStamp;
|
||||
_pos.time_gps_usec = _gps.timestamp_position;
|
||||
_pos.valid = true;
|
||||
|
|
@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
|
|||
_pos.vz = vD;
|
||||
_pos.yaw = psi;
|
||||
|
||||
// local position publication
|
||||
float x;
|
||||
float y;
|
||||
bool landed = alt < (alt0 + 0.1); // XXX improve?
|
||||
map_projection_project(lat, lon, &x, &y);
|
||||
_localPos.timestamp = _pubTimeStamp;
|
||||
_localPos.xy_valid = true;
|
||||
_localPos.z_valid = true;
|
||||
_localPos.v_xy_valid = true;
|
||||
_localPos.v_z_valid = true;
|
||||
_localPos.x = x;
|
||||
_localPos.y = y;
|
||||
_localPos.z = alt0 - alt;
|
||||
_localPos.vx = vN;
|
||||
_localPos.vy = vE;
|
||||
_localPos.vz = vD;
|
||||
_localPos.yaw = psi;
|
||||
_localPos.xy_global = true;
|
||||
_localPos.z_global = true;
|
||||
_localPos.ref_timestamp = _pubTimeStamp;
|
||||
_localPos.ref_lat = getLatDegE7();
|
||||
_localPos.ref_lon = getLonDegE7();
|
||||
_localPos.ref_alt = 0;
|
||||
_localPos.landed = landed;
|
||||
|
||||
// attitude publication
|
||||
_att.timestamp = _pubTimeStamp;
|
||||
_att.roll = phi;
|
||||
|
|
@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
|
|||
|
||||
// selectively update publications,
|
||||
// do NOT call superblock do-all method
|
||||
if (_positionInitialized)
|
||||
if (_positionInitialized) {
|
||||
_pos.update();
|
||||
_localPos.update();
|
||||
}
|
||||
|
||||
if (_attitudeInitialized)
|
||||
_att.update();
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@
|
|||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
|
@ -142,12 +143,8 @@ protected:
|
|||
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
|
||||
int _accel_sub; /**< Accelerometer subscription */
|
||||
int _gyro_sub; /**< Gyroscope subscription */
|
||||
int _mag_sub; /**< Magnetometer subscription */
|
||||
|
||||
// time stamps
|
||||
uint64_t _pubTimeStamp; /**< output data publication time stamp */
|
||||
uint64_t _predictTimeStamp; /**< prediction time stamp */
|
||||
|
|
@ -164,8 +161,10 @@ protected:
|
|||
float phi, theta, psi; /**< 3-2-1 euler angles */
|
||||
float vN, vE, vD; /**< navigation velocity, m/s */
|
||||
double lat, lon; /**< lat, lon radians */
|
||||
float alt; /**< altitude, meters */
|
||||
// parameters
|
||||
float alt; /**< altitude, meters */
|
||||
double lat0, lon0; /**< reference latitude and longitude */
|
||||
float alt0; /**< refeerence altitude (ground height) */
|
||||
control::BlockParamFloat _vGyro; /**< gyro process noise */
|
||||
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
|
||||
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
|
||||
|
|
|
|||
|
|
@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
_actuators.control[i] = 0.0f;
|
||||
|
||||
// only update guidance in auto mode
|
||||
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
|
||||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
// TODO use vehicle_control_mode here?
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
}
|
||||
|
|
@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// the setpoint should update to loitering around this position
|
||||
|
||||
// handle autopilot modes
|
||||
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
|
||||
_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
|
||||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
|
||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||
// for the purpose of control we will limit the velocity feedback between
|
||||
// calculate velocity, XXX should be airspeed,
|
||||
// but using ground speed for now for the purpose
|
||||
// of control we will limit the velocity feedback between
|
||||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vx * _pos.vx +
|
||||
|
|
@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// a first binary release can be targeted.
|
||||
// This is not a hack, but a design choice.
|
||||
|
||||
/* do not limit in HIL */
|
||||
// do not limit in HIL
|
||||
if (_status.hil_state != HIL_STATE_ON) {
|
||||
/* limit to value of manual throttle */
|
||||
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
|
||||
_actuators.control[CH_THR] : _manual.throttle;
|
||||
}
|
||||
|
||||
} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
|
||||
} else if (_status.main_state == MAIN_STATE_MANUAL) {
|
||||
_actuators.control[CH_AIL] = _manual.roll;
|
||||
_actuators.control[CH_ELV] = _manual.pitch;
|
||||
_actuators.control[CH_RDR] = _manual.yaw;
|
||||
_actuators.control[CH_THR] = _manual.throttle;
|
||||
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
|
||||
|
||||
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
|
||||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
|
||||
|
||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||
// for the purpose of control we will limit the velocity feedback between
|
||||
// the min/max velocity
|
||||
|
|
|
|||
|
|
@ -255,13 +255,13 @@ private:
|
|||
BlockWaypointGuidance _guide;
|
||||
|
||||
// block params
|
||||
BlockParam<float> _trimAil;
|
||||
BlockParam<float> _trimElv;
|
||||
BlockParam<float> _trimRdr;
|
||||
BlockParam<float> _trimThr;
|
||||
BlockParam<float> _trimV;
|
||||
BlockParam<float> _vCmd;
|
||||
BlockParam<float> _crMax;
|
||||
BlockParamFloat _trimAil;
|
||||
BlockParamFloat _trimElv;
|
||||
BlockParamFloat _trimRdr;
|
||||
BlockParamFloat _trimThr;
|
||||
BlockParamFloat _trimV;
|
||||
BlockParamFloat _vCmd;
|
||||
BlockParamFloat _crMax;
|
||||
|
||||
struct pollfd _attPoll;
|
||||
vehicle_global_position_set_triplet_s _lastPosCmd;
|
||||
|
|
|
|||
|
|
@ -72,6 +72,7 @@
|
|||
#include <systemlib/airspeed.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <commander/px4_custom_mode.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
|
|
@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
|
|||
static struct offboard_control_setpoint_s offboard_control_sp;
|
||||
|
||||
struct vehicle_global_position_s hil_global_pos;
|
||||
struct vehicle_local_position_s hil_local_pos;
|
||||
struct vehicle_attitude_s hil_attitude;
|
||||
struct vehicle_gps_position_s hil_gps;
|
||||
struct sensor_combined_s hil_sensors;
|
||||
struct battery_status_s hil_battery_status;
|
||||
static orb_advert_t pub_hil_global_pos = -1;
|
||||
static orb_advert_t pub_hil_local_pos = -1;
|
||||
static orb_advert_t pub_hil_attitude = -1;
|
||||
static orb_advert_t pub_hil_gps = -1;
|
||||
static orb_advert_t pub_hil_sensors = -1;
|
||||
|
|
@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
|
|||
static orb_advert_t vicon_position_pub = -1;
|
||||
static orb_advert_t telemetry_status_pub = -1;
|
||||
|
||||
// variables for HIL reference position
|
||||
static int32_t lat0 = 0;
|
||||
static int32_t lon0 = 0;
|
||||
static double alt0 = 0;
|
||||
|
||||
static void
|
||||
handle_message(mavlink_message_t *msg)
|
||||
{
|
||||
|
|
@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg)
|
|||
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
|
||||
}
|
||||
|
||||
hil_global_pos.valid = true;
|
||||
hil_global_pos.lat = hil_state.lat;
|
||||
hil_global_pos.lon = hil_state.lon;
|
||||
hil_global_pos.alt = hil_state.alt / 1000.0f;
|
||||
hil_global_pos.vx = hil_state.vx / 100.0f;
|
||||
hil_global_pos.vy = hil_state.vy / 100.0f;
|
||||
hil_global_pos.vz = hil_state.vz / 100.0f;
|
||||
|
||||
/* set timestamp and notify processes (broadcast) */
|
||||
hil_global_pos.timestamp = hrt_absolute_time();
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
// publish global position
|
||||
if (pub_hil_global_pos > 0) {
|
||||
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
|
||||
// global position packet
|
||||
hil_global_pos.timestamp = timestamp;
|
||||
hil_global_pos.valid = true;
|
||||
hil_global_pos.lat = hil_state.lat;
|
||||
hil_global_pos.lon = hil_state.lon;
|
||||
hil_global_pos.alt = hil_state.alt / 1000.0f;
|
||||
hil_global_pos.vx = hil_state.vx / 100.0f;
|
||||
hil_global_pos.vy = hil_state.vy / 100.0f;
|
||||
hil_global_pos.vz = hil_state.vz / 100.0f;
|
||||
|
||||
} else {
|
||||
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
|
||||
}
|
||||
|
||||
// publish local position
|
||||
if (pub_hil_local_pos > 0) {
|
||||
float x;
|
||||
float y;
|
||||
bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
|
||||
double lat = hil_state.lat*1e-7;
|
||||
double lon = hil_state.lon*1e-7;
|
||||
map_projection_project(lat, lon, &x, &y);
|
||||
hil_local_pos.timestamp = timestamp;
|
||||
hil_local_pos.xy_valid = true;
|
||||
hil_local_pos.z_valid = true;
|
||||
hil_local_pos.v_xy_valid = true;
|
||||
hil_local_pos.v_z_valid = true;
|
||||
hil_local_pos.x = x;
|
||||
hil_local_pos.y = y;
|
||||
hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
|
||||
hil_local_pos.vx = hil_state.vx/100.0f;
|
||||
hil_local_pos.vy = hil_state.vy/100.0f;
|
||||
hil_local_pos.vz = hil_state.vz/100.0f;
|
||||
hil_local_pos.yaw = hil_attitude.yaw;
|
||||
hil_local_pos.xy_global = true;
|
||||
hil_local_pos.z_global = true;
|
||||
hil_local_pos.ref_timestamp = timestamp;
|
||||
hil_local_pos.ref_lat = hil_state.lat;
|
||||
hil_local_pos.ref_lon = hil_state.lon;
|
||||
hil_local_pos.ref_alt = alt0;
|
||||
hil_local_pos.landed = landed;
|
||||
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
|
||||
} else {
|
||||
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
|
||||
lat0 = hil_state.lat;
|
||||
lon0 = hil_state.lon;
|
||||
alt0 = hil_state.alt / 1000.0f;
|
||||
map_projection_init(hil_state.lat, hil_state.lon);
|
||||
}
|
||||
|
||||
/* Calculate Rotation Matrix */
|
||||
math::Quaternion q(hil_state.attitude_quaternion);
|
||||
math::Dcm C_nb(q);
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ struct actuator_armed_s armed;
|
|||
struct actuator_controls_effective_s actuators_effective_0;
|
||||
struct actuator_controls_s actuators_0;
|
||||
struct vehicle_attitude_s att;
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
struct mavlink_subscriptions mavlink_subs;
|
||||
|
||||
|
|
@ -92,6 +93,8 @@ static unsigned int gps_counter;
|
|||
*/
|
||||
static uint64_t last_sensor_timestamp;
|
||||
|
||||
static hrt_abstime last_sent_vfr = 0;
|
||||
|
||||
static void *uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener {
|
||||
|
|
@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
|
|||
/* copy attitude data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
|
||||
|
||||
if (gcs_link)
|
||||
if (gcs_link) {
|
||||
/* send sensor values */
|
||||
mavlink_msg_attitude_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
|
|
@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
|
|||
att.pitchspeed,
|
||||
att.yawspeed);
|
||||
|
||||
/* limit VFR message rate to 10Hz */
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
if (t >= last_sent_vfr + 100000) {
|
||||
last_sent_vfr = t;
|
||||
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
|
||||
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
|
||||
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
|
||||
}
|
||||
}
|
||||
|
||||
attitude_counter++;
|
||||
}
|
||||
|
||||
|
|
@ -681,17 +695,7 @@ l_home(const struct listener *l)
|
|||
void
|
||||
l_airspeed(const struct listener *l)
|
||||
{
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
|
||||
|
||||
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
|
||||
uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
|
||||
float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
|
||||
float alt = global_pos.relative_alt;
|
||||
float climb = -global_pos.vz;
|
||||
|
||||
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -849,7 +853,7 @@ uorb_receive_start(void)
|
|||
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
|
||||
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
|
||||
|
||||
/* --- AIRSPEED / VFR / HUD --- */
|
||||
/* --- AIRSPEED --- */
|
||||
mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
|
||||
|
||||
|
|
|
|||
|
|
@ -59,31 +59,23 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
|
||||
//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */
|
||||
|
||||
struct mc_rate_control_params {
|
||||
|
||||
float yawrate_p;
|
||||
float yawrate_d;
|
||||
float yawrate_i;
|
||||
//float yawrate_awu;
|
||||
//float yawrate_lim;
|
||||
|
||||
float attrate_p;
|
||||
float attrate_d;
|
||||
float attrate_i;
|
||||
//float attrate_awu;
|
||||
//float attrate_lim;
|
||||
|
||||
float rate_lim;
|
||||
};
|
||||
|
|
@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
|
|||
param_t yawrate_p;
|
||||
param_t yawrate_i;
|
||||
param_t yawrate_d;
|
||||
//param_t yawrate_awu;
|
||||
//param_t yawrate_lim;
|
||||
|
||||
param_t attrate_p;
|
||||
param_t attrate_i;
|
||||
param_t attrate_d;
|
||||
//param_t attrate_awu;
|
||||
//param_t attrate_lim;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
|
|||
h->yawrate_p = param_find("MC_YAWRATE_P");
|
||||
h->yawrate_i = param_find("MC_YAWRATE_I");
|
||||
h->yawrate_d = param_find("MC_YAWRATE_D");
|
||||
//h->yawrate_awu = param_find("MC_YAWRATE_AWU");
|
||||
//h->yawrate_lim = param_find("MC_YAWRATE_LIM");
|
||||
|
||||
h->attrate_p = param_find("MC_ATTRATE_P");
|
||||
h->attrate_i = param_find("MC_ATTRATE_I");
|
||||
h->attrate_d = param_find("MC_ATTRATE_D");
|
||||
//h->attrate_awu = param_find("MC_ATTRATE_AWU");
|
||||
//h->attrate_lim = param_find("MC_ATTRATE_LIM");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
|
|||
param_get(h->yawrate_p, &(p->yawrate_p));
|
||||
param_get(h->yawrate_i, &(p->yawrate_i));
|
||||
param_get(h->yawrate_d, &(p->yawrate_d));
|
||||
//param_get(h->yawrate_awu, &(p->yawrate_awu));
|
||||
//param_get(h->yawrate_lim, &(p->yawrate_lim));
|
||||
|
||||
param_get(h->attrate_p, &(p->attrate_p));
|
||||
param_get(h->attrate_i, &(p->attrate_i));
|
||||
param_get(h->attrate_d, &(p->attrate_d));
|
||||
//param_get(h->attrate_awu, &(p->attrate_awu));
|
||||
//param_get(h->attrate_lim, &(p->attrate_lim));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
static PID_t pitch_rate_controller;
|
||||
static PID_t roll_rate_controller;
|
||||
static PID_t yaw_rate_controller;
|
||||
|
||||
static struct mc_rate_control_params p;
|
||||
static struct mc_rate_control_param_handles h;
|
||||
|
|
@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
|
||||
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
|
||||
|
||||
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
|
||||
}
|
||||
|
||||
/* load new parameters with lower rate */
|
||||
|
|
@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
|
||||
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
|
||||
pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* reset integrals if needed */
|
||||
if (reset_integral) {
|
||||
pid_reset_integral(&pitch_rate_controller);
|
||||
pid_reset_integral(&roll_rate_controller);
|
||||
// TODO pid_reset_integral(&yaw_rate_controller);
|
||||
pid_reset_integral(&yaw_rate_controller);
|
||||
}
|
||||
|
||||
/* control pitch (forward) output */
|
||||
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
|
||||
rates[1], 0.0f, deltaT);
|
||||
|
||||
/* control roll (left/right) output */
|
||||
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
|
||||
rates[0], 0.0f, deltaT);
|
||||
|
||||
/* control yaw rate */ //XXX use library here
|
||||
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (!isfinite(yaw_rate_control)) {
|
||||
yaw_rate_control = 0.0f;
|
||||
warnx("rej. NaN ctrl yaw");
|
||||
}
|
||||
/* run pitch, roll and yaw controllers */
|
||||
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
|
||||
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
|
||||
float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
|
||||
|
||||
actuators->control[0] = roll_control;
|
||||
actuators->control[1] = pitch_control;
|
||||
actuators->control[2] = yaw_rate_control;
|
||||
actuators->control[2] = yaw_control;
|
||||
actuators->control[3] = rate_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
|
|
|
|||
|
|
@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
|
|||
} else {
|
||||
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
|
||||
}
|
||||
att_sp.yaw_body = global_pos_sp.yaw;
|
||||
/* update yaw setpoint only if value is valid */
|
||||
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
|
||||
att_sp.yaw_body = global_pos_sp.yaw;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue