Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Merge branch 'navigator_new' into navigator_new_vector, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-28 10:04:13 +04:00
parent 153114aec8 72d9c80ce9
commit 7b7539fbbd
77 changed files with 5914 additions and 1412 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+10
View File
@@ -113,6 +113,16 @@ then
#
commander start
#
# Start the datamanager
#
dataman start
#
# Start the Navigator
#
navigator start
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/1000_rc_fw_easystar.hil
makefiles/config_px4fmu-v1_backside.mk
+1
View File
@@ -107,6 +107,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
makefiles/config_px4fmu-v1_default.mk
+1
View File
@@ -106,6 +106,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -104,6 +104,7 @@ MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/controllib
MODULES += modules/uORB
MODULES += modules/dataman
#
# Libraries
makefiles/config_px4fmu-v2_logging.mk
+1
View File
@@ -115,6 +115,7 @@ MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
MODULES += modules/dataman
#
# Demo apps
src/drivers/drv_rc_input.h
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/drivers/hott/messages.cpp
+2 -2
View File
@@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size)
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), _home_sub, &home);
_home_lat = ((double)(home.lat))*1e-7d;
_home_lon = ((double)(home.lon))*1e-7d;
_home_lat = home.lat;
_home_lon = home.lon;
_home_position_set = true;
}
src/drivers/px4io/px4io.cpp
+111 -34
View File
@@ -228,33 +228,36 @@ private:
device::Device *_interface;
// XXX
unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///<Maximum relays supported by PX4IO
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
unsigned _hardware; ///< Hardware revision
unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO
unsigned _max_controls; ///< Maximum # of controls supported by PX4IO
unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO
unsigned _max_relays; ///< Maximum relays supported by PX4IO
unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///<mavlink file descriptor for thread.
int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
int _thread_mavlink_fd; ///< mavlink file descriptor for thread.
perf_counter_t _perf_update; ///<local performance counter for status updates
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
/* cached IO state */
uint16_t _status; ///<Various IO status flags
uint16_t _alarms; ///<Various IO alarms
uint16_t _status; ///< Various IO status flags
uint16_t _alarms; ///< Various IO alarms
/* subscribed topics */
int _t_actuators; ///< actuator controls topic
int _t_actuator_controls_0; ///< actuator controls group 0 topic
int _t_actuator_controls_1; ///< actuator controls group 1 topic
int _t_actuator_controls_2; ///< actuator controls group 2 topic
int _t_actuator_controls_3; ///< actuator controls group 3 topic
int _t_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode;///< vehicle control mode topic
int _t_param; ///< parameter update topic
@@ -269,15 +272,15 @@ private:
actuator_outputs_s _outputs; ///<mixed outputs
bool _primary_pwm_device; ///<true if we are the default PWM output
bool _primary_pwm_device; ///< true if we are the default PWM output
float _battery_amp_per_volt; ///<current sensor amps/volt
float _battery_amp_bias; ///<current sensor bias
float _battery_mamphour_total;///<amp hours consumed so far
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
float _battery_amp_per_volt; ///< current sensor amps/volt
float _battery_amp_bias; ///< current sensor bias
float _battery_mamphour_total;///< amp hours consumed so far
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
#endif
/**
@@ -291,9 +294,14 @@ private:
void task_main();
/**
* Send controls to IO
* Send controls for one group to IO
*/
int io_set_control_state();
int io_set_control_state(unsigned group);
/**
* Send all controls to IO
*/
int io_set_control_groups();
/**
* Update IO's arming-related state
@@ -467,7 +475,10 @@ PX4IO::PX4IO(device::Device *interface) :
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_status(0),
_alarms(0),
_t_actuators(-1),
_t_actuator_controls_0(-1),
_t_actuator_controls_1(-1),
_t_actuator_controls_2(-1),
_t_actuator_controls_3(-1),
_t_actuator_armed(-1),
_t_vehicle_control_mode(-1),
_t_param(-1),
@@ -769,15 +780,20 @@ PX4IO::task_main()
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */
_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */
_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
orb_set_interval(_t_actuator_controls_2, 33); /* default to 30Hz */
_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
orb_set_interval(_t_actuator_controls_3, 33); /* default to 30Hz */
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
_t_param = orb_subscribe(ORB_ID(parameter_update));
_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
if ((_t_actuators < 0) ||
if ((_t_actuator_controls_0 < 0) ||
(_t_actuator_armed < 0) ||
(_t_vehicle_control_mode < 0) ||
(_t_param < 0) ||
@@ -788,7 +804,7 @@ PX4IO::task_main()
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _t_actuators;
fds[0].fd = _t_actuator_controls_0;
fds[0].events = POLLIN;
log("ready");
@@ -807,7 +823,11 @@ PX4IO::task_main()
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
orb_set_interval(_t_actuator_controls_0, _update_interval);
/*
* NOT changing the rate of groups 1-3 here, because only attitude
* really needs to run fast.
*/
_update_interval = 0;
}
@@ -827,7 +847,10 @@ PX4IO::task_main()
/* if we have new control data from the ORB, handle it */
if (fds[0].revents & POLLIN) {
io_set_control_state();
/* we're not nice to the lower-priority control groups and only check them
when the primary group updated (which is now). */
io_set_control_groups();
}
if (now >= poll_last + IO_POLL_INTERVAL) {
@@ -870,7 +893,7 @@ PX4IO::task_main()
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
// Check for a DSM pairing command
if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) {
if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
dsm_bind_ioctl((int)cmd.param2);
}
}
@@ -937,20 +960,74 @@ out:
}
int
PX4IO::io_set_control_state()
PX4IO::io_set_control_groups()
{
bool attitude_ok = io_set_control_state(0);
/* send auxiliary control groups */
(void)io_set_control_state(1);
(void)io_set_control_state(2);
(void)io_set_control_state(3);
return attitude_ok;
}
int
PX4IO::io_set_control_state(unsigned group)
{
actuator_controls_s controls; ///< actuator outputs
uint16_t regs[_max_actuators];
/* get controls */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &controls);
bool changed;
switch (group) {
case 0:
{
orb_check(_t_actuator_controls_0, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
}
}
break;
case 1:
{
orb_check(_t_actuator_controls_1, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
}
}
break;
case 2:
{
orb_check(_t_actuator_controls_2, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
}
}
break;
case 3:
{
orb_check(_t_actuator_controls_3, &changed);
if (changed) {
orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
}
}
break;
}
if (!changed)
return -1;
for (unsigned i = 0; i < _max_controls; i++)
regs[i] = FLOAT_TO_REG(controls.control[i]);
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
}
src/examples/flow_position_control/flow_position_control_main.c
-1
View File
@@ -61,7 +61,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+72 -35
View File
@@ -45,39 +45,30 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
_max_rate_neg(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
_bodyrate_setpoint(0.0f)
{
}
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
@@ -105,29 +96,72 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
if (inverted)
turn_offset = -turn_offset;
/* Calculate the error */
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
_rate_error = _rate_setpoint - pitch_rate;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
float ilimit_scaled = _integrator_max * scaler;
}
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
return _rate_setpoint;
}
float id = _rate_error * k_i_rate * dt * scaler;
float ECL_PitchController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
/* Transform estimation to body angular rates */
float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_PitchController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+24 -4
View File
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal pitch PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -51,13 +52,18 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_PitchController
class __EXPORT ECL_PitchController //XXX: create controller superclass
{
public:
ECL_PitchController();
float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
float control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -67,21 +73,30 @@ public:
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate_pos(float max_rate_pos) {
_max_rate_pos = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
_max_rate_neg = max_rate_neg;
}
void set_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
@@ -94,6 +109,10 @@ public:
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
@@ -101,6 +120,7 @@ private:
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate_pos;
float _max_rate_neg;
@@ -109,7 +129,7 @@ private:
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
float _bodyrate_setpoint;
};
#endif // ECL_PITCH_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+51 -26
View File
@@ -45,21 +45,47 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
_bodyrate_setpoint(0.0f)
{
}
float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
/* Calculate error */
float roll_error = roll_setpoint - roll;
/* Apply P controller */
_rate_setpoint = roll_error / _tc;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -70,10 +96,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
if (dt_micros > 500000)
lock_integrator = true;
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0; //xxx: param
/* input conditioning */
// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +108,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
airspeed = airspeed_min;
}
float roll_error = roll_setpoint - roll;
_rate_setpoint = roll_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
_rate_error = _rate_setpoint - roll_rate;
/* Transform estimation to body angular rates */
float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
float ilimit_scaled = _integrator_max * scaler;
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -115,11 +137,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_RollController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_roll_controller.h
+23 -4
View File
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal roll PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -51,13 +52,17 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_RollController
class __EXPORT ECL_RollController //XXX: create controller superclass
{
public:
ECL_RollController();
float control(float roll_setpoint, float roll, float roll_rate,
float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
float control_attitude(float roll_setpoint, float roll);
float control_bodyrate(float pitch,
float roll_rate, float yaw_rate,
float yaw_rate_setpoint,
float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -66,18 +71,27 @@ public:
_tc = time_constant;
}
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
@@ -90,19 +104,24 @@ public:
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _k_ff;
float _integrator_max;
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
float _bodyrate_setpoint;
};
#endif // ECL_ROLL_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+108 -10
View File
@@ -44,29 +44,127 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
_last_error(0.0f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_last_rate_hp_out(0.0f),
_last_rate_hp_in(0.0f),
_k_d_last(0.0f),
_integrator(0.0f)
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_coordinated_min_speed(1.0f)
{
}
float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
float airspeed_min, float airspeed_max, float aspeed)
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
if(denumerator != 0.0f) { //XXX: floating point comparison
_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
// if(counter % 20 == 0) {
// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
// }
}
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
// counter++;
if(!isfinite(_rate_setpoint)){
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
}
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
return 0.0f;
// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_ff = 0;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* Transform setpoint to body angular rates */
_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
/* Transform estimation to body angular rates */
float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
//xxx: until start detection is available: integral part in control signal is limited here
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_YawController::reset_integrator()
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+59 -15
View File
@@ -35,6 +35,15 @@
* @file ecl_yaw_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
* The control design is based on a design
* by Paul Riseborough and Andrew Tridgell, 2013,
* which in turn is based on initial work of
* Jonathan Challinger, 2012.
*/
#ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H
@@ -42,47 +51,82 @@
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_YawController
class __EXPORT ECL_YawController //XXX: create controller superclass
{
public:
ECL_YawController();
float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
float control_bodyrate( float roll, float pitch,
float pitch_rate, float yaw_rate,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
void reset_integrator();
void set_k_side(float k_a) {
_k_side = k_a;
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_roll_ff(float k_roll_ff) {
_k_roll_ff = k_roll_ff;
void set_k_ff(float k_ff) {
_k_ff = k_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
void set_k_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
float get_desired_bodyrate() {
return _bodyrate_setpoint;
}
private:
uint64_t _last_run;
float _k_side;
float _k_p;
float _k_i;
float _k_d;
float _k_roll_ff;
float _k_ff;
float _integrator_max;
float _last_error;
float _max_rate;
float _roll_ff;
float _last_output;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _k_d_last;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
float _coordinated_min_speed;
};
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+2 -2
View File
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{
/* following [2], switching on L1 distance */
return math::max(wp_radius, _L1_distance);
return math::min(wp_radius, _L1_distance);
}
bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
*/
// XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
src/lib/external_lgpl/tecs/tecs.cpp
+94 -57
View File
@@ -2,6 +2,7 @@
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
using namespace math;
@@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void)
// calculate velocity rate limits based on physical performance limits
// provision to use a different rate limit if bad descent or underspeed condition exists
// Use 50% of maximum energy rate to allow margin for total energy contgroller
float velRateMax;
float velRateMin;
// float velRateMax;
// float velRateMin;
//
// if ((_badDescent) || (_underspeed)) {
// velRateMax = 0.5f * _STEdot_max / _integ5_state;
// velRateMin = 0.5f * _STEdot_min / _integ5_state;
//
// } else {
// velRateMax = 0.5f * _STEdot_max / _integ5_state;
// velRateMin = 0.5f * _STEdot_min / _integ5_state;
// }
//
// // Apply rate limit
// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
// _TAS_rate_dem = velRateMax;
//
// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
// _TAS_rate_dem = velRateMin;
//
// } else {
// _TAS_dem_adj = _TAS_dem;
//
//
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
// }
if ((_badDescent) || (_underspeed)) {
velRateMax = 0.5f * _STEdot_max / _integ5_state;
velRateMin = 0.5f * _STEdot_min / _integ5_state;
} else {
velRateMax = 0.5f * _STEdot_max / _integ5_state;
velRateMin = 0.5f * _STEdot_min / _integ5_state;
}
// Apply rate limit
if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
_TAS_rate_dem = velRateMax;
} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
_TAS_rate_dem = velRateMin;
} else {
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
}
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
// Constrain speed demand again to protect against bad values on initialisation.
_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
_TAS_dem_last = _TAS_dem;
// _TAS_dem_last = _TAS_dem;
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed);
}
void TECS::_update_height_demand(float demand)
void TECS::_update_height_demand(float demand, float state)
{
// Apply 2 point moving average to demanded height
// This is required because height demand is only updated at 5Hz
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
_hgt_dem_in_old = _hgt_dem;
// // Apply 2 point moving average to demanded height
// // This is required because height demand is only updated at 5Hz
// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
// _hgt_dem_in_old = _hgt_dem;
//
// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
// // _maxClimbRate);
//
// // Limit height rate of change
// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
//
// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
// }
//
// _hgt_dem_prev = _hgt_dem;
//
// // Apply first order lag to height demand
// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
// _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
// _hgt_dem_adj_last = _hgt_dem_adj;
//
// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
// // _hgt_rate_dem);
// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
// _maxClimbRate);
// Limit height rate of change
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
}
_hgt_dem_prev = _hgt_dem;
// Apply first order lag to height demand
_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_dem_adj_last = _hgt_dem_adj;
// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
// _hgt_rate_dem);
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
} else if (_hgt_rate_dem < -_maxSinkRate) {
_hgt_rate_dem = -_maxSinkRate;
}
//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
}
void TECS::_detect_underspeed(void)
@@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0) {
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void)
// 1) Underspeed protection not active
// 2) Specific total energy error > 0
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
float STEdot = _SPEdot + _SKEdot;
// float STEdot = _SPEdot + _SKEdot;
//
// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
//
// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
// _badDescent = true;
//
// } else {
// _badDescent = false;
// }
if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
_badDescent = true;
} else {
_badDescent = false;
}
_badDescent = false;
}
void TECS::_update_pitch(void)
@@ -404,10 +433,18 @@ void TECS::_update_pitch(void)
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem)
{
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;
@@ -500,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
_update_speed_demand();
// Calculate the height demand
_update_height_demand(hgt_dem);
_update_height_demand(hgt_dem, baro_altitude);
// Detect underspeed condition
_detect_underspeed();
src/lib/external_lgpl/tecs/tecs.h
+11 -1
View File
@@ -180,6 +180,14 @@ public:
_indicated_airspeed_max = airspeed;
}
void set_heightrate_p(float heightrate_p) {
_heightrate_p = heightrate_p;
}
void set_speedrate_p(float speedrate_p) {
_speedrate_p = speedrate_p;
}
private:
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -203,6 +211,8 @@ private:
float _vertAccLim;
float _rollComp;
float _spdWeight;
float _heightrate_p;
float _speedrate_p;
// throttle demand in the range from 0.0 to 1.0
float _throttle_dem;
@@ -329,7 +339,7 @@ private:
void _update_speed_demand(void);
// Update the demanded height
void _update_height_demand(float demand);
void _update_height_demand(float demand, float state);
// Detect an underspeed condition
void _detect_underspeed(void);
src/lib/geo/geo.c
+61
View File
@@ -387,6 +387,45 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
return return_value;
}
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
double x_rad = lat_now / 180.0 * M_PI;
double y_rad = lon_now / 180.0 * M_PI;
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
float dz = alt_now - alt_next;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
}
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
float dz = z_now - z_next;
*dist_xy = sqrtf(dx * dx + dy * dy);
*dist_z = fabsf(dz);
return sqrtf(dx * dx + dy * dy + dz * dz);
}
__EXPORT float _wrap_pi(float bearing)
{
/* value is inf or NaN */
@@ -465,4 +504,26 @@ __EXPORT float _wrap_360(float bearing)
return bearing;
}
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence)
{
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
unsigned int i, j, vertices = fence->count;
bool c = false;
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
// skip vertex 0 (return point)
for (i = 0, j = vertices - 1; i < vertices; j = i++)
if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) &&
(lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) /
(fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat))
c = !c;
return c;
}
src/lib/geo/geo.h
+28
View File
@@ -47,6 +47,9 @@
#pragma once
#include "uORB/topics/fence.h"
#include "uORB/topics/vehicle_global_position.h"
__BEGIN_DECLS
#include <stdbool.h>
@@ -121,9 +124,34 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame
*/
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z);
/*
* Calculate distance in local frame (NED)
*/
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
__EXPORT float _wrap_pi(float bearing);
__EXPORT float _wrap_2pi(float bearing);
/**
* Return whether craft is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
*/
__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence);
__END_DECLS
src/modules/commander/commander.cpp
+35 -14
View File
@@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
/* reject parachute depoyment not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
default:
break;
}
@@ -1033,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[])
if (!home_position_set && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
(hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
&& global_position.valid) {
/* copy position data to uORB home message, store it locally as well */
// TODO use global position estimate
home.lat = gps_position.lat;
home.lon = gps_position.lon;
home.alt = gps_position.alt;
home.eph_m = gps_position.eph_m;
home.epv_m = gps_position.epv_m;
home.s_variance_m_s = gps_position.s_variance_m_s;
home.p_variance_m = gps_position.p_variance_m;
home.lat = (double)global_position.lat / 1e7d;
home.lon = (double)global_position.lon / 1e7d;
home.altitude = (float)global_position.alt;
double home_lat_d = home.lat * 1e-7;
double home_lon_d = home.lon * 1e-7;
warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
/* announce new home position */
if (home_pub > 0) {
@@ -1174,6 +1183,16 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode);
if (flighttermination_res == TRANSITION_CHANGED) {
tune_positive();
}
} else {
flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode);
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1199,6 +1218,7 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool navigation_state_changed = check_navigation_state_changed();
bool flighttermination_state_changed = check_flighttermination_state_changed();
hrt_abstime t1 = hrt_absolute_time();
@@ -1725,7 +1745,8 @@ void *commander_low_prio_loop(void *arg)
/* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == VEHICLE_CMD_DO_SET_SERVO)
continue;
/* only handle low-priority commands here */
src/modules/commander/state_machine_helper.cpp
+54 -2
View File
@@ -65,6 +65,7 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool navigation_state_changed = true;
static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -239,8 +240,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT:
/* need at minimum altitude estimate */
if (current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid) {
if (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
@@ -450,6 +452,18 @@ check_navigation_state_changed()
}
}
bool
check_flighttermination_state_changed()
{
if (flighttermination_state_changed) {
flighttermination_state_changed = false;
return true;
} else {
return false;
}
}
void
set_navigation_state_changed()
{
@@ -522,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
}
/**
* Transition from one flightermination state to another
*/
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_flighttermination_state == status->flighttermination_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_flighttermination_state) {
case FLIGHTTERMINATION_STATE_ON:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
break;
case FLIGHTTERMINATION_STATE_OFF:
ret = TRANSITION_CHANGED;
status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
flighttermination_state_changed = true;
control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
}
return ret;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
src/modules/commander/state_machine_helper.h
+4
View File
@@ -70,8 +70,12 @@ bool check_main_state_changed();
transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode);
bool check_navigation_state_changed();
bool check_flighttermination_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
src/modules/controllib/uorb/blocks.cpp
+9 -9
View File
@@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
missionCmd.lat,
missionCmd.lon);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
lastMissionCmd.lat,
lastMissionCmd.lon,
missionCmd.lat,
missionCmd.lon);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
@@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
_missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
src/modules/controllib/uorb/blocks.hpp
+4 -4
View File
@@ -43,7 +43,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
@@ -82,8 +82,8 @@ public:
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
mission_item_s &missionCmd,
mission_item_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};
@@ -98,7 +98,7 @@ protected:
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
UOrbSubscription<mission_item_triplet_s> _missionCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
src/modules/dataman/dataman.c
+741
View File
File diff suppressed because it is too large Load Diff
src/modules/dataman/dataman.h
+119
View File
@@ -0,0 +1,119 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file dataman.h
*
* DATAMANAGER driver.
*/
#ifndef _DATAMANAGER_H
#define _DATAMANAGER_H
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Types of items that the data manager can store */
typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
/* The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
};
/* Data persistence levels */
typedef enum {
DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */
DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */
DM_PERSIST_VOLATILE /* Data does not survive resets */
} dm_persitence_t;
/* The reason for the last reset */
typedef enum {
DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */
DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */
} dm_reset_reason;
/* Maximum size in bytes of a single item instance */
#define DM_MAX_DATA_SIZE 126
/* Retrieve from the data manager store */
__EXPORT ssize_t
dm_read(
dm_item_t item, /* The item type to retrieve */
unsigned char index, /* The index of the item */
void *buffer, /* Pointer to caller data buffer */
size_t buflen /* Length in bytes of data to retrieve */
);
/* write to the data manager store */
__EXPORT ssize_t
dm_write(
dm_item_t item, /* The item type to store */
unsigned char index, /* The index of the item */
dm_persitence_t persistence, /* The persistence level of this item */
const void *buffer, /* Pointer to caller data buffer */
size_t buflen /* Length in bytes of data to retrieve */
);
/* Retrieve from the data manager store */
__EXPORT int
dm_clear(
dm_item_t item /* The item type to clear */
);
/* Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(
dm_reset_reason restart_type /* The last reset type */
);
#ifdef __cplusplus
}
#endif
#endif
src/modules/dataman/module.mk
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Main Navigation Controller
#
MODULE_COMMAND = dataman
SRCS = dataman.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/fixedwing_backside/fixedwing.cpp
+5 -5
View File
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_vCmd(this, "V_CMD"),
_crMax(this, "CR_MAX"),
_attPoll(),
_lastPosCmd(),
_lastMissionCmd(),
_timeStamp(0)
{
_attPoll.fd = _att.getHandle();
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
if (_missionCmd.updated()) {
_lastMissionCmd = _missionCmd.getData();
}
// check for new updates
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
if (_status.main_state == MAIN_STATE_AUTO) {
// TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
}
// XXX handle STABILIZED (loiter on spot) as well
@@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
src/modules/fixedwing_backside/fixedwing.hpp
+1 -1
View File
@@ -264,7 +264,7 @@ private:
BlockParamFloat _crMax;
struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd;
mission_item_triplet_s _lastMissionCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
src/modules/fw_att_control/fw_att_control_main.cpp
+202 -73
View File
@@ -37,6 +37,7 @@
* Implementation of a generic attitude controller based on classic orthogonal PIDs.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
*/
@@ -62,6 +63,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -106,26 +108,30 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -137,6 +143,7 @@ private:
float p_p;
float p_d;
float p_i;
float p_ff;
float p_rmax_pos;
float p_rmax_neg;
float p_integrator_max;
@@ -144,13 +151,17 @@ private:
float r_p;
float r_d;
float r_i;
float r_ff;
float r_integrator_max;
float r_rmax;
float y_p;
float y_i;
float y_d;
float y_ff;
float y_roll_feedforward;
float y_integrator_max;
float y_coordinated_min_speed;
float y_rmax;
float airspeed_min;
float airspeed_trim;
@@ -163,6 +174,7 @@ private:
param_t p_p;
param_t p_d;
param_t p_i;
param_t p_ff;
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
@@ -170,13 +182,17 @@ private:
param_t r_p;
param_t r_d;
param_t r_i;
param_t r_ff;
param_t r_integrator_max;
param_t r_rmax;
param_t y_p;
param_t y_i;
param_t y_d;
param_t y_ff;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t y_coordinated_min_speed;
param_t y_rmax;
param_t airspeed_min;
param_t airspeed_trim;
@@ -226,6 +242,11 @@ private:
*/
void vehicle_setpoint_poll();
/**
* Check for global position updates.
*/
void global_pos_poll();
/**
* Shim for calling task_main from task_create.
*/
@@ -261,11 +282,13 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
_global_pos_sub(-1),
/* publications */
_rate_sp_pub(-1),
_actuators_0_pub(-1),
_attitude_sp_pub(-1),
_actuators_1_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -273,31 +296,49 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_setpoint_valid(false),
_airspeed_valid(false)
{
/* safely initialize structs */
_att = {0};
_accel = {0};
_att_sp = {0};
_manual = {0};
_airspeed = {0};
_vcontrol_mode = {0};
_actuators = {0};
_actuators_airframe = {0};
_global_pos = {0};
_parameter_handles.tconst = param_find("FW_ATT_TC");
_parameter_handles.p_p = param_find("FW_P_P");
_parameter_handles.p_d = param_find("FW_P_D");
_parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_d = param_find("FW_PR_D");
_parameter_handles.p_i = param_find("FW_PR_I");
_parameter_handles.p_ff = param_find("FW_PR_FF");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I");
_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
_parameter_handles.r_p = param_find("FW_RR_P");
_parameter_handles.r_d = param_find("FW_RR_D");
_parameter_handles.r_i = param_find("FW_RR_I");
_parameter_handles.r_ff = param_find("FW_RR_FF");
_parameter_handles.r_integrator_max = param_find("FW_RR_IMAX");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_p = param_find("FW_YR_P");
_parameter_handles.y_i = param_find("FW_YR_I");
_parameter_handles.y_d = param_find("FW_YR_D");
_parameter_handles.y_ff = param_find("FW_YR_FF");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
/* fetch initial parameter values */
parameters_update();
}
@@ -335,6 +376,7 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_d, &(_parameters.p_d));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
@@ -343,14 +385,19 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_d, &(_parameters.r_d));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_d, &(_parameters.y_d));
param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@@ -358,28 +405,33 @@ FixedwingAttitudeControl::parameters_update()
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
_pitch_ctrl.set_k_p(_parameters.p_p);
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_d(_parameters.p_d);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
_roll_ctrl.set_k_p(_parameters.r_p);
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_d(_parameters.r_d);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_d(_parameters.y_d);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
return OK;
}
@@ -421,6 +473,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
return true;
}
@@ -452,6 +505,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
}
}
void
FixedwingAttitudeControl::global_pos_poll()
{
/* check if there is a new global position */
bool global_pos_updated;
orb_check(_global_pos_sub, &global_pos_updated);
if (global_pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
}
void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@@ -476,6 +541,7 @@ FixedwingAttitudeControl::task_main()
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
@@ -551,6 +617,8 @@ FixedwingAttitudeControl::task_main()
vehicle_manual_poll();
global_pos_poll();
/* lock integrator until control is started */
bool lock_integrator;
@@ -561,22 +629,28 @@ FixedwingAttitudeControl::task_main()
lock_integrator = true;
}
/* Simple handling of failsafe: deploy parachute if failsafe is on */
if (_vcontrol_mode.flag_control_flighttermination_enabled) {
_actuators_airframe.control[1] = 1.0f;
// warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[1] = 0.0f;
// warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale from radians to normalized -1 .. 1 range */
const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if (!_airspeed_valid ||
(_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
(_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s)) {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
airspeed = _parameters.airspeed_trim;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
@@ -585,7 +659,8 @@ FixedwingAttitudeControl::task_main()
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
float roll_sp, pitch_sp;
float roll_sp = 0.0f;
float pitch_sp = 0.0f;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
@@ -635,46 +710,86 @@ FixedwingAttitudeControl::task_main()
}
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
} else {
warnx("Did not get a valid R\n");
}
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
_roll_ctrl.control_attitude(roll_sp, _att.roll);
_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
/* Run attitude RATE controllers which need the desired attitudes from above */
float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
if (!isfinite(roll_u)) {
warnx("roll_u %.4f", roll_u);
}
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
}
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u);
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
if (!isfinite(throttle_sp)) {
warnx("throttle_sp %.4f", throttle_sp);
}
} else {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
}
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = _yaw_ctrl.get_desired_rate();
rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
} else {
@@ -692,6 +807,7 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
/* publish the attitude setpoint */
@@ -702,6 +818,19 @@ FixedwingAttitudeControl::task_main()
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
if (_actuators_1_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
} else {
/* advertise and publish */
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
}
}
perf_end(_loop_perf);
src/modules/fw_att_control/fw_att_control_params.c
+25 -17
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -44,11 +44,13 @@
#include <systemlib/param/param.h>
//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P
/*
* Controller parameters, accessible via MAVLink
*
*/
//xxx: update descriptions
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Range 0.4 to 1.0 seconds, in tens of seconds
@@ -57,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Proportional gain.
// @Description This defines how much the elevator input will be commanded dependend on the current pitch error.
// @Range 10 to 200, 1 increments
PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f);
// @DisplayName Damping gain.
// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings.
// @Range 0.0 to 10.0, 0.1 increments
PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove
// @DisplayName Integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f);
// @DisplayName Maximum positive / up pitch rate.
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
// @DisplayName Maximum negative / down pitch rate.
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
// @DisplayName Pitch Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @DisplayName Roll feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
@@ -97,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
// @Range 10.0 200.0
// @Increment 10.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f);
// @DisplayName Damping Gain
// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
// @Range 0.0 10.0
// @Increment 1.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove
// @DisplayName Integrator Gain
// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
// @Range 0.0 100.0
// @Increment 5.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f);
// @DisplayName Roll Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
@@ -126,11 +128,17 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_Y_P, 0);
PARAM_DEFINE_FLOAT(FW_Y_I, 0);
PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_Y_D, 0);
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
PARAM_DEFINE_FLOAT(FW_YR_P, 0.5);
PARAM_DEFINE_FLOAT(FW_YR_I, 0.05);
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f);
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f);
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+361 -259
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+9
View File
@@ -111,3 +111,12 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
src/modules/fw_pos_control_l1/landingslope.cpp
+82
View File
@@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file: landingslope.cpp
*
*/
#include "landingslope.h"
#include <nuttx/config.h>
#include <stdlib.h>
#include <errno.h>
#include <math.h>
#include <unistd.h>
#include <mathlib/mathlib.h>
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float H1_virt)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
_H1_virt = H1_virt;
calculateSlopeValues();
}
void Landingslope::calculateSlopeValues()
{
_H0 = _flare_relative_alt + _H1_virt;
_d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad);
_flare_constant = (_H0 * _d1)/_flare_relative_alt;
_flare_length = - logf(_H1_virt/_H0) * _flare_constant;
_horizontal_slope_displacement = (_flare_length - _d1);
}
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
{
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
{
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
}
src/modules/fw_pos_control_l1/landingslope.h
+109
View File
@@ -0,0 +1,109 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file: landingslope.h
*
*/
#ifndef LANDINGSLOPE_H_
#define LANDINGSLOPE_H_
#include <math.h>
#include <systemlib/err.h>
class Landingslope
{
private:
//xxx: documentation of landing pending
float _landing_slope_angle_rad;
float _flare_relative_alt;
float _motor_lim_horizontal_distance;
float _H1_virt;
float _H0;
float _d1;
float _flare_constant;
float _flare_length;
float _horizontal_slope_displacement;
void calculateSlopeValues();
public:
Landingslope() {}
~Landingslope() {}
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
}
/**
*
* @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
*/
__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
}
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float H1_virt);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
inline float flare_relative_alt() {return _flare_relative_alt;}
inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
inline float H1_virt() {return _H1_virt;}
inline float H0() {return _H0;}
inline float d1() {return _d1;}
inline float flare_constant() {return _flare_constant;}
inline float flare_length() {return _flare_length;}
inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;}
};
#endif /* LANDINGSLOPE_H_ */
src/modules/fw_pos_control_l1/module.mk
+2 -1
View File
@@ -38,4 +38,5 @@
MODULE_COMMAND = fw_pos_control_l1
SRCS = fw_pos_control_l1_main.cpp \
fw_pos_control_l1_params.c
fw_pos_control_l1_params.c \
landingslope.cpp
src/modules/mavlink/mavlink.c
+17
View File
@@ -74,6 +74,8 @@
#include "waypoints.h"
#include "mavlink_parameters.h"
#include <uORB/topics/mission_result.h>
/* define MAVLink specific parameters */
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
@@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
thread_running = true;
/* arm counter to go off immediately */
@@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[])
lowspeed_counter++;
bool updated;
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
}
}
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
/* sleep quarter the time */
src/modules/mavlink/missionlib.c
+49 -40
View File
@@ -73,11 +73,15 @@
#include "waypoints.h"
#include "mavlink_parameters.h"
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
static uint64_t loiter_start_time;
#if 0
static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
struct vehicle_global_position_setpoint_s *sp);
#endif
int
mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -88,6 +92,8 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
return 0;
}
int
mavlink_missionlib_send_gcs_string(const char *string)
{
@@ -127,6 +133,7 @@ uint64_t mavlink_missionlib_get_system_timestamp()
return hrt_absolute_time();
}
#if 0
/**
* Set special vehicle setpoint fields based on current mission item.
*
@@ -206,7 +213,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
{
static orb_advert_t global_position_setpoint_pub = -1;
static orb_advert_t global_position_set_triplet_pub = -1;
// static orb_advert_t global_position_set_triplet_pub = -1;
static orb_advert_t local_position_setpoint_pub = -1;
static unsigned last_waypoint_index = -1;
char buf[50] = {0};
@@ -234,10 +241,10 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
/* fill triplet: previous, current, next waypoint */
struct vehicle_global_position_set_triplet_s triplet;
// struct vehicle_global_position_set_triplet_s triplet;
/* current waypoint is same as sp */
memcpy(&(triplet.current), &sp, sizeof(sp));
// memcpy(&(triplet.current), &sp, sizeof(sp));
/*
* Check if previous WP (in mission, not in execution order)
@@ -291,48 +298,48 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
/* populate last and next */
triplet.previous_valid = false;
triplet.next_valid = false;
// triplet.previous_valid = false;
// triplet.next_valid = false;
if (last_setpoint_valid) {
triplet.previous_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
wpm->waypoints[last_setpoint_index].param4,
wpm->waypoints[last_setpoint_index].command, &sp);
memcpy(&(triplet.previous), &sp, sizeof(sp));
}
// if (last_setpoint_valid) {
// triplet.previous_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[last_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[last_setpoint_index].param1,
// wpm->waypoints[last_setpoint_index].param2,
// wpm->waypoints[last_setpoint_index].param3,
// wpm->waypoints[last_setpoint_index].param4,
// wpm->waypoints[last_setpoint_index].command, &sp);
// memcpy(&(triplet.previous), &sp, sizeof(sp));
// }
if (next_setpoint_valid) {
triplet.next_valid = true;
struct vehicle_global_position_setpoint_s sp;
sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
wpm->waypoints[next_setpoint_index].param4,
wpm->waypoints[next_setpoint_index].command, &sp);
memcpy(&(triplet.next), &sp, sizeof(sp));
}
// if (next_setpoint_valid) {
// triplet.next_valid = true;
// struct vehicle_global_position_setpoint_s sp;
// sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
// sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
// sp.altitude = wpm->waypoints[next_setpoint_index].z;
// sp.altitude_is_relative = false;
// sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
// set_special_fields(wpm->waypoints[next_setpoint_index].param1,
// wpm->waypoints[next_setpoint_index].param2,
// wpm->waypoints[next_setpoint_index].param3,
// wpm->waypoints[next_setpoint_index].param4,
// wpm->waypoints[next_setpoint_index].command, &sp);
// memcpy(&(triplet.next), &sp, sizeof(sp));
// }
/* Initialize triplet publication if necessary */
if (global_position_set_triplet_pub < 0) {
global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
// if (global_position_set_triplet_pub < 0) {
// global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
} else {
orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
}
// } else {
// orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
// }
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
@@ -388,3 +395,5 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
printf("%s\n", buf);
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
}
#endif
src/modules/mavlink/orb_listener.c
+17 -16
View File
@@ -136,7 +136,7 @@ static const struct listener listeners[] = {
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
{l_global_position, &mavlink_subs.global_pos_sub, 0},
{l_local_position, &mavlink_subs.local_pos_sub, 0},
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
{l_global_position_setpoint, &mavlink_subs.triplet_sub, 0},
{l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
{l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
{l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
@@ -402,23 +402,24 @@ l_local_position(const struct listener *l)
void
l_global_position_setpoint(const struct listener *l)
{
struct vehicle_global_position_setpoint_s global_sp;
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
struct mission_item_triplet_s triplet;
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (!triplet.current_valid)
return;
if (global_sp.altitude_is_relative)
if (triplet.current.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
coordinate_frame,
global_sp.lat,
global_sp.lon,
global_sp.altitude * 1000.0f,
global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
(int32_t)(triplet.current.altitude * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
void
@@ -499,8 +500,8 @@ l_actuator_outputs(const struct listener *l)
act_outputs.output[6],
act_outputs.output[7]);
/* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) {
/* only send in HIL mode and only send first group for HIL */
if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -656,7 +657,7 @@ l_home(const struct listener *l)
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
}
void
@@ -770,9 +771,9 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */
src/modules/mavlink/orb_topics.h
+3 -3
View File
@@ -50,9 +50,9 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -86,7 +86,7 @@ struct mavlink_subscriptions {
int local_pos_sub;
int spa_sub;
int spl_sub;
int spg_sub;
int triplet_sub;
int debug_key_value;
int input_rc_sub;
int optical_flow;
src/modules/mavlink/waypoints.c
+464 -325
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/waypoints.h
+20 -14
View File
@@ -56,6 +56,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/mission.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
@@ -81,7 +82,7 @@ enum MAVLINK_WPM_CODES {
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 15
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
#endif
@@ -92,33 +93,38 @@ enum MAVLINK_WPM_CODES {
struct mavlink_wpm_storage {
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
#endif
// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
// #endif
uint16_t size;
uint16_t max_size;
uint16_t rcv_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
// int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_firstinside_orbit;
uint64_t timestamp_lastoutside_orbit;
// uint64_t timestamp_last_send_setpoint;
// uint64_t timestamp_firstinside_orbit;
// uint64_t timestamp_lastoutside_orbit;
uint32_t timeout;
uint32_t delay_setpoint;
float accept_range_yaw;
float accept_range_distance;
bool yaw_reached;
bool pos_reached;
bool idle;
int current_dataman_id;
// uint32_t delay_setpoint;
// float accept_range_yaw;
// float accept_range_distance;
// bool yaw_reached;
// bool pos_reached;
// bool idle;
};
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void mavlink_wpm_init(mavlink_wpm_storage *state);
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
src/modules/mavlink_onboard/orb_topics.h
+1 -1
View File
@@ -50,7 +50,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
src/modules/multirotor_pos_control/multirotor_pos_control.c
+685
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/mission_feasibility_checker.cpp
+184
View File
@@ -0,0 +1,184 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_feasibility_checker.cpp
* Provides checks if mission is feasible given the navigation capabilities
*/
#include "mission_feasibility_checker.h"
#include <geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <fw_pos_control_l1/landingslope.h>
#include <systemlib/err.h>
#include <stdio.h>
#include <fcntl.h>
#include <errno.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
{
_nav_caps = {0};
}
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
{
/* Init if not done yet */
init();
/* Open mavlink fd */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
if (isRotarywing)
return checkMissionFeasibleRotarywing(dm_current, nItems);
else
return checkMissionFeasibleFixedwing(dm_current, nItems);
}
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
{
return checkGeofence(dm_current, nItems);
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
{
//xxx: check geofence
return true;
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
for (size_t i = 0; i < nItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
struct mission_item_s missionitem_previous;
if (i != 0) {
if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
if (wp_distance > _nav_caps.landing_flare_length) {
/* Last wp is before flare region */
if (delta_altitude < 0) {
if (missionitem_previous.altitude <= slope_alt_req) {
/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
return false;
}
} else {
mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
return false;
}
}
}
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
void MissionFeasibilityChecker::init()
{
if (!_initDone) {
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_initDone = true;
}
}
src/modules/navigator/mission_feasibility_checker.h
+82
View File
@@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_feasibility_checker.h
* Provides checks if mission is feasible given the navigation capabilities
*/
#ifndef MISSION_FEASIBILITY_CHECKER_H_
#define MISSION_FEASIBILITY_CHECKER_H_
#include <unistd.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/navigation_capabilities.h>
#include <dataman/dataman.h>
class MissionFeasibilityChecker
{
private:
int _mavlink_fd;
int _capabilities_sub;
struct navigation_capabilities_s _nav_caps;
bool _initDone;
void init();
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nItems);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
void updateNavigationCapabilities();
/* Checks specific to rotarywing airframes */
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
public:
MissionFeasibilityChecker();
~MissionFeasibilityChecker() {}
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
};
#endif /* MISSION_FEASIBILITY_CHECKER_H_ */
src/modules/navigator/module.mk
+5 -1
View File
@@ -38,4 +38,8 @@
MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c
navigator_params.c \
navigator_mission.cpp \
mission_feasibility_checker.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/navigator/navigator_main.cpp
+991 -268
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/navigator_mission.cpp
+257
View File
@@ -0,0 +1,257 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
* Helper class to access missions
*/
// #include <stdio.h>
// #include <stdlib.h>
// #include <string.h>
// #include <unistd.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Mission::Mission() :
_offboard_dataman_id(-1),
_current_offboard_mission_index(0),
_current_onboard_mission_index(0),
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE)
{}
Mission::~Mission()
{
}
void
Mission::set_offboard_dataman_id(int new_id)
{
_offboard_dataman_id = new_id;
}
void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_offboard_mission_index = (unsigned)new_index;
}
}
void
Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
}
}
void
Mission::set_offboard_mission_count(unsigned new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(unsigned new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::current_mission_available()
{
return (current_onboard_mission_available() || current_offboard_mission_available());
}
bool
Mission::next_mission_available()
{
return (next_onboard_mission_available() || next_offboard_mission_available());
}
int
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
{
/* try onboard mission first */
if (current_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
} else {
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
return OK;
}
int
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
/* try onboard mission first */
if (next_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
} else {
/* happens when no more mission items can be added as a next item */
return ERROR;
}
return OK;
}
bool
Mission::current_onboard_mission_available()
{
return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
}
bool
Mission::current_offboard_mission_available()
{
return _offboard_mission_item_count > _current_offboard_mission_index;
}
bool
Mission::next_onboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_ONBOARD) {
next = 1;
}
return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
}
bool
Mission::next_offboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
next = 1;
}
return _offboard_mission_item_count > (_current_offboard_mission_index + next);
}
void
Mission::move_to_next()
{
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}

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