fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master' into obcfailsafe

Browse Source
This commit is contained in:
Thomas Gubler 2014-08-12 12:30:20 +02:00
commit 017f82e2b8
31 changed files with 634 additions and 421 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ROMFS/px4fmu_common/init.d/3035_viper Normal file
View File

@ -0,0 +1,10 @@
#!nsh
#
# Viper
#
# Simon Wilks <sjwilks@gmail.com>
#
sh /etc/init.d/rc.fw_defaults
set MIXER Viper

5
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -103,6 +103,11 @@ then
sh /etc/init.d/3034_fx79
fi
if param compare SYS_AUTOSTART 3035 35
then
sh /etc/init.d/3035_viper
fi
if param compare SYS_AUTOSTART 3100
then
sh /etc/init.d/3100_tbs_caipirinha

13
ROMFS/px4fmu_common/init.d/rcS
View File

@ -83,9 +83,12 @@ then
param select $PARAM_FILE
if param load
then
echo "[init] Params loaded: $PARAM_FILE"
echo "[param] Loaded: $PARAM_FILE"
else
echo "[init] ERROR: Params loading failed: $PARAM_FILE"
echo "[param] FAILED loading $PARAM_FILE"
if param reset
then
fi
fi
#
@ -280,9 +283,11 @@ then
nshterm /dev/ttyACM0 &
#
# Start the datamanager
# Start the datamanager (and do not abort boot if it fails)
#
dataman start
if dataman start
then
fi
#
# Start the Commander (needs to be this early for in-air-restarts)

4
ROMFS/px4fmu_common/mixers/FMU_FX79.mix
View File

@ -27,12 +27,12 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 8000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 8000 5000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2

72
ROMFS/px4fmu_common/mixers/Viper.mix Executable file
View File

@ -0,0 +1,72 @@
Viper Delta-wing mixer
=================================
Designed for Viper.
TODO (sjwilks): Add mixers for flaps.
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 7500 7500 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000

2
Tools/px_uploader.py
View File

@ -180,7 +180,7 @@ class uploader(object):
def __init__(self, portname, baudrate):
# open the port, keep the default timeout short so we can poll quickly
self.port = serial.Serial(portname, baudrate, timeout=0.5)
self.port = serial.Serial(portname, baudrate, timeout=2.0)
self.otp = b''
self.sn = b''

2
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9
Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742

2
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -103,7 +103,7 @@
/* Measurement rate is 100Hz */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.5f
#define MEAS_DRIVER_FILTER_FREQ 1.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed

47
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem = _throttle_dem;
}
// Ensure _last_throttle_dem is always initialized properly
// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
// // Write internal variables to the log_tuning structure. This
// // structure will be logged in dataflash at 10Hz
// log_tuning.hgt_dem = _hgt_dem_adj;
// log_tuning.hgt = _integ3_state;
// log_tuning.dhgt_dem = _hgt_rate_dem;
// log_tuning.dhgt = _integ2_state;
// log_tuning.spd_dem = _TAS_dem_adj;
// log_tuning.spd = _integ5_state;
// log_tuning.dspd = _vel_dot;
// log_tuning.ithr = _integ6_state;
// log_tuning.iptch = _integ7_state;
// log_tuning.thr = _throttle_dem;
// log_tuning.ptch = _pitch_dem;
// log_tuning.dspd_dem = _TAS_rate_dem;
_tecs_state.timestamp = now;
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
}

72
src/lib/external_lgpl/tecs/tecs.h
View File

@ -28,6 +28,27 @@ class __EXPORT TECS
{
public:
TECS() :
_tecs_state {},
_update_50hz_last_usec(0),
_update_speed_last_usec(0),
_update_pitch_throttle_last_usec(0),
// TECS tuning parameters
_hgtCompFiltOmega(0.0f),
_spdCompFiltOmega(0.0f),
_maxClimbRate(2.0f),
_minSinkRate(1.0f),
_maxSinkRate(2.0f),
_timeConst(5.0f),
_timeConstThrot(8.0f),
_ptchDamp(0.0f),
_thrDamp(0.0f),
_integGain(0.0f),
_vertAccLim(0.0f),
_rollComp(0.0f),
_spdWeight(0.5f),
_heightrate_p(0.0f),
_speedrate_p(0.0f),
_throttle_dem(0.0f),
_pitch_dem(0.0f),
_integ1_state(0.0f),
_integ2_state(0.0f),
@ -100,29 +121,42 @@ public:
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
};
// struct PACKED log_TECS_Tuning {
// LOG_PACKET_HEADER;
// float hgt;
// float dhgt;
// float hgt_dem;
// float dhgt_dem;
// float spd_dem;
// float spd;
// float dspd;
// float ithr;
// float iptch;
// float thr;
// float ptch;
// float dspd_dem;
// } log_tuning;
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
state = _tecs_state;
}
void set_time_const(float time_const) {
_timeConst = time_const;
}
void set_time_const_throt(float time_const_throt) {
_timeConstThrot = time_const_throt;
}
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
@ -188,6 +222,9 @@ public:
}
private:
struct tecs_state _tecs_state;
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@ -204,6 +241,7 @@ private:
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
float _timeConstThrot;
float _ptchDamp;
float _thrDamp;
float _integGain;

18
src/modules/commander/commander.cpp
View File

@ -392,7 +392,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@ -1092,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@ -1319,14 +1319,14 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -1340,7 +1340,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@ -1399,7 +1399,7 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -1425,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@ -2195,7 +2195,7 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@ -2258,7 +2258,7 @@ void *commander_low_prio_loop(void *arg)
tune_negative(true);
}
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}

10
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",

13
src/modules/commander/state_machine_helper.cpp
View File

@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
const struct safety_s *safety, /// current safety settings
arming_state_t new_arming_state, /// arming state requested
struct actuator_armed_s *armed, /// current armed status
const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
const struct safety_s *safety, ///< current safety settings
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}

2
src/modules/commander/state_machine_helper.h
View File

@ -57,7 +57,7 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);

31
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
if (current_ekf_state.velHealth || staticMode) {
current_ekf_state.velHealth = true;
current_ekf_state.velFailTime = millis();
} else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
// XXX check
current_ekf_state.velHealth = true;
ResetVelocity();
ResetStoredStates();
// do not fuse bad data
fuseVelData = false;
}
else
{
@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
{
current_ekf_state.posHealth = true;
current_ekf_state.posFailTime = millis();
if (current_ekf_state.posTimeout) {
ResetPosition();
// XXX cross-check the state reset
ResetStoredStates();
// do not fuse position data on this time
// step
fusePosData = false;
}
}
else
{
@ -1070,10 +1087,18 @@ void AttPosEKF::FuseVelposNED()
// apply a 10-sigma threshold
current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
{
current_ekf_state.hgtHealth = true;
current_ekf_state.hgtFailTime = millis();
// if we just reset from a timeout, do not fuse
// the height data, but reset height and stored states
if (current_ekf_state.hgtTimeout) {
ResetHeight();
ResetStoredStates();
fuseHgtData = false;
}
}
else
{

63
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -146,6 +146,7 @@ private:
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
@ -203,9 +204,11 @@ private:
float l1_damping;
float time_const;
float time_const_throt;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float climbout_diff;
float heightrate_p;
float speedrate_p;
float throttle_damp;
@ -227,6 +230,7 @@ private:
float throttle_min;
float throttle_max;
float throttle_cruise;
float throttle_slew_max;
float throttle_land_max;
@ -245,9 +249,11 @@ private:
param_t l1_damping;
param_t time_const;
param_t time_const_throt;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t climbout_diff;
param_t heightrate_p;
param_t speedrate_p;
param_t throttle_damp;
@ -269,6 +275,7 @@ private:
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_slew_max;
param_t throttle_land_max;
@ -408,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* publications */
_attitude_sp_pub(-1),
_tecs_status_pub(-1),
_nav_capabilities_pub(-1),
/* states */
@ -460,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.roll_limit = param_find("FW_R_LIM");
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
@ -471,9 +480,11 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
@ -532,10 +543,12 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
@ -547,6 +560,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
@ -564,9 +578,11 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_time_const_throt(_parameters.time_const_throt);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_throttle_slewrate(_parameters.throttle_slew_max);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
@ -1095,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@ -1370,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
struct TECS::tecs_state s;
_tecs.get_tecs_state(s);
struct tecs_status_s t;
t.timestamp = s.timestamp;
switch (s.mode) {
case TECS::ECL_TECS_MODE_NORMAL:
t.mode = TECS_MODE_NORMAL;
break;
case TECS::ECL_TECS_MODE_UNDERSPEED:
t.mode = TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
t.mode = TECS_MODE_BAD_DESCENT;
break;
case TECS::ECL_TECS_MODE_CLIMBOUT:
t.mode = TECS_MODE_CLIMBOUT;
break;
}
t.altitudeSp = s.hgt_dem;
t.altitude_filtered = s.hgt;
t.airspeedSp = s.spd_dem;
t.airspeed_filtered = s.spd;
t.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
if (_tecs_status_pub > 0) {
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
} else {
_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
}
}
}

60
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
/**
* Throttle max slew rate
*
* Maximum slew rate for the commanded throttle
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
/**
* Negative pitch limit
*
@ -154,6 +165,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Climbout Altitude difference
*
* If the altitude error exceeds this parameter, the system will climb out
* with maximum throttle and minimum airspeed until it is closer than this
* distance to the desired altitude. Mostly used for takeoff waypoints / modes.
* Set to zero to disable climbout mode (not recommended).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
/**
* Maximum climb rate
*
@ -181,7 +204,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
@ -194,7 +217,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
@ -205,17 +228,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* TECS Throttle time constant
*
* This is the time constant of the TECS throttle control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
@ -227,7 +261,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
@ -240,7 +274,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
@ -253,7 +287,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
@ -266,7 +300,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
@ -282,7 +316,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
@ -300,7 +334,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
@ -312,21 +346,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group L1 Control
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);

7
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -76,7 +76,6 @@ mTecs::mTecs() :
_counter(0),
_debug(false)
{
warnx("starting");
}
mTecs::~mTecs()
@ -110,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
_status.altitude_filtered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */
@ -147,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
_status.airspeed_filtered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */

48
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -35,6 +35,7 @@
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/stat.h>
#include "mavlink_ftp.h"
@ -190,6 +191,8 @@ void
MavlinkFTP::_reply(Request *req)
{
auto hdr = req->header();
hdr->magic = kProtocolMagic;
// message is assumed to be already constructed in the request buffer, so generate the CRC
hdr->crc32 = 0;
@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req)
break;
}
// name too big to fit?
if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
break;
}
uint32_t fileSize = 0;
char buf[256];
// store the type marker
switch (entry.d_type) {
case DTYPE_FILE:
hdr->data[offset++] = kDirentFile;
snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
struct stat st;
if (stat(buf, &st) == 0) {
fileSize = st.st_size;
}
break;
case DTYPE_DIRECTORY:
hdr->data[offset++] = kDirentDir;
@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req)
hdr->data[offset++] = kDirentUnknown;
break;
}
if (entry.d_type == DTYPE_FILE) {
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
} else {
strncpy(buf, entry.d_name, sizeof(buf));
buf[sizeof(buf)-1] = 0;
}
size_t nameLen = strlen(buf);
// name too big to fit?
if ((nameLen + offset + 2) > kMaxDataLength) {
break;
}
// copy the name, which we know will fit
strcpy((char *)&hdr->data[offset], entry.d_name);
strcpy((char *)&hdr->data[offset], buf);
//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
offset += strlen(entry.d_name) + 1;
offset += nameLen + 1;
}
closedir(dp);
@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create)
return kErrNoSession;
}
uint32_t fileSize = 0;
if (!create) {
struct stat st;
if (stat(req->dataAsCString(), &st) != 0) {
return kErrNotFile;
}
fileSize = st.st_size;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create)
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
if (create) {
hdr->size = 0;
} else {
hdr->size = sizeof(uint32_t);
*((uint32_t*)hdr->data) = fileSize;
}
return kErrNone;
}

2
src/modules/mavlink/mavlink_ftp.h
View File

@ -146,7 +146,7 @@ private:
mavlink_message_t msg;
msg.checksum = 0;
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
_mavlink->get_channel(), &msg, sequence(), rawData());
_mavlink->get_channel(), &msg, sequence()+1, rawData());
_mavlink->lockMessageBufferMutex();
bool success = _mavlink->message_buffer_write(&msg, len);

156
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1385,43 +1385,43 @@ protected:
};
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
return MavlinkStreamPositionTargetGlobalInt::get_name_static();
}
static const char *get_name_static()
{
return "GLOBAL_POSITION_SETPOINT_INT";
return "POSITION_TARGET_GLOBAL_INT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
return new MavlinkStreamPositionTargetGlobalInt(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_pos_sp_triplet_sub;
/* do not allow top copying this class */
MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &);
protected:
explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
@ -1430,15 +1430,14 @@ protected:
struct position_setpoint_triplet_s pos_sp_triplet;
if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
mavlink_global_position_setpoint_int_t msg;
mavlink_position_target_global_int_t msg{};
msg.coordinate_frame = MAV_FRAME_GLOBAL;
msg.latitude = pos_sp_triplet.current.lat * 1e7;
msg.longitude = pos_sp_triplet.current.lon * 1e7;
msg.altitude = pos_sp_triplet.current.alt * 1000;
msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
msg.lat_int = pos_sp_triplet.current.lat * 1e7;
msg.lon_int = pos_sp_triplet.current.lon * 1e7;
msg.alt = pos_sp_triplet.current.alt;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
}
}
};
@ -1454,12 +1453,12 @@ public:
static const char *get_name_static()
{
return "LOCAL_POSITION_SETPOINT";
return "POSITION_TARGET_LOCAL_NED";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
@ -1469,7 +1468,7 @@ public:
unsigned get_size()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
@ -1491,137 +1490,87 @@ protected:
struct vehicle_local_position_setpoint_s pos_sp;
if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
mavlink_local_position_setpoint_t msg;
mavlink_position_target_local_ned_t msg{};
msg.time_boot_ms = pos_sp.timestamp / 1000;
msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
msg.x = pos_sp.x;
msg.y = pos_sp.y;
msg.z = pos_sp.z;
msg.yaw = pos_sp.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
}
}
};
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
class MavlinkStreamAttitudeTarget : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
return MavlinkStreamAttitudeTarget::get_name_static();
}
static const char *get_name_static()
{
return "ROLL_PITCH_YAW_THRUST_SETPOINT";
return "ATTITUDE_TARGET";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
return MAVLINK_MSG_ID_ATTITUDE_TARGET;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
return new MavlinkStreamAttitudeTarget(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *att_sp_sub;
uint64_t att_sp_time;
MavlinkOrbSubscription *_att_sp_sub;
MavlinkOrbSubscription *_att_rates_sp_sub;
uint64_t _att_sp_time;
uint64_t _att_rates_sp_time;
/* do not allow top copying this class */
MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &);
protected:
explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
att_sp_time(0)
explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
_att_sp_time(0),
_att_rates_sp_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_attitude_setpoint_s att_sp;
if (att_sp_sub->update(&att_sp_time, &att_sp)) {
mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
struct vehicle_rates_setpoint_s att_rates_sp;
(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
mavlink_attitude_target_t msg{};
msg.time_boot_ms = att_sp.timestamp / 1000;
msg.roll = att_sp.roll_body;
msg.pitch = att_sp.pitch_body;
msg.yaw = att_sp.yaw_body;
mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
msg.body_roll_rate = att_rates_sp.roll;
msg.body_pitch_rate = att_rates_sp.pitch;
msg.body_yaw_rate = att_rates_sp.yaw;
msg.thrust = att_sp.thrust;
_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
}
}
};
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
}
static const char *get_name_static()
{
return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_att_rates_sp_sub;
uint64_t _att_rates_sp_time;
/* do not allow top copying this class */
MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
protected:
explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
_att_rates_sp_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_rates_setpoint_s att_rates_sp;
if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
msg.time_boot_ms = att_rates_sp.timestamp / 1000;
msg.roll_rate = att_rates_sp.roll;
msg.pitch_rate = att_rates_sp.pitch;
msg.yaw_rate = att_rates_sp.yaw;
msg.thrust = att_rates_sp.thrust;
_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
}
}
};
@ -2149,10 +2098,9 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),

34
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
handle_message_radio_status(msg);
break;
@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
{
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
/* Only accept system IDs from 1 to 4 */
if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
struct offboard_control_setpoint_s offboard_control_sp;
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
/* Convert values * 1000 back */
offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{

283
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -105,6 +105,7 @@ public:
int start();
private:
const float alt_ctl_dz = 0.2f;
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
@ -184,6 +185,8 @@ private:
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Vector<3> _sp_move_rate;
/**
* Update our local parameter cache.
@ -216,6 +219,16 @@ private:
*/
void reset_alt_sp();
/**
* Set position setpoint using manual control
*/
void control_manual(float dt);
/**
* Set position setpoint using offboard control
*/
void control_offboard(float dt);
/**
* Select between barometric and global (AMSL) altitudes
*/
@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_sp_move_rate.zero();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@ -508,11 +528,125 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2);
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
void
MulticopterPositionControl::control_manual(float dt)
{
_sp_move_rate.zero();
if (_control_mode.flag_control_altitude_enabled) {
/* move altitude setpoint with throttle stick */
_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* move position setpoint with roll/pitch stick */
_sp_move_rate(0) = _manual.x;
_sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = _sp_move_rate.length();
if (sp_move_norm > 1.0f) {
_sp_move_rate /= sp_move_norm;
}
/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
}
void
MulticopterPositionControl::control_offboard(float dt)
{
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
} else {
reset_pos_sp();
reset_alt_sp();
}
}
void
MulticopterPositionControl::task_main()
{
@ -551,13 +685,6 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
float yaw_sp_move_rate;
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
@ -616,138 +743,17 @@ MulticopterPositionControl::task_main()
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
_vel_ff.zero();
_sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
control_manual(dt);
} else if (_control_mode.flag_control_offboard_enabled) {
/* Offboard control */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _pos_sp_triplet.current.vx;
sp_move_rate(1) = _pos_sp_triplet.current.vy;
yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
}
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
reset_pos_sp();
reset_alt_sp();
}
/* offboard control */
control_offboard(dt);
} else {
/* AUTO */
@ -782,6 +788,7 @@ MulticopterPositionControl::task_main()
}
/* fill local position setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
@ -821,7 +828,7 @@ MulticopterPositionControl::task_main()
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
@ -901,7 +908,7 @@ MulticopterPositionControl::task_main()
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */

4
src/modules/navigator/mission.cpp
View File

@ -271,6 +271,10 @@ Mission::check_dist_1wp()
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
}
return true;
} else {

4
src/modules/navigator/mission_params.c
View File

@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
* waypoint is more distant than MIS_DIS_1WP from the current position.
*
* @min 0
* @max 250
* @max 1000
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);

6
src/modules/sdlog2/sdlog2.c
View File

@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;

4
src/modules/sdlog2/sdlog2_messages.h
View File

@ -333,13 +333,11 @@ struct log_GS1B_s {
#define LOG_TECS_MSG 30
struct log_TECS_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */

8
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -60,7 +60,7 @@ enum SETPOINT_TYPE
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */
SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
};
struct position_setpoint_s
@ -71,9 +71,9 @@ struct position_setpoint_s
float y; /**< local position setpoint in m in NED */
float z; /**< local position setpoint in m in NED */
bool position_valid; /**< true if local position setpoint valid */
float vx; /**< local velocity setpoint in m in NED */
float vy; /**< local velocity setpoint in m in NED */
float vz; /**< local velocity setpoint in m in NED */
float vx; /**< local velocity setpoint in m/s in NED */
float vy; /**< local velocity setpoint in m/s in NED */
float vz; /**< local velocity setpoint in m/s in NED */
bool velocity_valid; /**< true if local velocity setpoint valid */
double lat; /**< latitude, in deg */
double lon; /**< longitude, in deg */

12
src/modules/uORB/topics/tecs_status.h
View File

@ -51,11 +51,13 @@
*/
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_NORMAL = 0,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
TECS_MODE_LAND_THROTTLELIM,
TECS_MODE_BAD_DESCENT,
TECS_MODE_CLIMBOUT
} tecs_mode;
/**
@ -65,14 +67,12 @@ struct tecs_status_s {
uint64_t timestamp; /**< timestamp, in microseconds since system start */
float altitudeSp;
float altitude;
float altitudeFiltered;
float altitude_filtered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeed_filtered;
float airspeedDerivativeSp;
float airspeedDerivative;

3
src/modules/uORB/topics/vehicle_local_position_setpoint.h
View File

@ -50,11 +50,12 @@
*/
struct vehicle_local_position_setpoint_s {
uint64_t timestamp; /**< timestamp of the setpoint */
float x; /**< in meters NED */
float y; /**< in meters NED */
float z; /**< in meters NED */
float yaw; /**< in radians NED -PI..+PI */
}; /**< Local position in NED frame to go to */
}; /**< Local position in NED frame */
/**
* @}

58
src/modules/uavcan/uavcan_main.cpp
View File

@ -180,16 +180,13 @@ void UavcanNode::fill_node_info()
/* software version */
uavcan::protocol::SoftwareVersion swver;
// Extracting the last 8 hex digits of FW_GIT and converting them to int
const unsigned fw_git_len = std::strlen(FW_GIT);
if (fw_git_len >= 8) {
char fw_git_short[9] = {};
std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8);
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
}
// Extracting the first 8 hex digits of FW_GIT and converting them to int
char fw_git_short[9] = {};
std::memmove(fw_git_short, FW_GIT, 8);
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
@ -256,7 +253,6 @@ int UavcanNode::run()
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
@ -276,21 +272,23 @@ int UavcanNode::run()
_node.setStatusOk();
while (!_task_should_exit) {
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds_num = 0;
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
while (!_task_should_exit) {
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@ -304,7 +302,7 @@ int UavcanNode::run()
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 0;
unsigned poll_id = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
@ -315,12 +313,7 @@ int UavcanNode::run()
}
}
if (!controls_updated) {
// timeout: no control data, switch to failsafe values
// XXX trigger failsafe
}
//can we mix?
// can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
@ -420,7 +413,8 @@ UavcanNode::subscribe()
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
// the first fd used by CAN
_poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@ -526,8 +520,8 @@ UavcanNode::print_info()
warnx("not running, start first");
}
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
}
/*