fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 09:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated to master (solved merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-27 09:26:27 +01:00
parent 560efcbc5b 4241e526aa
commit dd59409e31
289 changed files with 10555 additions and 4461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+60 -33
View File
@@ -79,12 +79,14 @@ using math::Quaternion;
class AttitudeEstimatorQ;
namespace attitude_estimator_q {
namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}
class AttitudeEstimatorQ {
class AttitudeEstimatorQ
{
public:
/**
* Constructor
@@ -160,6 +162,7 @@ private:
/* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate;
math::LowPassFilter2p _lp_pitch_rate;
math::LowPassFilter2p _lp_yaw_rate;
hrt_abstime _vel_prev_t = 0;
@@ -187,11 +190,12 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
_lp_roll_rate(250.0f, 20.0f),
_lp_pitch_rate(250.0f, 20.0f)
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
@@ -235,11 +239,11 @@ int AttitudeEstimatorQ::start()
/* start the task */
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
@@ -289,6 +293,7 @@ void AttitudeEstimatorQ::task_main()
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
continue;
@@ -298,6 +303,7 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
@@ -311,6 +317,7 @@ void AttitudeEstimatorQ::task_main()
for (unsigned j = 0; j < 3; j++) {
if (sensors.gyro_integral_dt[i] > 0) {
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
} else {
/* fall back to angular rate */
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
@@ -319,10 +326,11 @@ void AttitudeEstimatorQ::task_main()
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
int best_gyro, best_accel, best_mag;
@@ -341,26 +349,28 @@ void AttitudeEstimatorQ::task_main()
_data_good = true;
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
}
@@ -368,8 +378,10 @@ void AttitudeEstimatorQ::task_main()
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
/* set magnetic declination automatically */
_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
@@ -387,6 +399,7 @@ void AttitudeEstimatorQ::task_main()
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = _gpos.timestamp;
_vel_prev = vel;
}
@@ -400,7 +413,7 @@ void AttitudeEstimatorQ::task_main()
// Time from previous iteration
hrt_abstime now = hrt_absolute_time();
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f;
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f;
last_time = now;
if (dt > _dt_max) {
@@ -420,9 +433,12 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -443,6 +459,7 @@ void AttitudeEstimatorQ::task_main()
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
@@ -472,11 +489,14 @@ void AttitudeEstimatorQ::task_main()
}
}
void AttitudeEstimatorQ::update_parameters(bool force) {
void AttitudeEstimatorQ::update_parameters(bool force)
{
bool updated = force;
if (!updated) {
orb_check(_params_sub, &updated);
}
if (updated) {
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
@@ -498,7 +518,8 @@ void AttitudeEstimatorQ::update_parameters(bool force) {
}
}
bool AttitudeEstimatorQ::init() {
bool AttitudeEstimatorQ::init()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
Vector<3> k = -_accel;
@@ -522,9 +543,10 @@ bool AttitudeEstimatorQ::init() {
_q.normalize();
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
_inited = true;
} else {
_inited = false;
}
@@ -532,7 +554,8 @@ bool AttitudeEstimatorQ::init() {
return _inited;
}
bool AttitudeEstimatorQ::update(float dt) {
bool AttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_data_good) {
@@ -559,18 +582,20 @@ bool AttitudeEstimatorQ::update(float dt) {
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
@@ -583,7 +608,7 @@ bool AttitudeEstimatorQ::update(float dt) {
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
@@ -594,7 +619,9 @@ bool AttitudeEstimatorQ::update(float dt) {
return true;
}
int attitude_estimator_q_main(int argc, char *argv[]) {
int attitude_estimator_q_main(int argc, char *argv[])
{
if (argc < 1) {
warnx("usage: attitude_estimator_q {start|stop|status}");
return 1;
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+8 -2
View File
@@ -47,6 +47,7 @@
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
@@ -56,6 +57,7 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
@@ -65,6 +67,7 @@ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
@@ -77,6 +80,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
*
* @group Attitude Q estimator
* @unit degrees
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
@@ -106,6 +110,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
* @min 0
* @max 2
* @unit rad/s
* @decimal 3
*/
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
@@ -113,7 +118,8 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
* Threshold (of RMS) to warn about high vibration levels
*
* @group Attitude Q estimator
* @min 0.001
* @max 100
* @min 0.01
* @max 10
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f);
src/modules/commander/commander_helper.cpp
+1 -1
View File
@@ -387,7 +387,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float
// XXX this time constant needs to become tunable
// but really, the right fix are smart batteries.
float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f;
if (isfinite(val)) {
if (PX4_ISFINITE(val)) {
throttle_lowpassed = val;
}
src/modules/controllib/block/Block.hpp
+18 -14
View File
@@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class SuperBlock;
class BlockParamBase;
class SuperBlock;
/**
*/
@@ -81,9 +81,9 @@ public:
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
List<uORB::SubscriptionNode *> &getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> &getPublications() { return _publications; }
List<BlockParamBase *> &getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
@@ -94,8 +94,8 @@ protected:
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block&);
Block operator=(const control::Block&);
Block(const control::Block &);
Block operator=(const control::Block &);
};
class __EXPORT SuperBlock :
@@ -106,28 +106,32 @@ public:
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children() {
_children()
{
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams() {
virtual void updateParams()
{
Block::updateParams();
if (getChildren().getHead() != NULL) updateChildParams();
if (getChildren().getHead() != NULL) { updateChildParams(); }
}
virtual void updateSubscriptions() {
virtual void updateSubscriptions()
{
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) updateChildSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
}
virtual void updatePublications() {
virtual void updatePublications()
{
Block::updatePublications();
if (getChildren().getHead() != NULL) updateChildPublications();
if (getChildren().getHead() != NULL) { updateChildPublications(); }
}
protected:
// methods
List<Block *> & getChildren() { return _children; }
List<Block *> &getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
void updateChildPublications();
src/modules/controllib/block/BlockParam.cpp
+12 -7
View File
@@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
}
@@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
_handle = param_find(fullname);
if (_handle == PARAM_INVALID)
if (_handle == PARAM_INVALID) {
printf("error finding param: %s\n", fullname);
}
};
template <class T>
BlockParam<T>::BlockParam(Block *block, const char *name,
bool parent_prefix) :
bool parent_prefix) :
BlockParamBase(block, name, parent_prefix),
_val() {
_val()
{
update();
}
@@ -93,13 +96,15 @@ template <class T>
void BlockParam<T>::set(T val) { _val = val; }
template <class T>
void BlockParam<T>::update() {
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
void BlockParam<T>::update()
{
if (_handle != PARAM_INVALID) { param_get(_handle, &_val); }
}
template <class T>
void BlockParam<T>::commit() {
if (_handle != PARAM_INVALID) param_set(_handle, &_val);
void BlockParam<T>::commit()
{
if (_handle != PARAM_INVALID) { param_set(_handle, &_val); }
}
template <class T>
src/modules/controllib/blocks.cpp
+8 -2
View File
@@ -123,9 +123,10 @@ int blockLimitSymTest()
float BlockLowPass::update(float input)
{
if (!isfinite(getState())) {
if (!PX4_ISFINITE(getState())) {
setState(input);
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
@@ -202,13 +203,14 @@ int blockHighPassTest()
float BlockLowPass2::update(float input)
{
if (!isfinite(getState())) {
if (!PX4_ISFINITE(getState())) {
setState(input);
}
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
_lp.set_cutoff_frequency(_fs, getFCutParam());
}
_state = _lp.apply(input);
return _state;
}
@@ -340,8 +342,10 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
@@ -351,6 +355,7 @@ float BlockDerivative::update(float input)
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}
@@ -534,6 +539,7 @@ int blockRandGaussTest()
}
float stdDev = sqrt(sum / (n - 1));
(void)(stdDev);
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n");
src/modules/controllib/blocks.hpp
+20 -10
View File
@@ -317,7 +317,8 @@ public:
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input) {
float update(float input)
{
return getKP() * input;
}
// accessors
@@ -343,7 +344,8 @@ public:
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input);
}
@@ -374,7 +376,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKD() * getDerivative().update(input);
}
@@ -407,7 +410,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
@@ -440,11 +444,13 @@ public:
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0) {
_val(0)
{
update(0);
};
virtual ~BlockOutput() {};
void update(float input) {
void update(float input)
{
_val = _limit.update(input + getTrim());
}
// accessors
@@ -472,13 +478,15 @@ public:
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX") {
_max(this, "MAX")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update() {
float update()
{
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
@@ -503,13 +511,15 @@ public:
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV") {
_stdDev(this, "DEV")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update() {
float update()
{
static float V1, V2, S;
static int phase = 0;
float X;
src/modules/controllib/uorb/blocks.cpp
+14 -13
View File
@@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
void BlockWaypointGuidance::update(
const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
@@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), &getPublications())
_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
{
}
src/modules/controllib/uorb/blocks.hpp
+4 -4
View File
@@ -80,10 +80,10 @@ private:
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd);
void update(const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};
src/modules/ekf_att_pos_estimator/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS )
if(NOT ${OS} STREQUAL "qurt")
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
endif()
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+7 -7
View File
@@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
3900,
4600,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@@ -1317,9 +1317,9 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]);
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]);
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]);
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
}
_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
@@ -1337,9 +1337,9 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU;
}
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+10 -11
View File
@@ -43,7 +43,6 @@
#include <stdio.h>
#include <stdarg.h>
#include <cmath>
#include <algorithm>
#ifndef M_PI_F
#define M_PI_F static_cast<float>(M_PI)
@@ -1853,7 +1852,7 @@ void AttPosEKF::FuseOptFlow()
Vector3f relVelSensor;
// Perform sequential fusion of optical flow measurements only with valid tilt and height
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
bool validTilt = Tnb.z.z > 0.71f;
if (validTilt)
@@ -2113,7 +2112,7 @@ void AttPosEKF::OpticalFlowEKF()
} else {
return;
}
distanceTravelledSq = std::min(distanceTravelledSq, 100.0f);
distanceTravelledSq = fmin(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
}
@@ -2153,7 +2152,7 @@ void AttPosEKF::OpticalFlowEKF()
varInnovRng = 1.0f/SK_RNG[1];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@@ -2173,7 +2172,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// correct the covariance matrix
float nextPopt[2][2];
@@ -2182,8 +2181,8 @@ void AttPosEKF::OpticalFlowEKF()
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
@@ -2222,7 +2221,7 @@ void AttPosEKF::OpticalFlowEKF()
vel.z = statesAtFlowTime[6];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@@ -2290,7 +2289,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// correct the covariance matrix
for (uint8_t i = 0; i < 2 ; i++) {
@@ -2306,8 +2305,8 @@ void AttPosEKF::OpticalFlowEKF()
}
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
src/modules/fixedwing_backside/fixedwing.cpp
+59 -51
View File
@@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update()
// store old position command before update if new command sent
if (_missionCmd.updated()) {
_lastMissionCmd = _missionCmd.getData();
_lastMissionCmd = _missionCmd.get();
}
// check for new updates
@@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update()
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.get().control[i] = 0.0f;
}
// only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) {
if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
// TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
_guide.update(_pos.get(), _att.get(),
_missionCmd.get().current,
_lastMissionCmd.current);
}
// XXX handle STABILIZED (loiter on spot) as well
@@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO) {
if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) {
// calculate velocity, XXX should be airspeed,
// but using ground speed for now for the purpose
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.get().vel_n * _pos.get().vel_n +
_pos.get().vel_e * _pos.get().vel_e +
_pos.get().vel_d * _pos.get().vel_d));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
float pCmd = _phi2P.update(phiCmd - _att.roll);
float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd
float rCmd = 0;
@@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update()
float outputScale = velocityRatio * velocityRatio;
// this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
_att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed,
outputScale);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.get().control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
@@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
// do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) {
if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
_actuators.get().control[CH_THR] =
(_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
}
} else if (_status.main_state == MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
_actuators.get().control[CH_AIL] = _manual.get().y;
_actuators.get().control[CH_ELV] = _manual.get().x;
_actuators.get().control[CH_RDR] = _manual.get().r;
_actuators.get().control[CH_THR] = _manual.get().z;
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL ||
_status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.get().vel_n * _pos.get().vel_n +
_pos.get().vel_e * _pos.get().vel_e +
_pos.get().vel_d * _pos.get().vel_d));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
@@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update()
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle *
float vCmd = _vLimit.update(_manual.get().z *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
_actuators.get().control[CH_THR] = _manual.get().z;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
@@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (_status.hil_state != HIL_STATE_ON) {
if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
_actuators.get().control[CH_THR] =
(_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
}
// body rates controller, disabled for now
@@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update()
} else if (
0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r,
_att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
_actuators.get().control[CH_AIL] = _stabilization.getAileron();
_actuators.get().control[CH_ELV] = _stabilization.getElevator();
_actuators.get().control[CH_RDR] = _stabilization.getRudder();
_actuators.get().control[CH_THR] = _manual.get().z;
}
// update all publications
@@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.get().control[i] = 0.0f;
}
updatePublications();
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+1 -1
View File
@@ -179,7 +179,7 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
* @max 150.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
/**
* Maximum climb rate
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+11 -11
View File
@@ -53,7 +53,7 @@ mTecs::mTecs() :
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
#if defined(__PX4_NUTTX)
_status(ORB_ID(tecs_status), &getPublications()),
_status(ORB_ID(tecs_status), -1, &getPublications()),
#endif // defined(__PX4_NUTTX)
/* control blocks */
_controlTotalEnergy(this, "THR"),
@@ -88,8 +88,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
float airspeedSp, unsigned mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) ||
!PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
return -1;
}
@@ -111,8 +111,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude_filtered = altitudeFiltered;
_status.get().altitudeSp = altitudeSp;
_status.get().altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)
@@ -125,8 +125,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
float airspeedSp, unsigned mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
!PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
return -1;
}
@@ -149,8 +149,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed_filtered = airspeedFiltered;
_status.get().airspeedSp = airspeedSp;
_status.get().airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)
@@ -163,8 +163,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
!PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) {
return -1;
}
/* time measurement */
src/modules/land_detector/FixedwingLandDetector.cpp
+15 -13
View File
@@ -49,18 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}),
_airspeedSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeed{},
_vehicleStatus{},
_arming{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
_vehicleLocalPosition( {}),
_airspeedSub(-1),
_vehicleStatusSub(-1),
_armingSub(-1),
_airspeed{},
_vehicleStatus{},
_arming{},
_parameterSub(-1),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
{
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
@@ -101,10 +101,12 @@ bool FixedwingLandDetector::update()
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
if (PX4_ISFINITE(val)) {
src/modules/land_detector/LandDetector.cpp
+7 -7
View File
@@ -47,12 +47,11 @@
LandDetector::LandDetector() :
_landDetectedPub(0),
_landDetected({0, false}),
_arming_time(0),
_taskShouldExit(false),
_taskIsRunning(false),
_work{}
{
_landDetected( {0, false}),
_arming_time(0),
_taskShouldExit(false),
_taskIsRunning(false),
_work{} {
// ctor
}
@@ -111,7 +110,8 @@ void LandDetector::cycle()
}
if (!_taskShouldExit) {
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
}
}
src/modules/land_detector/LandDetector.h
+2 -1
View File
@@ -99,7 +99,8 @@ protected:
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold
before triggering a land */
static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */
static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME =
1000000; /**< time interval in which wider acceptance thresholds are used after arming */
protected:
orb_advert_t _landDetectedPub; /**< publisher for position in local frame */
src/modules/land_detector/MulticopterLandDetector.cpp
+5 -4
View File
@@ -99,13 +99,14 @@ bool MulticopterLandDetector::update()
if (!_arming.armed) {
_arming_time = 0;
return true;
} else if (_arming_time == 0) {
_arming_time = hrt_absolute_time();
}
// return status based on armed state if no position lock is available
if (_vehicleGlobalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// no position lock - not landed if armed
return !_arming.armed;
@@ -129,14 +130,14 @@ bool MulticopterLandDetector::update()
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity
&& _vehicleStatus.condition_global_position_valid;
&& _vehicleStatus.condition_global_position_valid;
// next look if all rotation angles are not moving
float maxRotationScaled = _params.maxRotation * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
src/modules/land_detector/land_detector_main.cpp
+2
View File
@@ -152,6 +152,7 @@ static int land_detector_start(const char *mode)
return 1;
}
}
printf("\n");
}
@@ -176,6 +177,7 @@ int land_detector_main(int argc, char *argv[])
warnx("land_detector start failed");
return 1;
}
return 0;
}
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+1378
View File
File diff suppressed because it is too large Load Diff
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+310
View File
@@ -0,0 +1,310 @@
#pragma once
#include <controllib/uorb/blocks.hpp>
#include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h>
#include <lib/geo/geo.h>
#ifdef USE_MATRIX_LIB
#include "matrix/src/Matrix.hpp"
using namespace matrix;
#else
#include <Eigen/Eigen>
using namespace Eigen;
#endif
// uORB Subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
// uORB Publications
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <uORB/topics/estimator_status.h>
#define CBRK_NO_VISION_KEY 328754
using namespace control;
enum fault_t {
FAULT_NONE = 0,
FAULT_MINOR,
FAULT_SEVERE
};
enum sensor_t {
SENSOR_BARO = 0,
SENSOR_GPS,
SENSOR_LIDAR,
SENSOR_FLOW,
SENSOR_SONAR,
SENSOR_VISION,
SENSOR_MOCAP
};
class BlockLocalPositionEstimator : public control::SuperBlock
{
//
// The purpose of this estimator is to provide a robust solution for
// indoor flight.
//
// dynamics:
//
// x(+) = A * x(-) + B * u(+)
// y_i = C_i*x
//
// kalman filter
//
// E[xx'] = P
// E[uu'] = W
// E[y_iy_i'] = R_i
//
// prediction
// x(+|-) = A*x(-|-) + B*u(+)
// P(+|-) = A*P(-|-)*A' + B*W*B'
//
// correction
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED)
// vx, vy, vz ( vel NED),
// bx, by, bz ( TODO accelerometer bias)
// tz (TODO terrain altitude)
//
// measurements:
//
// sonar: pz (measured d*cos(phi)*cos(theta))
//
// baro: pz
//
// flow: vx, vy (flow is in body x, y frame)
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: px (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
//
public:
BlockLocalPositionEstimator();
void update();
virtual ~BlockLocalPositionEstimator();
private:
// prevent copy and assignment
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
// constants
static const uint8_t n_x = 9;
static const uint8_t n_u = 3; // 3 accelerations
static const uint8_t n_y_flow = 2;
static const uint8_t n_y_sonar = 1;
static const uint8_t n_y_baro = 1;
static const uint8_t n_y_lidar = 1;
static const uint8_t n_y_gps = 6;
static const uint8_t n_y_vision = 3;
static const uint8_t n_y_mocap = 3;
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
enum {U_ax = 0, U_ay, U_az};
enum {Y_baro_z = 0};
enum {Y_lidar_z = 0};
enum {Y_flow_x = 0, Y_flow_y};
enum {Y_sonar_z = 0};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};
// methods
// ----------------------------
void initP();
// predict the next state
void predict();
// correct the state prediction wtih a measurement
void correctBaro();
void correctGps();
void correctLidar();
void correctFlow();
void correctSonar();
void correctVision();
void correctmocap();
// sensor initialization
void updateHome();
void initBaro();
void initGps();
void initLidar();
void initSonar();
void initFlow();
void initVision();
void initmocap();
// publications
void publishLocalPos();
void publishGlobalPos();
void publishFilteredFlow();
void publishEstimatorStatus();
// attributes
// ----------------------------
// subscriptions
uORB::Subscription<vehicle_status_s> _sub_status;
uORB::Subscription<actuator_armed_s> _sub_armed;
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
uORB::Subscription<vehicle_attitude_s> _sub_att;
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
uORB::Subscription<optical_flow_s> _sub_flow;
uORB::Subscription<sensor_combined_s> _sub_sensor;
uORB::Subscription<distance_sensor_s> _sub_distance;
uORB::Subscription<parameter_update_s> _sub_param_update;
uORB::Subscription<manual_control_setpoint_s> _sub_manual;
uORB::Subscription<home_position_s> _sub_home;
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
uORB::Subscription<att_pos_mocap_s> _sub_mocap;
// publications
uORB::Publication<vehicle_local_position_s> _pub_lpos;
uORB::Publication<vehicle_global_position_s> _pub_gpos;
uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
uORB::Publication<estimator_status_s> _pub_est_status;
// map projection
struct map_projection_reference_s _map_ref;
// parameters
BlockParamInt _integrate;
BlockParamFloat _flow_xy_stddev;
BlockParamFloat _sonar_z_stddev;
BlockParamFloat _lidar_z_stddev;
BlockParamFloat _accel_xy_stddev;
BlockParamFloat _accel_z_stddev;
BlockParamFloat _baro_stddev;
BlockParamFloat _gps_xy_stddev;
BlockParamFloat _gps_z_stddev;
BlockParamFloat _gps_vxy_stddev;
BlockParamFloat _gps_vz_stddev;
BlockParamFloat _gps_eph_max;
BlockParamFloat _vision_xy_stddev;
BlockParamFloat _vision_z_stddev;
BlockParamInt _no_vision;
BlockParamFloat _beta_max;
BlockParamFloat _mocap_p_stddev;
// process noise
BlockParamFloat _pn_p_noise_power;
BlockParamFloat _pn_v_noise_power;
BlockParamFloat _pn_b_noise_power;
// misc
struct pollfd _polls[3];
uint64_t _timeStamp;
uint64_t _time_last_xy;
uint64_t _time_last_flow;
uint64_t _time_last_baro;
uint64_t _time_last_gps;
uint64_t _time_last_lidar;
uint64_t _time_last_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
int _mavlink_fd;
// initialization flags
bool _baroInitialized;
bool _gpsInitialized;
bool _lidarInitialized;
bool _sonarInitialized;
bool _flowInitialized;
bool _visionInitialized;
bool _mocapInitialized;
// init counts
int _baroInitCount;
int _gpsInitCount;
int _lidarInitCount;
int _sonarInitCount;
int _flowInitCount;
int _visionInitCount;
int _mocapInitCount;
// reference altitudes
float _altHome;
bool _altHomeInitialized;
float _baroAltHome;
float _gpsAltHome;
float _lidarAltHome;
float _sonarAltHome;
float _flowAltHome;
Vector3f _visionHome;
Vector3f _mocapHome;
// flow integration
float _flowX;
float _flowY;
float _flowMeanQual;
// referene lat/lon
double _gpsLatHome;
double _gpsLonHome;
// status
bool _canEstimateXY;
bool _canEstimateZ;
bool _xyTimeout;
// sensor faults
fault_t _baroFault;
fault_t _gpsFault;
fault_t _lidarFault;
fault_t _flowFault;
fault_t _sonarFault;
fault_t _visionFault;
fault_t _mocapFault;
bool _visionTimeout;
bool _mocapTimeout;
perf_counter_t _loop_perf;
perf_counter_t _interval_perf;
perf_counter_t _err_perf;
// state space
Matrix<float, n_x, 1> _x; // state vector
Matrix<float, n_u, 1> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
};
src/modules/local_position_estimator/CMakeLists.txt
+57
View File
@@ -0,0 +1,57 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error)
# add matrix tests
add_subdirectory(matrix)
endif()
# use custom matrix lib instead of Eigen
add_definitions(-DUSE_MATRIX_LIB)
px4_add_module(
MODULE modules__local_position_estimator
MAIN local_position_estimator
STACK 9216
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
local_position_estimator_main.cpp
BlockLocalPositionEstimator.cpp
params.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/local_position_estimator/local_position_estimator_main.cpp
+169
View File
@@ -0,0 +1,169 @@
/****************************************************************************
*
* Copyright (c) 2015 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 local_position_estimator.cpp
* @author James Goppert <james.goppert@gmail.com>
* @author Mohammed Kabir
* @author Nuno Marques <n.marques21@hotmail.com>
*
* Local position estimator
*/
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <fcntl.h>
#include <px4_posix.h>
#include "BlockLocalPositionEstimator.hpp"
static volatile bool thread_should_exit = false; /**< Deamon exit flag */
static volatile bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int local_position_estimator_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static int usage(const char *reason);
static int
usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
}
fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n");
return 1;
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int local_position_estimator_main(int argc, char *argv[])
{
if (argc < 1) {
usage("missing command");
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
return 0;
}
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("lp_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
10240,
local_position_estimator_thread_main,
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (thread_running) {
warnx("stop");
thread_should_exit = true;
} else {
warnx("not started");
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
return 0;
}
usage("unrecognized command");
return 1;
}
int local_position_estimator_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace control;
thread_running = true;
BlockLocalPositionEstimator est;
while (!thread_should_exit) {
est.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
src/modules/local_position_estimator/matrix/.gitignore
+1
View File
@@ -0,0 +1 @@
build*/
src/modules/local_position_estimator/matrix/CMakeLists.txt
+13
View File
@@ -0,0 +1,13 @@
cmake_minimum_required(VERSION 2.8)
project(matrix CXX)
set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
enable_testing()
include_directories(src)
add_subdirectory(test)
src/modules/local_position_estimator/matrix/src/Matrix.hpp
+464
View File
@@ -0,0 +1,464 @@
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename T, size_t M, size_t N>
class Matrix
{
private:
T _data[M][N];
size_t _rows;
size_t _cols;
public:
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const T *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
Matrix(T x, T y, T z) :
_data(),
_rows(M),
_cols(N)
{
// TODO, limit to only 3x1 matrices
_data[0][0] = x;
_data[1][0] = y;
_data[2][0] = z;
}
/**
* Accessors/ Assignment etc.
*/
T *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline T operator()(size_t i) const
{
return _data[i][0];
}
inline T operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline T &operator()(size_t i)
{
return _data[i][0];
}
inline T &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
{
const Matrix<T, M, N> &self = *this;
Matrix<T, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<T, M, N> &other)
{
Matrix<T, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<T, M, N> operator*(T scalar) const
{
Matrix<T, M, N> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<T, M, N> operator+(T scalar) const
{
Matrix<T, M, N> res;
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(T scalar)
{
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(T scalar)
{
Matrix<T, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<T, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<T, N, M> transpose() const
{
Matrix<T, N, M> res;
const Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
Matrix<T, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<T, M, 1> diagonal() const
{
Matrix<T, M, 1> res;
// force square for now
const Matrix<T, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
T tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<T, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
T tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <T, M, M> inverse() const
{
Matrix<T, M, M> L;
L.setIdentity();
const Matrix<T, M, M> &A = (*this);
Matrix<T, M, M> U = A;
Matrix<T, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<T, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<T, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<T, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
};
typedef Matrix<float, 2, 1> Vector2f;
typedef Matrix<float, 3, 1> Vector3f;
typedef Matrix<float, 3, 3> Matrix3f;
}; // namespace matrix
src/modules/local_position_estimator/matrix/test/CMakeLists.txt
+15
View File
@@ -0,0 +1,15 @@
set(tests
setIdentity
inverse
matrixMult
vectorAssignment
matrixAssignment
matrixScalarMult
transpose
)
foreach(test ${tests})
add_executable(${test}
${test}.cpp)
add_test(${test} ${test})
endforeach()
src/modules/local_position_estimator/matrix/test/inverse.cpp
+30
View File
@@ -0,0 +1,30 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
Matrix3f A_I = A.inverse();
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I_check(data_check);
(void)A_I;
assert(A_I == A_I_check);
// stess test
static const size_t n = 100;
Matrix<float, n, n> A_large;
A_large.setIdentity();
Matrix<float, n, n> A_large_I;
A_large_I.setZero();
for (size_t i = 0; i < 100; i++) {
A_large_I = A_large.inverse();
assert(A_large == A_large_I);
}
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp
+29
View File
@@ -0,0 +1,29 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f m;
m.setZero();
m(0, 0) = 1;
m(0, 1) = 2;
m(0, 2) = 3;
m(1, 0) = 4;
m(1, 1) = 5;
m(1, 2) = 6;
m(2, 0) = 7;
m(2, 1) = 8;
m(2, 2) = 9;
m.print();
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f m2(data);
m2.print();
assert(m == m2);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixMult.cpp
+26
View File
@@ -0,0 +1,26 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
Matrix3f A(data);
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
Matrix3f A_I(data_check);
Matrix3f I;
I.setIdentity();
A.print();
A_I.print();
Matrix3f R = A * A_I;
R.print();
assert(R == I);
Matrix3f R2 = A;
R2 *= A_I;
R2.print();
assert(R2 == I);
return 0;
}
src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp
+18
View File
@@ -0,0 +1,18 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix3f A(data);
A = A * 2;
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
Matrix3f A_check(data_check);
A.print();
A_check.print();
assert(A == A_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/setIdentity.cpp
+25
View File
@@ -0,0 +1,25 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Matrix3f A;
A.setIdentity();
assert(A.rows() == 3);
assert(A.cols() == 3);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i == j) {
assert(A(i, j) == 1);
} else {
assert(A(i, j) == 0);
}
}
}
return 0;
}
src/modules/local_position_estimator/matrix/test/transpose.cpp
+18
View File
@@ -0,0 +1,18 @@
#include "Matrix.hpp"
#include <assert.h>
#include <stdio.h>
using namespace matrix;
int main()
{
float data[9] = {1, 2, 3, 4, 5, 6};
Matrix<float, 2, 3> A(data);
Matrix<float, 3, 2> A_T = A.transpose();
float data_check[9] = {1, 4, 2, 5, 3, 6};
Matrix<float, 3, 2> A_T_check(data_check);
A_T.print();
A_T_check.print();
assert(A_T == A_T_check);
return 0;
}
src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp
+28
View File
@@ -0,0 +1,28 @@
#include "Matrix.hpp"
#include <assert.h>
using namespace matrix;
int main()
{
Vector3f v;
v(0) = 1;
v(1) = 2;
v(2) = 3;
v.print();
assert(v(0) == 1);
assert(v(1) == 2);
assert(v(2) == 3);
Vector3f v2(4, 5, 6);
v2.print();
assert(v2(0) == 4);
assert(v2(1) == 5);
assert(v2(2) == 6);
return 0;
}
src/modules/local_position_estimator/params.c
+222
View File
@@ -0,0 +1,222 @@
#include <systemlib/param/param.h>
// 16 is max name length
/**
* Enable local position estimator.
*
* @group Local Position Estimator
*/
PARAM_DEFINE_INT32(LPE_ENABLED, 1);
/**
* Enable accelerometer integration for prediction.
*
* @group Local Position Estimator
*/
PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
/**
* Optical flow xy standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
/**
* Sonar z standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
/**
* Lidar z standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
/**
* Accelerometer xy standard deviation
*
* Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
* std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
* Since accels sampled at 1000 Hz.
*
* should be 0.0464
*
* @group Local Position Estimator
* @unit m/s^2
* @min 0.00001
* @max 2
*/
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
/**
* Accelerometer z standard deviation
*
* (see Accel x comments)
*
* @group Local Position Estimator
* @unit m/s^2
* @min 0.00001
* @max 2
*/
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
/**
* Barometric presssure altitude z standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 3
*/
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
/**
* GPS xy standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 5
*/
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
/**
* GPS z standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 20
*/
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
/**
* GPS xy velocity standard deviation.
*
* @group Local Position Estimator
* @unit m/s
* @min 0.01
* @max 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
/**
* GPS z velocity standard deviation.
*
* @group Local Position Estimator
* @unit m/s
* @min 0.01
* @max 2
*/
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
/**
* GPS max eph
*
* @group Local Position Estimator
* @unit m
* @min 1.0
* @max 5.0
*/
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
/**
* Vision xy standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
/**
* Vision z standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 2
*/
PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
/**
* Circuit breaker to disable vision input.
*
* Set to the appropriate key (328754) to disable vision input.
*
* @group Local Position Estimator
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
/**
* Vicon position standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
/**
* Position propagation process noise power (variance*sampling rate).
*
* @group Local Position Estimator
* @unit (m/s^2)-s
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
/**
* Velocity propagation process noise power (variance*sampling rate).
*
* @group Local Position Estimator
* @unit (m/s)-s
* @min 0
* @max 5
*/
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
/**
* Accel bias propagation process noise power (variance*sampling rate).
*
* @group Local Position Estimator
* @unit (m/s)-s
* @min 0
* @max 1
*/
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f);
/**
* Fault detection threshold, for chi-squared dist.
*
* TODO add separate params for 1 dof, 3 dof, and 6 dof beta
* or false alarm rate in false alarms/hr
*
* @group Local Position Estimator
* @unit
* @min 3
* @max 1000
*/
PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f);
src/modules/mavlink/mavlink_main.cpp
-1
View File
@@ -1676,7 +1676,6 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GPS_RAW_INT", 1.0f);
configure_stream("GLOBAL_POSITION_INT", 3.0f);
configure_stream("LOCAL_POSITION_NED", 3.0f);
configure_stream("NAMED_VALUE_FLOAT", 2.0f);
configure_stream("RC_CHANNELS", 1.0f);
configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
src/modules/mavlink/mavlink_messages.cpp
+74
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/estimator_status.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/err.h>
@@ -1271,6 +1272,78 @@ protected:
};
class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamLocalPositionNEDCOV::get_name_static();
}
static const char *get_name_static()
{
return "LOCAL_POSITION_NED_COV";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamLocalPositionNEDCOV(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_est_sub;
uint64_t _est_time;
/* do not allow top copying this class */
MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &);
MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &);
protected:
explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink),
_est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
_est_time(0)
{}
void send(const hrt_abstime t)
{
struct estimator_status_s est;
if (_est_sub->update(&_est_time, &est)) {
mavlink_local_position_ned_cov_t msg;
msg.time_boot_ms = est.timestamp / 1000;
msg.x = est.states[0];
msg.y = est.states[1];
msg.z = est.states[2];
msg.vx = est.states[3];
msg.vy = est.states[4];
msg.vz = est.states[5];
msg.ax = est.states[6];
msg.ay = est.states[7];
msg.az = est.states[8];
for (int i=0;i<9;i++) {
msg.covariance[i] = est.covariances[i];
}
msg.covariance[9] = est.nan_flags;
msg.covariance[10] = est.health_flags;
msg.covariance[11] = est.timeout_flags;
memcpy(msg.covariance, est.covariances, sizeof(est.covariances));
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg);
}
}
};
class MavlinkStreamAttPosMocap : public MavlinkStream
{
public:
@@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static),
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
src/modules/mc_pos_control/mc_pos_control_main.cpp
+146 -62
View File
@@ -155,6 +155,10 @@ private:
control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
control::BlockDerivative _vel_x_deriv;
control::BlockDerivative _vel_y_deriv;
control::BlockDerivative _vel_z_deriv;
struct {
param_t thr_min;
param_t thr_max;
@@ -177,6 +181,10 @@ private:
param_t man_pitch_max;
param_t man_yaw_max;
param_t mc_att_yaw_p;
param_t hold_xy_dz;
param_t hold_z_dz;
param_t hold_max_xy;
param_t hold_max_z;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -189,6 +197,10 @@ private:
float man_pitch_max;
float man_yaw_max;
float mc_att_yaw_p;
float hold_xy_dz;
float hold_z_dz;
float hold_max_xy;
float hold_max_z;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
@@ -206,6 +218,10 @@ private:
bool _reset_pos_sp;
bool _reset_alt_sp;
bool _mode_auto;
bool _pos_hold_engaged;
bool _alt_hold_engaged;
bool _run_pos_control;
bool _run_alt_control;
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
@@ -213,7 +229,6 @@ private:
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;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
@@ -323,12 +338,19 @@ MulticopterPositionControl::MulticopterPositionControl() :
_global_vel_sp_pub(nullptr),
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
_ref_alt(0.0f),
_ref_timestamp(0),
_reset_pos_sp(true),
_reset_alt_sp(true),
_mode_auto(false),
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
@@ -358,7 +380,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_sp_move_rate.zero();
_R.identity();
@@ -383,6 +404,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
_params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ");
_params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ");
_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
/* fetch initial parameter values */
parameters_update(true);
@@ -470,6 +496,16 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
param_get(_params_handles.hold_xy_dz, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.hold_xy_dz = v;
param_get(_params_handles.hold_z_dz, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.hold_z_dz = v;
param_get(_params_handles.hold_max_xy, &v);
_params.hold_max_xy = (v < 0.0f ? 0.0f : v);
param_get(_params_handles.hold_max_z, &v);
_params.hold_max_z = (v < 0.0f ? 0.0f : v);
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -590,9 +626,9 @@ MulticopterPositionControl::reset_pos_sp()
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_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);
- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_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);
- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
}
@@ -602,7 +638,7 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
@@ -633,31 +669,20 @@ MulticopterPositionControl::limit_pos_sp_offset()
void
MulticopterPositionControl::control_manual(float dt)
{
_sp_move_rate.zero();
math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
req_vel_sp.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);
/* set vertical velocity setpoint with throttle stick */
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D
}
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;
/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp(0) = _manual.x;
req_vel_sp(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();
@@ -668,30 +693,70 @@ MulticopterPositionControl::control_manual(float dt)
reset_pos_sp();
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* limit velocity setpoint */
float req_vel_sp_norm = req_vel_sp.length();
/* 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 (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
/* _req_vel_sp 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);
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis
*/
/* horizontal axes */
if (_control_mode.flag_control_position_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
{
if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
(fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
{
_pos_hold_engaged = true;
}
}
else {
_pos_hold_engaged = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* set requested velocity setpoint */
if (!_pos_hold_engaged) {
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
}
}
float pos_sp_offs_norm = pos_sp_offs.length();
/* vertical axis */
if (_control_mode.flag_control_altitude_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
{
if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
{
_alt_hold_engaged = true;
}
}
else {
_alt_hold_engaged = false;
_pos_sp(2) = _pos(2);
}
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);
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
}
}
}
@@ -716,8 +781,10 @@ MulticopterPositionControl::control_offboard(float dt)
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;
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
if (_pos_sp_triplet.current.yaw_valid) {
@@ -734,15 +801,10 @@ MulticopterPositionControl::control_offboard(float dt)
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
_vel_sp(2) = _pos_sp_triplet.current.vz;
_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
/* 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();
@@ -998,6 +1060,9 @@ MulticopterPositionControl::task_main()
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
t_prev = t;
// set dt for control blocks
setDt(dt);
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
@@ -1034,7 +1099,11 @@ MulticopterPositionControl::task_main()
_vel(2) = _local_pos.vz;
_vel_ff.zero();
_sp_move_rate.zero();
/* by default, run position/altitude controller. the control_* functions
* can disable this and run velocity controllers directly in this cycle */
_run_pos_control = true;
_run_alt_control = true;
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
@@ -1074,10 +1143,15 @@ MulticopterPositionControl::task_main()
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
@@ -1094,17 +1168,23 @@ MulticopterPositionControl::task_main()
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
@@ -1161,8 +1241,12 @@ MulticopterPositionControl::task_main()
/* velocity error */
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;
/* derivative of velocity error, /
* does not includes setpoint acceleration */
math::Vector<3> vel_err_d;
vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
src/modules/mc_pos_control/mc_pos_control_params.c
+46
View File
@@ -265,3 +265,49 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
*/
PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
/**
* Deadzone of X,Y sticks where position hold is enabled
*
* @unit %
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
/**
* Deadzone of Z stick where altitude hold is enabled
*
* @unit %
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f);
/**
* Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f);
/**
* Maximum vertical velocity for which position hold is enabled (use 0 to disable check)
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f);
/**
* Low pass filter cut freq. for numerical velocity derivative
*
* @unit Hz
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
src/modules/muorb/adsp/px4muorb.hpp
+10 -10
View File
@@ -37,22 +37,22 @@
extern "C" {
int px4muorb_orb_initialize() __EXPORT;
int px4muorb_orb_initialize() __EXPORT;
int px4muorb_add_subscriber(const char *name) __EXPORT;
int px4muorb_add_subscriber(const char *name) __EXPORT;
int px4muorb_remove_subscriber(const char *name) __EXPORT;
int px4muorb_remove_subscriber(const char *name) __EXPORT;
int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT;
int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT;
int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes,
int *bytes_returned) __EXPORT;
int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes,
int *bytes_returned) __EXPORT;
int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes,
int *length_in_bytes, int *topic_count) __EXPORT;
int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes,
int *length_in_bytes, int *topic_count) __EXPORT;
int px4muorb_unblock_recieve_msg(void) __EXPORT;
int px4muorb_unblock_recieve_msg(void) __EXPORT;
}
src/modules/muorb/adsp/uORBFastRpcChannel.cpp
+6
View File
@@ -183,6 +183,7 @@ int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t leng
}
if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); }
if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); }
_count++;
@@ -367,6 +368,7 @@ int16_t uORB::FastRpcChannel::get_data
if (rc != 0) {
topic_name[0] = '\0';
}
/*
PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2,
(unsigned long)t3);
@@ -464,9 +466,13 @@ int16_t uORB::FastRpcChannel::get_bulk_data
hrt_abstime t3 = hrt_absolute_time();
if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); }
if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); }
if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); }
if ((unsigned long)(t3 - check_time) > 10000000) {
//PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3);
//PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize());
src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp
+28 -28
View File
@@ -36,41 +36,41 @@
namespace px4muorb
{
class KraitRpcWrapper;
class KraitRpcWrapper;
}
class px4muorb::KraitRpcWrapper
{
public:
/**
* Constructor
*/
KraitRpcWrapper() {}
/**
* destructor
*/
~KraitRpcWrapper() {}
/**
* Constructor
*/
KraitRpcWrapper() {}
/**
* Initiatizes the rpc channel px4 muorb
*/
bool Initialize() { return true; }
/**
* destructor
*/
~KraitRpcWrapper() {}
/**
* Terminate to clean up the resources. This should be called at program exit
*/
bool Terminate() { return true; }
/**
* Initiatizes the rpc channel px4 muorb
*/
bool Initialize() { return true; }
/**
* Muorb related functions to pub/sub of orb topic from krait to adsp
*/
int32_t AddSubscriber( const char* topic ) { return 1; }
int32_t RemoveSubscriber( const char* topic ) { return 1; }
int32_t SendData( const char* topic, int32_t length_in_bytes, const uint8_t* data ) { return 1; }
int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data ) { return 1; }
int32_t IsSubscriberPresent( const char* topic, int32_t* status ) { return 1; }
int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count ) { return 1; }
int32_t UnblockReceiveData() { return 1; }
/**
* Terminate to clean up the resources. This should be called at program exit
*/
bool Terminate() { return true; }
/**
* Muorb related functions to pub/sub of orb topic from krait to adsp
*/
int32_t AddSubscriber(const char *topic) { return 1; }
int32_t RemoveSubscriber(const char *topic) { return 1; }
int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return 1; }
int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) { return 1; }
int32_t IsSubscriberPresent(const char *topic, int32_t *status) { return 1; }
int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { return 1; }
int32_t UnblockReceiveData() { return 1; }
};
#endif // _px4muorb_KraitWrapper_hpp_
src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp
+2 -2
View File
@@ -139,11 +139,11 @@ int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t
if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); }
_snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) +
(unsigned long)(t3 - t2))) / (double)(_snd_msg_count);
(unsigned long)(t3 - t2))) / (double)(_snd_msg_count);
}
_overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) +
(unsigned long)(t4 - t1))) / (double)(_overall_snd_count);
(unsigned long)(t4 - t1))) / (double)(_overall_snd_count);
if ((t4 - _log_check_time) > _log_check_interval) {
/*
src/modules/navigator/mission_block.cpp
+1 -1
View File
@@ -153,7 +153,7 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
src/modules/px4iofirmware/CMakeLists.txt
+1 -1
View File
@@ -103,7 +103,7 @@ elseif(${config_io_board} STREQUAL "px4io-v2")
)
endif()
set(fw_io_name ${config_io_board}_${LABEL})
set(fw_io_name ${config_io_board})
add_executable(${fw_io_name}
${srcs})
src/modules/px4iofirmware/adc.c
+6 -3
View File
@@ -96,19 +96,22 @@ adc_init(void)
rCR2 |= ADC_CR2_RSTCAL;
up_udelay(1);
if (rCR2 & ADC_CR2_RSTCAL)
if (rCR2 & ADC_CR2_RSTCAL) {
return -1;
}
rCR2 |= ADC_CR2_CAL;
up_udelay(100);
if (rCR2 & ADC_CR2_CAL)
if (rCR2 & ADC_CR2_CAL) {
return -1;
}
#endif
/*
* Configure sampling time.
*
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
src/modules/px4iofirmware/dsm.c
+34 -16
View File
@@ -90,8 +90,9 @@ static bool
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
if (raw == 0xffff)
if (raw == 0xffff) {
return false;
}
*channel = (raw >> shift) & 0xf;
@@ -132,18 +133,21 @@ dsm_guess_format(bool reset)
unsigned channel, value;
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) {
cs10 |= (1 << channel);
}
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) {
cs11 |= (1 << channel);
}
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5)
if (samples++ < 5) {
return;
}
/*
* Iterate the set of sensible sniffed channel sets and see whether
@@ -170,11 +174,13 @@ dsm_guess_format(bool reset)
for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
if (cs10 == masks[i])
if (cs10 == masks[i]) {
votes10++;
}
if (cs11 == masks[i])
if (cs11 == masks[i]) {
votes11++;
}
}
if ((votes11 == 1) && (votes10 == 0)) {
@@ -210,8 +216,9 @@ dsm_init(const char *device)
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0)
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
if (dsm_fd >= 0) {
@@ -251,13 +258,14 @@ void
dsm_bind(uint16_t cmd, int pulses)
{
#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#else
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0)
if (dsm_fd < 0) {
return;
}
switch (cmd) {
@@ -297,6 +305,7 @@ dsm_bind(uint16_t cmd, int pulses)
up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, true);
}
break;
case dsm_bind_reinit_uart:
@@ -306,6 +315,7 @@ dsm_bind(uint16_t cmd, int pulses)
break;
}
#endif
}
@@ -329,8 +339,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
dsm_guess_format(true);
}
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
@@ -358,20 +369,24 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) {
continue;
}
/* ignore channels out of range */
if (channel >= PX4IO_RC_INPUT_CHANNELS)
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
continue;
}
/* update the decoded channel count */
if (channel >= *num_values)
if (channel >= *num_values) {
*num_values = channel + 1;
}
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (dsm_channel_shift == 10)
if (dsm_channel_shift == 10) {
value *= 2;
}
/*
* Spektrum scaling is special. There are these basic considerations
@@ -421,8 +436,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
* lines, so if we get a channel count of 13, we'll return 12 (the last
* data index that is stable).
*/
if (*num_values == 13)
if (*num_values == 13) {
*num_values = 12;
}
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
@@ -482,6 +498,7 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
/* if the read failed for any reason, just give up here */
if (ret < 1) {
return false;
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
@@ -497,8 +514,9 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
/*
* If we don't have a full dsm frame, return
*/
if (dsm_partial_frame_count < DSM_FRAME_SIZE)
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a dsm frame. Go ahead and
src/modules/px4iofirmware/i2c.c
+32 -18
View File
@@ -120,7 +120,7 @@ interface_init(void)
rCR1 = 0;
/* set for DMA operation */
rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN;
/* set the frequency value in CR2 */
rCR2 &= ~I2C_CR2_FREQ_MASK;
@@ -128,8 +128,11 @@ interface_init(void)
/* set divisor and risetime for fast mode */
uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
if (result < 1)
if (result < 1) {
result = 1;
}
result = 3;
rCCR &= ~I2C_CCR_CCR_MASK;
rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
@@ -163,7 +166,7 @@ i2c_reset(void)
rCR1 = 0;
/* set for DMA operation */
rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN;
/* set the frequency value in CR2 */
rCR2 &= ~I2C_CR2_FREQ_MASK;
@@ -171,8 +174,11 @@ i2c_reset(void)
/* set divisor and risetime for fast mode */
uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
if (result < 1)
if (result < 1) {
result = 1;
}
result = 3;
rCCR &= ~I2C_CCR_CCR_MASK;
rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
@@ -203,13 +209,16 @@ i2c_interrupt(int irq, FAR void *context)
case DIR_TX:
i2c_tx_complete();
break;
case DIR_RX:
i2c_rx_complete();
break;
default:
/* not currently transferring - must be a new txn */
break;
}
direction = DIR_NONE;
}
@@ -232,8 +241,9 @@ i2c_interrupt(int irq, FAR void *context)
}
/* clear any errors that might need it (this handles AF as well */
if (sr1 & I2C_SR1_ERRORMASK)
if (sr1 & I2C_SR1_ERRORMASK) {
rSR1 = 0;
}
return 0;
}
@@ -248,13 +258,13 @@ i2c_rx_setup(void)
*/
rx_len = 0;
stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_32BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_32BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
stm32_dmastart(rx_dma, NULL, NULL, false);
stm32_dmastart(rx_dma, NULL, NULL, false);
}
static void
@@ -269,8 +279,10 @@ i2c_rx_complete(void)
/* work out how many registers are being written */
unsigned count = (rx_len - 2) / 2;
if (count > 0) {
registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
} else {
/* no registers written, must be an address cycle */
uint16_t *regs;
@@ -278,9 +290,11 @@ i2c_rx_complete(void)
/* work out which registers are being addressed */
int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
if (ret == 0) {
tx_buf = (uint8_t *)regs;
tx_len = reg_count * 2;
} else {
tx_buf = junk_buf;
tx_len = sizeof(junk_buf);
@@ -306,16 +320,16 @@ i2c_tx_setup(void)
/*
* Note that we configure DMA in circular mode; this means that a too-long
* transfer will copy the buffer more than once, but that avoids us having
* to deal with bailing out of a transaction while the master is still
* to deal with bailing out of a transaction while the master is still
* babbling at us.
*/
stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
DMA_CCR_DIR |
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_8BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
DMA_CCR_DIR |
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_8BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
stm32_dmastart(tx_dma, NULL, NULL, false);
}
src/modules/px4iofirmware/mixer.cpp
+56 -43
View File
@@ -97,12 +97,13 @@ mixer_tick(void)
/* check that we are receiving fresh data from the FMU */
if ((system_state.fmu_data_received_time == 0) ||
hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
@@ -122,7 +123,7 @@ mixer_tick(void)
/* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
@@ -130,28 +131,29 @@ mixer_tick(void)
} else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* mix from FMU controls */
source = MIX_FMU;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
/* do not enter manual override if we asked for termination failsafe and FMU is lost */
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
/* do not enter manual override if we asked for termination failsafe and FMU is lost */
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
/* if allowed, mix from RC inputs directly */
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
} else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly up to available rc channels */
source = MIX_OVERRIDE_FMU_OK;
@@ -166,31 +168,32 @@ mixer_tick(void)
*
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
&& !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
should_arm_nothrottle = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Check if failsafe termination is set - if yes,
* set the force failsafe flag once entering the first
* failsafe condition.
*/
if ( /* if we have requested flight termination style failsafe (noreturn) */
if ( /* if we have requested flight termination style failsafe (noreturn) */
(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
/* and we ended up in a failsafe condition */
(source == MIX_FAILSAFE) &&
@@ -213,6 +216,7 @@ mixer_tick(void)
*/
if (source == MIX_FAILSAFE) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
@@ -244,7 +248,8 @@ mixer_tick(void)
in_mixer = false;
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
@@ -291,6 +296,7 @@ mixer_tick(void)
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
}
@@ -324,10 +330,11 @@ mixer_callback(uintptr_t handle,
switch (source) {
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_OVERRIDE:
@@ -335,17 +342,21 @@ mixer_callback(uintptr_t handle,
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
}
return -1;
case MIX_OVERRIDE_FMU_OK:
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_FAILSAFE:
@@ -357,15 +368,15 @@ mixer_callback(uintptr_t handle,
/* apply trim offsets for override channels */
if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_ROLL) {
control_index == actuator_controls_s::INDEX_ROLL) {
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_PITCH) {
control_index == actuator_controls_s::INDEX_PITCH) {
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_YAW) {
control_index == actuator_controls_s::INDEX_YAW) {
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
}
@@ -373,6 +384,7 @@ mixer_callback(uintptr_t handle,
/* limit output */
if (control > 1.0f) {
control = 1.0f;
} else if (control < -1.0f) {
control = -1.0f;
}
@@ -380,7 +392,7 @@ mixer_callback(uintptr_t handle,
/* motor spinup phase - lock throttle to zero */
if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
@@ -391,7 +403,7 @@ mixer_callback(uintptr_t handle,
/* only safety off, but not armed - set throttle as invalid */
if (should_arm_nothrottle && !should_arm) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* mark the throttle as invalid */
control = NAN_VALUE;
}
@@ -414,7 +426,7 @@ mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
@@ -444,7 +456,7 @@ mixer_handle_text(const void *buffer, size_t length)
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
@@ -470,6 +482,7 @@ mixer_handle_text(const void *buffer, size_t length)
/* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
@@ -497,13 +510,13 @@ mixer_handle_text(const void *buffer, size_t length)
static void
mixer_set_failsafe()
{
/*
/*
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
return;
}
src/modules/px4iofirmware/protocol.h
+9 -9
View File
@@ -46,7 +46,7 @@
*
* The first two bytes of each write select a page and offset address
* respectively. Subsequent reads and writes increment the offset within
* the page.
* the page.
*
* Some pages are read- or write-only.
*
@@ -56,7 +56,7 @@
* Writes to unimplemented registers are ignored. Reads from unimplemented
* registers return undefined values.
*
* As convention, values that would be floating point in other parts of
* As convention, values that would be floating point in other parts of
* the PX4 system are expressed as signed integer values scaled by 10000,
* e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
* SIGNED_TO_REG macros to convert between register representation and
@@ -66,7 +66,7 @@
* readable pages to be densely packed. Page numbers do not need to be
* packed.
*
* Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
* Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
* [2] denotes definitions specific to the PX4IOv2 board.
*/
@@ -218,14 +218,14 @@ enum { /* DSM bind states */
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
/* 8 */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* storage space of 12 occupied by CRC */
/* storage space of 12 occupied by CRC */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state - this is a non-data write and
hence index 12 can safely be used. */
@@ -339,8 +339,7 @@ struct IOPacket {
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{
static const uint8_t crc8_tab[256] __attribute__((unused)) = {
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
@@ -383,8 +382,9 @@ crc_packet(struct IOPacket *pkt)
uint8_t *p = (uint8_t *)pkt;
uint8_t c = 0;
while (p < end)
c = crc8_tab[c ^ *(p++)];
while (p < end) {
c = crc8_tab[c ^ * (p++)];
}
return c;
}
src/modules/px4iofirmware/px4io.c
+27 -18
View File
@@ -97,11 +97,12 @@ isr_debug(uint8_t level, const char *fmt, ...)
if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
return;
}
va_list ap;
va_start(ap, fmt);
vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
va_end(ap);
msg_next_in = (msg_next_in+1) % NUM_MSG;
msg_next_in = (msg_next_in + 1) % NUM_MSG;
msg_counter++;
}
@@ -113,11 +114,14 @@ show_debug_messages(void)
{
if (msg_counter != last_msg_counter) {
uint32_t n = msg_counter - last_msg_counter;
if (n > NUM_MSG) n = NUM_MSG;
if (n > NUM_MSG) { n = NUM_MSG; }
last_msg_counter = msg_counter;
while (n--) {
debug("%s", msg[msg_next_out]);
msg_next_out = (msg_next_out+1) % NUM_MSG;
msg_next_out = (msg_next_out + 1) % NUM_MSG;
}
}
}
@@ -135,7 +139,7 @@ ring_blink(void)
#ifdef GPIO_LED4
if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
LED_RING(1);
return;
}
@@ -151,7 +155,7 @@ ring_blink(void)
if (brightness_counter < max_brightness) {
bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1);
// XXX once led is PWM driven,
// remove the ! in the line below
@@ -195,7 +199,7 @@ static uint64_t reboot_time;
*/
void schedule_reboot(uint32_t time_delta_usec)
{
reboot_time = hrt_absolute_time() + time_delta_usec;
reboot_time = hrt_absolute_time() + time_delta_usec;
}
/**
@@ -203,9 +207,9 @@ void schedule_reboot(uint32_t time_delta_usec)
*/
static void check_reboot(void)
{
if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
up_systemreset();
}
if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
up_systemreset();
}
}
static void
@@ -215,12 +219,14 @@ calculate_fw_crc(void)
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
}
r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16;
}
int
@@ -308,7 +314,7 @@ user_start(int argc, char *argv[])
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*
*/
if (minfo.mxordblk < 600) {
@@ -320,10 +326,12 @@ user_start(int argc, char *argv[])
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
@@ -338,7 +346,8 @@ user_start(int argc, char *argv[])
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
@@ -354,14 +363,14 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
ring_blink();
check_reboot();
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
@@ -371,7 +380,7 @@ user_start(int argc, char *argv[])
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
src/modules/px4iofirmware/registers.c
+198 -133
View File
@@ -112,15 +112,14 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
*
* Raw RC input
*/
uint16_t r_page_raw_rc_input[] =
{
uint16_t r_page_raw_rc_input[] = {
[PX4IO_P_RAW_RC_COUNT] = 0,
[PX4IO_P_RAW_RC_FLAGS] = 0,
[PX4IO_P_RAW_RC_NRSSI] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_FRAME_COUNT] = 0,
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
[PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
/**
@@ -130,7 +129,7 @@ uint16_t r_page_raw_rc_input[] =
*/
uint16_t r_page_rc_input[] = {
[PX4IO_P_RC_VALID] = 0,
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
[PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
};
/**
@@ -146,8 +145,7 @@ uint16_t r_page_scratch[32];
*
* Setup registers
*/
volatile uint16_t r_page_setup[] =
{
volatile uint16_t r_page_setup[] = {
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
/* default to RSSI ADC functionality */
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
@@ -171,7 +169,7 @@ volatile uint16_t r_page_setup[] =
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
[PX4IO_P_SETUP_PWM_REVERSE] = 0,
[PX4IO_P_SETUP_TRIM_ROLL] = 0,
@@ -181,23 +179,23 @@ volatile uint16_t r_page_setup[] =
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
#else
#define PX4IO_P_SETUP_FEATURES_VALID 0
#endif
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -230,7 +228,7 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR
* PAGE 105
*
* Failsafe servo PWM values
*
*
* Disable pulses as default.
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
@@ -265,7 +263,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
switch (page) {
/* handle bulk controls input */
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
@@ -282,10 +280,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
break;
/* handle raw PWM input */
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
@@ -306,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
break;
/* handle setup for servo failsafe values */
/* handle setup for servo failsafe values */
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
@@ -316,8 +314,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* ignore 0 */
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
@@ -329,6 +329,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
@@ -340,8 +341,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MIN) {
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
} else {
r_page_servo_control_min[offset] = *values;
}
@@ -350,8 +353,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
num_values--;
values++;
}
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
/* copy channel data */
@@ -361,8 +365,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
} else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
} else {
r_page_servo_control_max[offset] = *values;
}
@@ -371,10 +377,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
num_values--;
values++;
}
break;
case PX4IO_PAGE_DISARMED_PWM:
{
case PX4IO_PAGE_DISARMED_PWM: {
/* flag for all outputs */
bool all_disarmed_off = true;
@@ -384,12 +390,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
if (*values == 0) {
/* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
all_disarmed_off = false;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
all_disarmed_off = false;
} else {
r_page_servo_disarmed[offset] = *values;
all_disarmed_off = false;
@@ -403,6 +412,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
if (all_disarmed_off) {
/* disable PWM output if disarmed */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
} else {
/* enable PWM output always */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
@@ -410,7 +420,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
/* handle text going to the mixer parser */
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
/* do not change the mixer if FMU is armed and IO's safety is off
* this state defines an active system. This check is done in the
@@ -419,19 +429,25 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
return mixer_handle_text(values, num_values * sizeof(*values));
default:
/* avoid offset wrap */
if ((offset + num_values) > 255)
if ((offset + num_values) > 255) {
num_values = 255 - offset;
}
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
if (registers_set_one(page, offset, *values)) {
return -1;
}
offset++;
values++;
}
break;
}
return 0;
}
@@ -448,19 +464,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_STATUS_FLAGS:
/*
/*
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
break;
default:
/* just ignore writes to other registers in this page */
break;
}
break;
case PX4IO_PAGE_SETUP:
@@ -472,36 +491,37 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
/* switch S.Bus output pin as needed */
#ifdef ENABLE_SBUS_OUT
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
/* disable the conflicting options with SBUS 1 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* disable the conflicting options with SBUS 2 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
}
#endif
#endif
/* disable the conflicting options with ADC RSSI */
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* apply changes */
@@ -551,9 +571,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (value < 25) {
value = 25;
}
if (value > 400) {
value = 400;
}
pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
break;
@@ -561,13 +583,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (value < 25) {
value = 25;
}
if (value > 400) {
value = 400;
}
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
@@ -598,10 +623,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
}
// we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK
// the reboot command
schedule_reboot(100000);
// we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK
// the reboot command
schedule_reboot(100000);
break;
case PX4IO_P_SETUP_DSM:
@@ -612,18 +637,21 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
break;
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
break;
case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
if (value > 650 && value < 2350) {
r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
}
break;
case PX4IO_P_SETUP_PWM_REVERSE:
@@ -639,93 +667,103 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
default:
return -1;
}
break;
case PX4IO_PAGE_RC_CONFIG: {
/**
* do not allow a RC config change while safety is off
*/
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
break;
}
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
if (channel >= PX4IO_RC_INPUT_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
switch (index) {
case PX4IO_P_RC_CONFIG_MIN:
case PX4IO_P_RC_CONFIG_CENTER:
case PX4IO_P_RC_CONFIG_MAX:
case PX4IO_P_RC_CONFIG_DEADZONE:
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
conf[index] = value;
break;
case PX4IO_P_RC_CONFIG_OPTIONS:
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
/* clear any existing RC disabled flag */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
/* should the channel be enabled? */
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
count++;
}
/* assert deadzone is sane */
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
/* sanity checks pass, enable channel */
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
/**
* do not allow a RC config change while safety is off
*/
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
break;
}
break;
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
return -1;
}
/* disable the channel until we have a chance to sanity-check it */
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
switch (index) {
case PX4IO_P_RC_CONFIG_MIN:
case PX4IO_P_RC_CONFIG_CENTER:
case PX4IO_P_RC_CONFIG_MAX:
case PX4IO_P_RC_CONFIG_DEADZONE:
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
conf[index] = value;
break;
case PX4IO_P_RC_CONFIG_OPTIONS:
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
/* clear any existing RC disabled flag */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
/* should the channel be enabled? */
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
count++;
}
/* assert deadzone is sane */
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
/* sanity checks pass, enable channel */
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
break;
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
}
break;
/* case PX4IO_RC_PAGE_CONFIG */
}
break;
/* case PX4IO_RC_PAGE_CONFIG */
}
case PX4IO_PAGE_TEST:
switch (offset) {
@@ -733,11 +771,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
LED_AMBER(value & 1);
break;
}
break;
default:
return -1;
}
return 0;
}
@@ -787,6 +827,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
*
*/
unsigned counts = adc_measure(ADC_VBATT);
if (counts != 0xffff) {
unsigned mV = (166460 + (counts * 45934)) / 10000;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
@@ -806,6 +847,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
configuration for their sensor
*/
unsigned counts = adc_measure(ADC_IBATT);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
@@ -815,6 +857,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
/* PX4IO_P_STATUS_VSERVO */
{
unsigned counts = adc_measure(ADC_VSERVO);
if (counts != 0xffff) {
// use 3:1 scaling on 3.3V ADC input
unsigned mV = counts * 9900 / 4096;
@@ -826,6 +869,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
/* PX4IO_P_STATUS_VRSSI */
{
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
// use 1:1 scaling on 3.3V ADC input
unsigned mV = counts * 3300 / 4096;
@@ -858,8 +902,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
}
SELECT_PAGE(r_page_scratch);
break;
@@ -872,15 +918,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
case PX4IO_PAGE_RC_INPUT:
SELECT_PAGE(r_page_rc_input);
break;
@@ -889,24 +939,31 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_SETUP:
SELECT_PAGE(r_page_setup);
break;
case PX4IO_PAGE_CONTROLS:
SELECT_PAGE(r_page_controls);
break;
case PX4IO_PAGE_RC_CONFIG:
SELECT_PAGE(r_page_rc_input_config);
break;
case PX4IO_PAGE_DIRECT_PWM:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
case PX4IO_PAGE_CONTROL_MIN_PWM:
SELECT_PAGE(r_page_servo_control_min);
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
@@ -918,12 +975,13 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
#undef SELECT_PAGE
#undef COPY_PAGE
last_page = page;
last_offset = offset;
last_page = page;
last_offset = offset;
/* if the offset is at or beyond the end of the page, we have no data */
if (offset >= *num_values)
if (offset >= *num_values) {
return -1;
}
/* correct the data pointer and count for the offset */
*values += offset;
@@ -943,8 +1001,10 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
/* get the channel mask for this rate group */
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
if (mask == 0) {
continue;
}
/* all channels in the group must be either default or alt-rate */
uint32_t alt = map & mask;
@@ -956,18 +1016,23 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
return;
}
} else {
/* set it - errors here are unexpected */
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) {
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) {
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
}
}
}
}
r_setup_pwm_rates = map;
r_setup_pwm_defaultrate = defaultrate;
r_setup_pwm_altrate = altrate;
src/modules/px4iofirmware/safety.c
+3 -1
View File
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
@@ -149,6 +149,7 @@ safety_check_button(void *arg)
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
pattern = LED_PATTERN_FMU_OK_TO_ARM;
@@ -176,6 +177,7 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
failsafe = !failsafe;
} else {
failsafe = false;
}
src/modules/px4iofirmware/serial.c
+30 -10
View File
@@ -131,14 +131,19 @@ interface_init(void)
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
#if 0 /* keep this for signal integrity testing */
for (;;) {
while (!(rSR & USART_SR_TXE))
;
rDR = 0xfa;
while (!(rSR & USART_SR_TXE))
;
rDR = 0xa0;
}
#endif
/* configure RX DMA and return to listening state */
@@ -153,6 +158,7 @@ rx_handle_packet(void)
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
@@ -170,11 +176,13 @@ rx_handle_packet(void)
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
dma_packet.count_code = PKT_CODE_SUCCESS;
}
return;
}
}
if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
@@ -185,17 +193,22 @@ rx_handle_packet(void)
if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
/* constrain reply to requested size */
if (count > PKT_MAX_REGS)
if (count > PKT_MAX_REGS) {
count = PKT_MAX_REGS;
if (count > PKT_COUNT(dma_packet))
}
if (count > PKT_COUNT(dma_packet)) {
count = PKT_COUNT(dma_packet);
}
/* copy reply registers into DMA buffer */
memcpy((void *)&dma_packet.regs[0], registers, count * 2);
dma_packet.count_code = count | PKT_CODE_SUCCESS;
}
return;
}
@@ -250,16 +263,22 @@ serial_interrupt(int irq, void *context)
(void)rDR; /* required to clear any of the interrupt status that brought us here */
if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
perf_count(pc_errors);
if (sr & USART_SR_ORE)
if (sr & USART_SR_ORE) {
perf_count(pc_ore);
if (sr & USART_SR_NE)
}
if (sr & USART_SR_NE) {
perf_count(pc_ne);
if (sr & USART_SR_FE)
}
if (sr & USART_SR_FE) {
perf_count(pc_fe);
}
/* send a line break - this will abort transmission/reception on the other end */
rCR1 |= USART_CR1_SBK;
@@ -270,7 +289,7 @@ serial_interrupt(int irq, void *context)
if (sr & USART_SR_IDLE) {
/*
/*
* If we saw an error, don't bother looking at the packet - it should have
* been aborted by the sender and will definitely be bad. Get the DMA reconfigured
* ready for their retry.
@@ -284,10 +303,11 @@ serial_interrupt(int irq, void *context)
/*
* The sender has stopped sending - this is probably the end of a packet.
* Check the received length against the length in the header to see if
* Check the received length against the length in the header to see if
* we have something that looks like a packet.
*/
unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
/* it was too short - possibly truncated */
src/modules/sensors/sensors.cpp
+99 -57
View File
@@ -167,7 +167,8 @@ public:
int start();
private:
static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
static const unsigned _rc_max_chan_count =
input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
@@ -227,7 +228,7 @@ private:
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
orb_advert_t _diff_pres_pub; /**< differential_pressure */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
@@ -366,7 +367,7 @@ private:
int init_sensor_class(const struct orb_metadata *meta, int *subs,
uint32_t *priorities, uint32_t *errcount);
uint32_t *priorities, uint32_t *errcount);
/**
* Update our local parameter cache.
@@ -489,10 +490,10 @@ Sensors::Sensors() :
_armed(false),
/* subscriptions */
_gyro_sub{-1, -1, -1},
_accel_sub{-1, -1, -1},
_mag_sub{-1, -1, -1},
_baro_sub{-1, -1, -1},
_gyro_sub{ -1, -1, -1},
_accel_sub{ -1, -1, -1},
_mag_sub{ -1, -1, -1},
_baro_sub{ -1, -1, -1},
_gyro_count(0),
_accel_count(0),
_mag_count(0),
@@ -588,7 +589,8 @@ Sensors::Sensors() :
/* RC to parameter mapping for changing parameters with RC */
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
char name[rc_parameter_map_s::PARAM_ID_LEN];
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d",
i + 1); // shifted by 1 because param name starts at 1
_parameter_handles.rc_map_param[i] = param_find(name);
}
@@ -641,7 +643,7 @@ Sensors::Sensors() :
(void)param_find("PWM_AUX_MAX");
(void)param_find("PWM_AUX_DISARMED");
(void)param_find("TRIG_MODE");
/* fetch initial parameter values */
parameters_update();
}
@@ -825,19 +827,20 @@ Sensors::parameters_update()
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("%s", paramerr);
} else if (_parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
#elif CONFIG_ARCH_BOARD_AEROCORE
_parameters.battery_voltage_scaling = 0.0063f;
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
_parameters.battery_voltage_scaling = 0.00459340659f;
#else
#else
/* ensure a missing default trips a low voltage lockdown */
_parameters.battery_voltage_scaling = 0.00001f;
#endif
#endif
}
/* scaling of ADC ticks to battery current */
@@ -1098,9 +1101,11 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
raw.gyro_timestamp[i] = gyro_report.timestamp;
if (i == 0) {
raw.timestamp = gyro_report.timestamp;
}
raw.gyro_errcount[i] = gyro_report.error_count;
raw.gyro_temp[i] = gyro_report.temperature;
}
@@ -1183,8 +1188,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
@@ -1265,7 +1270,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -1297,15 +1302,19 @@ Sensors::parameter_update_poll(bool forced)
if (failed) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
config_ok = true;
}
}
break;
}
}
@@ -1331,7 +1340,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -1363,15 +1372,19 @@ Sensors::parameter_update_poll(bool forced)
if (failed) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
config_ok = true;
}
}
break;
}
}
@@ -1404,7 +1417,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false;
/* run through all stored calibrations */
/* run through all stored calibrations */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */
failed = false;
@@ -1444,6 +1457,7 @@ Sensors::parameter_update_poll(bool forced)
/* reset param to -1 to indicate internal mag */
int32_t minus_one = MAG_ROT_VAL_INTERNAL;
param_set_no_notification(param_find(str), &minus_one);
} else {
int32_t mag_rot;
@@ -1488,15 +1502,19 @@ Sensors::parameter_update_poll(bool forced)
if (failed) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
config_ok = true;
}
}
break;
}
}
@@ -1764,6 +1782,7 @@ Sensors::set_params_from_rc()
}
float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) {
@@ -1895,24 +1914,30 @@ Sensors::rc_poll()
manual.timestamp = rc_input.timestamp_last_signal;
/* limit controls */
manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
_parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
_parameters.rc_posctl_inv);
manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,
_parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th,
_parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th,
_parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub != nullptr) {
@@ -1964,7 +1989,7 @@ Sensors::task_main_trampoline(int argc, char *argv[])
int
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
uint32_t *priorities, uint32_t *errcount)
uint32_t *priorities, uint32_t *errcount)
{
unsigned group_count = orb_group_count(meta);
@@ -1975,7 +2000,7 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
for (unsigned i = 0; i < group_count; i++) {
if (subs[i] < 0) {
subs[i] = orb_subscribe_multi(meta, i);
orb_priority(subs[i], (int32_t*)&priorities[i]);
orb_priority(subs[i], (int32_t *)&priorities[i]);
}
}
@@ -1988,27 +2013,40 @@ Sensors::task_main()
/* start individual sensors */
int ret = 0;
do { /* create a scope to handle exit with break */
ret = accel_init();
if (ret) break;
if (ret) { break; }
ret = gyro_init();
if (ret) break;
if (ret) { break; }
ret = mag_init();
if (ret) break;
if (ret) { break; }
ret = baro_init();
if (ret) break;
if (ret) { break; }
ret = adc_init();
if (ret) break;
if (ret) { break; }
break;
} while (0);
if (ret) {
warnx("sensor initialization failed");
_sensors_task = -1;
if (_fd_adc >=0) {
if (_fd_adc >= 0) {
px4_close(_fd_adc);
_fd_adc = -1;
}
return;
}
@@ -2016,28 +2054,31 @@ Sensors::task_main()
/* ensure no overflows can occur */
static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
/*
* do subscriptions
*/
unsigned gcount_prev = _gyro_count;
unsigned mcount_prev = _mag_count;
unsigned acount_prev = _accel_count;
unsigned bcount_prev = _baro_count;
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
&raw.baro_priority[0], &raw.baro_errcount[0]);
if (gcount_prev != _gyro_count ||
mcount_prev != _mag_count ||
@@ -2130,6 +2171,7 @@ Sensors::task_main()
/* if the secondary failed as well, go to the tertiary */
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) {
fds[0].fd = _gyro_sub[2];
} else {
fds[0].fd = _gyro_sub[1];
}
@@ -2150,16 +2192,16 @@ Sensors::task_main()
*/
if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
&raw.baro_priority[0], &raw.baro_errcount[0]);
_last_config_update = hrt_absolute_time();
@@ -2190,11 +2232,11 @@ Sensors::start()
/* start the task */
_sensors_task = px4_task_spawn_cmd("sensors",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
(px4_main_t)&Sensors::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
(px4_main_t)&Sensors::task_main_trampoline,
nullptr);
/* wait until the task is up and running or has failed */
while (_sensors_task > 0 && _task_should_exit) {
src/modules/simulator/CMakeLists.txt
+2
View File
@@ -47,6 +47,8 @@ px4_add_module(
${SIMULATOR_SRCS}
DEPENDS
platforms__common
git_jmavsim
git_gazebo
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/simulator/simulator_mavlink.cpp
+3 -3
View File
@@ -769,7 +769,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
return OK;
}
int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
@@ -794,10 +794,10 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
flow.quality = flow_mavlink->quality;
if (_flow_pub == nullptr) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
}
}
src/modules/systemlib/circuit_breaker_params.c
+14 -1
View File
@@ -133,4 +133,17 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
/**
* Circuit breaker for disabling buzzer
*
* Setting this parameter to 782097 will disable the buzzer audio notification.
*
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 782097
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_BUZZER, 0);
src/modules/systemlib/mixer/mixer_multirotor.cpp
+15 -4
View File
@@ -315,8 +315,13 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
// scale yaw if it violates limits. inform about yaw limit reached
if (out < 0.0f) {
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
}
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
@@ -326,8 +331,14 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
// allow to reduce thrust to get some yaw response
float thrust_reduction = fminf(0.15f, out - 1.0f);
thrust -= thrust_reduction;
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
}
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
src/modules/uORB/Publication.cpp
+41 -14
View File
@@ -50,37 +50,63 @@
#include "topics/encoders.h"
#include "topics/tecs_status.h"
#include "topics/rc_channels.h"
#include "topics/filtered_bottom_flow.h"
#include <px4_defines.h>
namespace uORB
{
template<class T>
Publication<T>::Publication(
const struct orb_metadata *meta,
List<PublicationNode *> *list) :
T(), // initialize data structure to zero
PublicationNode(meta, list)
PublicationBase::PublicationBase(const struct orb_metadata *meta,
int priority) :
_meta(meta),
_priority(priority),
_instance(),
_handle(nullptr)
{
}
template<class T>
Publication<T>::~Publication() {}
template<class T>
void *Publication<T>::getDataVoidPtr()
void PublicationBase::update(void *data)
{
return (void *)(T *)(this);
if (_handle != nullptr) {
int ret = orb_publish(getMeta(), getHandle(), data);
if (ret != PX4_OK) { warnx("publish fail"); }
} else {
orb_advert_t handle;
if (_priority > 0) {
handle = orb_advertise_multi(
getMeta(), data,
&_instance, _priority);
} else {
handle = orb_advertise(getMeta(), data);
}
if (int64_t(handle) != PX4_ERROR) {
setHandle(handle);
} else {
warnx("advert fail");
}
}
}
PublicationBase::~PublicationBase()
{
}
PublicationNode::PublicationNode(const struct orb_metadata *meta,
int priority,
List<PublicationNode *> *list) :
PublicationBase(meta)
PublicationBase(meta, priority)
{
if (list != nullptr) { list->add(this); }
}
// explicit template instantiation
template class __EXPORT Publication<vehicle_attitude_s>;
template class __EXPORT Publication<vehicle_local_position_s>;
template class __EXPORT Publication<vehicle_global_position_s>;
@@ -94,5 +120,6 @@ template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
template class __EXPORT Publication<rc_channels_s>;
template class __EXPORT Publication<filtered_bottom_flow_s>;
}
src/modules/uORB/Publication.hpp
+37 -43
View File
@@ -42,13 +42,13 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
/**
* Base publication warapper class, used in list traversal
* Base publication wrapper class, used in list traversal
* of various publications.
*/
class __EXPORT PublicationBase
@@ -58,50 +58,40 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub/sub, 0-based, -1 means
* don't publish as multi
*/
PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(nullptr)
{
}
PublicationBase(const struct orb_metadata *meta,
int priority = -1);
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void *data)
{
if (_handle != nullptr) {
orb_publish(getMeta(), getHandle(), data);
} else {
setHandle(orb_advertise(getMeta(), data));
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~PublicationBase()
{
}
virtual ~PublicationBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
orb_advert_t getHandle() { return _handle; }
protected:
// disallow copy
PublicationBase(const PublicationBase &other);
// disallow assignment
PublicationBase &operator=(const PublicationBase &other);
// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _priority;
int _instance;
orb_advert_t _handle;
private:
// forbid copy
PublicationBase(const PublicationBase &) : _meta(), _handle() {};
// forbid assignment
PublicationBase &operator = (const PublicationBase &);
};
/**
@@ -121,13 +111,14 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
PublicationNode(const struct orb_metadata *meta,
int priority = -1,
List<PublicationNode *> *list = nullptr);
/**
@@ -142,7 +133,6 @@ public:
*/
template<class T>
class __EXPORT Publication :
public T, // this must be first!
public PublicationNode
{
public:
@@ -151,33 +141,37 @@ public:
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
Publication(const struct orb_metadata *meta,
List<PublicationNode *> *list = nullptr);
int priority = -1,
List<PublicationNode *> *list = nullptr) :
PublicationNode(meta, priority, list),
_data()
{
}
/**
* Deconstructor
**/
virtual ~Publication();
virtual ~Publication() {};
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
* This function gets the T struct
* */
T &get() { return _data; }
/**
* Create an update function that uses the embedded struct.
*/
void update()
{
PublicationBase::update(getDataVoidPtr());
PublicationBase::update((void *)(&_data));
}
private:
T _data;
};
} // namespace uORB
src/modules/uORB/Subscription.cpp
+74 -16
View File
@@ -54,37 +54,89 @@
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/rc_channels.h"
#include "topics/battery_status.h"
#include "topics/optical_flow.h"
#include "topics/distance_sensor.h"
#include "topics/home_position.h"
#include "topics/vehicle_control_mode.h"
#include "topics/actuator_armed.h"
#include "topics/att_pos_mocap.h"
#include "topics/vision_position_estimate.h"
#include <px4_defines.h>
namespace uORB
{
template<class T>
Subscription<T>::Subscription(
const struct orb_metadata *meta,
unsigned interval,
List<SubscriptionNode *> *list) :
T(), // initialize data structure to zero
SubscriptionNode(meta, interval, list)
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
unsigned interval, unsigned instance) :
_meta(meta),
_instance(instance),
_handle()
{
if (_instance > 0) {
_handle = orb_subscribe_multi(
getMeta(), instance);
} else {
_handle = orb_subscribe(getMeta());
}
if (_handle < 0) { warnx("sub failed"); }
orb_set_interval(getHandle(), interval);
}
bool SubscriptionBase::updated()
{
bool isUpdated = false;
int ret = orb_check(_handle, &isUpdated);
if (ret != PX4_OK) { warnx("orb check failed"); }
return isUpdated;
}
void SubscriptionBase::update(void *data)
{
if (updated()) {
int ret = orb_copy(_meta, _handle, data);
if (ret != PX4_OK) { warnx("orb copy failed"); }
}
}
SubscriptionBase::~SubscriptionBase()
{
int ret = orb_unsubscribe(_handle);
if (ret != PX4_OK) { warnx("orb unsubscribe failed"); }
}
template <class T>
Subscription<T>::Subscription(const struct orb_metadata *meta,
unsigned interval,
int instance,
List<SubscriptionNode *> *list) :
SubscriptionNode(meta, interval, instance, list),
_data() // initialize data structure to zero
{
}
template<class T>
Subscription<T>::~Subscription() {}
template<class T>
void *Subscription<T>::getDataVoidPtr()
template <class T>
Subscription<T>::~Subscription()
{
return (void *)(T *)(this);
}
template<class T>
T Subscription<T>::getData()
template <class T>
void Subscription<T>::update()
{
return T(*this);
SubscriptionBase::update((void *)(&_data));
}
template <class T>
const T &Subscription<T>::get() { return _data; }
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
@@ -104,5 +156,11 @@ template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
template class __EXPORT Subscription<rc_channels_s>;
template class __EXPORT Subscription<vehicle_control_mode_s>;
template class __EXPORT Subscription<actuator_armed_s>;
template class __EXPORT Subscription<battery_status_s>;
template class __EXPORT Subscription<home_position_s>;
template class __EXPORT Subscription<optical_flow_s>;
template class __EXPORT Subscription<distance_sensor_s>;
template class __EXPORT Subscription<att_pos_mocap_s>;
template class __EXPORT Subscription<vision_position_estimate_s>;
} // namespace uORB
src/modules/uORB/Subscription.hpp
+31 -45
View File
@@ -42,6 +42,7 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
@@ -60,43 +61,29 @@ public:
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
*
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
*/
SubscriptionBase(const struct orb_metadata *meta,
unsigned interval=0) :
_meta(meta),
_handle() {
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
unsigned interval = 0, unsigned instance = 0);
/**
* Check if there is a new update.
* */
bool updated() {
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
bool updated();
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void * data) {
if (updated()) {
orb_copy(_meta, _handle, data);
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~SubscriptionBase() {
orb_unsubscribe(_handle);
}
virtual ~SubscriptionBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
@@ -105,12 +92,13 @@ protected:
void setHandle(int handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _instance;
int _handle;
private:
// forbid copy
SubscriptionBase(const SubscriptionBase& other);
// forbid assignment
SubscriptionBase& operator = (const SubscriptionBase &);
// disallow copy
SubscriptionBase(const SubscriptionBase &other);
// disallow assignment
SubscriptionBase &operator=(const SubscriptionBase &other);
};
/**
@@ -119,7 +107,7 @@ private:
typedef SubscriptionBase SubscriptionTiny;
/**
* The publication base class as a list node.
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionNode :
@@ -130,20 +118,22 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
SubscriptionNode(const struct orb_metadata *meta,
unsigned interval=0,
List<SubscriptionNode *> * list=nullptr) :
SubscriptionBase(meta, interval),
_interval(interval) {
if (list != nullptr) list->add(this);
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr) :
SubscriptionBase(meta, interval, instance),
_interval(interval)
{
if (list != nullptr) { list->add(this); }
}
/**
@@ -164,7 +154,6 @@ protected:
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionNode
{
public:
@@ -179,8 +168,10 @@ public:
* list during construction
*/
Subscription(const struct orb_metadata *meta,
unsigned interval=0,
List<SubscriptionNode *> * list=nullptr);
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr);
/**
* Deconstructor
*/
@@ -190,19 +181,14 @@ public:
/**
* Create an update function that uses the embedded struct.
*/
void update() {
SubscriptionBase::update(getDataVoidPtr());
}
void update();
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
T getData();
* This function gets the T struct data
* */
const T &get();
private:
T _data;
};
} // namespace uORB
src/modules/uORB/uORB.cpp
+1
View File
@@ -269,6 +269,7 @@ int orb_exists(const struct orb_metadata *meta, int instance)
int orb_group_count(const struct orb_metadata *meta)
{
unsigned group_count = 0;
while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {};
return group_count;
src/modules/uORB/uORBDevices_nuttx.cpp
-2
View File
@@ -36,7 +36,6 @@
#include <fcntl.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <algorithm>
#include "uORBDevices_nuttx.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
@@ -684,4 +683,3 @@ uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath)
return rc;
}
src/modules/uORB/uORBMain.cpp
+63 -60
View File
@@ -45,83 +45,86 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
static uORB::DeviceMaster *g_dev = nullptr;
static void usage()
{
warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
}
int
uorb_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -EINVAL;
}
if (argc < 2) {
usage();
return -EINVAL;
}
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
warnx("already loaded");
/* user wanted to start uorb, its already running, no error */
return 0;
}
if (g_dev != nullptr) {
warnx("already loaded");
/* user wanted to start uorb, its already running, no error */
return 0;
}
/* create the driver */
g_dev = new uORB::DeviceMaster(uORB::PUBSUB);
/* create the driver */
g_dev = new uORB::DeviceMaster(uORB::PUBSUB);
if (g_dev == nullptr) {
warnx("driver alloc failed");
return -ENOMEM;
}
if (g_dev == nullptr) {
warnx("driver alloc failed");
return -ENOMEM;
}
if (OK != g_dev->init()) {
warnx("driver init failed");
delete g_dev;
g_dev = nullptr;
return -EIO;
}
if (OK != g_dev->init()) {
warnx("driver init failed");
delete g_dev;
g_dev = nullptr;
return -EIO;
}
return OK;
}
return OK;
}
#ifndef __PX4_QURT
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
{
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
return t.test();
}
/*
* Test the latency.
*/
if (!strcmp(argv[1], "latency_test")) {
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
return t.test();
}
/*
* Test the latency.
*/
if (!strcmp(argv[1], "latency_test")) {
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
if (argc > 2 && !strcmp(argv[2], "medium")) {
return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
} else if (argc > 2 && !strcmp(argv[2], "large")) {
return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
} else {
return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
}
}
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
if (argc > 2 && !strcmp(argv[2], "medium")) {
return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
} else if (argc > 2 && !strcmp(argv[2], "large")) {
return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
} else {
return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
}
}
#endif
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status"))
{
return OK;
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status")) {
return OK;
}
usage();
return -EINVAL;
usage();
return -EINVAL;
}
src/modules/uORB/uORBUtils.cpp
+25 -24
View File
@@ -37,45 +37,46 @@
int uORB::Utils::node_mkpath
(
char *buf,
Flavor f,
const struct orb_metadata *meta,
int *instance
char *buf,
Flavor f,
const struct orb_metadata *meta,
int *instance
)
{
unsigned len;
unsigned len;
unsigned index = 0;
unsigned index = 0;
if (instance != nullptr) {
index = *instance;
}
if (instance != nullptr) {
index = *instance;
}
len = snprintf(buf, orb_maxpath, "/%s/%s%d",
(f == PUBSUB) ? "obj" : "param",
meta->o_name, index);
len = snprintf(buf, orb_maxpath, "/%s/%s%d",
(f == PUBSUB) ? "obj" : "param",
meta->o_name, index);
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
return OK;
return OK;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int uORB::Utils::node_mkpath(char *buf, Flavor f,
const char *orbMsgName )
const char *orbMsgName)
{
unsigned len;
unsigned len;
unsigned index = 0;
unsigned index = 0;
len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
orbMsgName, index );
len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
orbMsgName, index);
if (len >= orb_maxpath)
return -ENAMETOOLONG;
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
return OK;
return OK;
}
src/modules/uORB/uORBUtils.hpp
+12 -12
View File
@@ -38,24 +38,24 @@
namespace uORB
{
class Utils;
class Utils;
}
class uORB::Utils
{
public:
static int node_mkpath
(
char *buf,
Flavor f,
const struct orb_metadata *meta,
int *instance = nullptr
);
static int node_mkpath
(
char *buf,
Flavor f,
const struct orb_metadata *meta,
int *instance = nullptr
);
/**
* same as above except this generators the path based on the string.
*/
static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
/**
* same as above except this generators the path based on the string.
*/
static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
};