From dc7369a1d0aca5c24b8523b2540f7a76a13b6aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:05:24 +0100 Subject: [PATCH 001/109] EKF: Report horizontal acceleration --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..5505b4c239 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -852,7 +852,9 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Accelerations in Body Frame */ _ctrl_state.x_acc = _ekf->accel.x; _ctrl_state.y_acc = _ekf->accel.y; - _ctrl_state.z_acc = _ekf->accel.z; + _ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13]; + + _ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal(); /* Velocity in Body Frame */ Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6eb325d531..c0a51afc0a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -382,6 +382,8 @@ public: void get_covariance(float c[28]); + float getAccNavMagHorizontal() { return _accNavMagHorizontal; } + protected: /** From f02dc3c95f716277a319699aeea233343574b8eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:06:00 +0100 Subject: [PATCH 002/109] Fixed wing land detector: Use control state, use horizontal acceleration for takeoff detect --- .../land_detector/FixedwingLandDetector.cpp | 63 +++++++++++-------- .../land_detector/FixedwingLandDetector.h | 14 +++-- src/modules/land_detector/LandDetector.cpp | 7 ++- .../land_detector/land_detector_params.c | 13 ++++ 4 files changed, 63 insertions(+), 34 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f8066241c5..7614ccd3d1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -36,6 +36,7 @@ * Land detection algorithm for fixedwings * * @author Johan Jansen + * @author Lorenz Meier */ #include "FixedwingLandDetector.h" @@ -48,30 +49,31 @@ 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) + _controlStateSub(-1), + _controlState{}, + _vehicleStatusSub(-1), + _armingSub(-1), + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _accel_x_integral(0.0f), + _lastTime(0), + _lastXAccel(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); + _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX"); } void FixedwingLandDetector::initialize() { // Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); @@ -80,14 +82,16 @@ void FixedwingLandDetector::initialize() void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() { + _lastXAccel = _controlState.x_acc; + _lastTime = _controlState.timestamp; + // First poll for new data from our subscriptions updateSubscriptions(); @@ -99,29 +103,35 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - 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); + if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel * + _controlState.x_vel + _controlState.y_vel * _controlState.y_vel); if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } - val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel); if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } - } - if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + + if (_lastTime != 0) { + /* a leaking integrator prevents biases from building up, but + * gives a mostly correct response for short impulses + */ + _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; + } } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate - && _airspeed_filtered < _params.maxAirSpeed) { + && _airspeed_filtered < _params.maxAirSpeed + && _accel_x_integral < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -133,6 +143,8 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } + _lastTime = _controlState.timestamp; + return landDetected; } @@ -151,5 +163,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 325faee794..cce99cb9fc 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,8 +42,7 @@ #define __FIXED_WING_LAND_DETECTOR_H__ #include "LandDetector.h" -#include -#include +#include #include #include #include @@ -83,21 +82,21 @@ private: param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; + param_t maxIntVelocity; } _paramHandle; struct { float maxVelocity; float maxClimbRate; float maxAirSpeed; + float maxIntVelocity; } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; + int _controlStateSub; /**< notification of local position */ + struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; - struct airspeed_s _airspeed; struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; int _parameterSub; @@ -105,6 +104,9 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; + float _accel_x_integral; + uint64_t _lastTime; + float _lastXAccel; uint64_t _landDetectTrigger; }; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 3040e23407..5140a2809c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,14 +57,14 @@ _work{} { LandDetector::~LandDetector() { - work_cancel(LPWORK, &_work); + work_cancel(HPWORK, &_work); _taskShouldExit = true; } int LandDetector::start() { /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); return 0; } @@ -107,10 +107,11 @@ void LandDetector::cycle() // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); + //warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF"); } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index d9201eb18a..690467b97c 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); */ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); +/** + * Fixedwing max short-term velocity + * + * Maximum velocity integral in flight direction allowed in the landed state (m/s) + * + * @min 2 + * @max 10 + * @unit m/s + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); + /** * Airspeed max * From 515d81b8d65dbab25aadf7543893621c918c9a82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:13:56 +0100 Subject: [PATCH 003/109] Land detector: Move back to more agile raw airspeed --- src/modules/land_detector/FixedwingLandDetector.cpp | 9 +++++++-- src/modules/land_detector/FixedwingLandDetector.h | 5 ++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7614ccd3d1..31c1195281 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -50,11 +50,13 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _controlStateSub(-1), - _controlState{}, _vehicleStatusSub(-1), _armingSub(-1), + _airspeedSub(-1), + _controlState{}, _vehicleStatus{}, _arming{}, + _airspeed{}, _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -76,6 +78,7 @@ void FixedwingLandDetector::initialize() _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); updateParameterCache(true); } @@ -85,6 +88,7 @@ void FixedwingLandDetector::updateSubscriptions() orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() @@ -117,7 +121,7 @@ bool FixedwingLandDetector::update() _velocity_z_filtered = val; } - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; if (_lastTime != 0) { /* a leaking integrator prevents biases from building up, but @@ -125,6 +129,7 @@ bool FixedwingLandDetector::update() */ _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; } + } // crude land detector for fixedwing diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index cce99cb9fc..32179c8698 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -46,6 +46,7 @@ #include #include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -94,11 +95,13 @@ private: private: int _controlStateSub; /**< notification of local position */ - struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; + int _airspeedSub; + struct control_state_s _controlState; /**< the result from local position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; + struct airspeed_s _airspeed; int _parameterSub; float _velocity_xy_filtered; From de46d8e872a3c6a6b6ee65669b036dfd6760b859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:17:50 +0100 Subject: [PATCH 004/109] Land detector: Code cleanup --- .../land_detector/FixedwingLandDetector.cpp | 20 +++++-------------- .../land_detector/FixedwingLandDetector.h | 4 +--- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 31c1195281..cff46c1a3f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -61,9 +61,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), - _accel_x_integral(0.0f), - _lastTime(0), - _lastXAccel(0.0f), + _accel_horz_lp(0.0f), _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); @@ -93,9 +91,6 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - _lastXAccel = _controlState.x_acc; - _lastTime = _controlState.timestamp; - // First poll for new data from our subscriptions updateSubscriptions(); @@ -123,12 +118,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - if (_lastTime != 0) { - /* a leaking integrator prevents biases from building up, but - * gives a mostly correct response for short impulses - */ - _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; - } + // a leaking lowpass prevents biases from building up, but + // gives a mostly correct response for short impulses + _accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f; } @@ -136,7 +128,7 @@ bool FixedwingLandDetector::update() if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed - && _accel_x_integral < _params.maxIntVelocity) { + && _accel_horz_lp < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -148,8 +140,6 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } - _lastTime = _controlState.timestamp; - return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 32179c8698..5c5b18a9e5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -107,9 +107,7 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; - float _accel_x_integral; - uint64_t _lastTime; - float _lastXAccel; + float _accel_horz_lp; uint64_t _landDetectTrigger; }; From f10bce2a631776412ad3badfbe840c71ee942277 Mon Sep 17 00:00:00 2001 From: sanderux Date: Thu, 19 Nov 2015 04:56:33 +0100 Subject: [PATCH 005/109] Added V-Tail VTOL config and mixer --- .../init.d/13007_vtol_AAVVT_quad | 51 ++++++++++++++++ ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix | 59 +++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix diff --git a/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad new file mode 100644 index 0000000000..2d48670e71 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad @@ -0,0 +1,51 @@ +#!nsh +# +# @name Generic AAVVT v-tail plane airframe with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Sander Smeets +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 + +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAVVT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix new file mode 100644 index 0000000000..86581585c2 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix @@ -0,0 +1,59 @@ +Aileron/v-tail/throttle VTOL mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator) and throttle using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU +AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 6 -10000 -10000 0 -10000 10000 + + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 2 -7000 -7000 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + From 4ee04e35308b91263479a164fb76a01383372d1d Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Mon, 23 Nov 2015 22:00:28 +0200 Subject: [PATCH 006/109] multirotor mixer: add H configuration --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 3 +++ src/modules/systemlib/mixer/multi_tables.py | 9 ++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 38074f7ae5..d46906385b 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -150,6 +150,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorGeometry::QUAD_X; + } else if (!strcmp(geomname, "4h")) { + geometry = MultirotorGeometry::QUAD_H; + } else if (!strcmp(geomname, "4v")) { geometry = MultirotorGeometry::QUAD_V; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index e01fcaf8f2..4ca40a2591 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -62,6 +62,13 @@ quad_x = [ [135, CW], ] +quad_h = [ + [ 45, CW], + [-135, CW], + [-45, CCW], + [135, CCW], +] + quad_plus = [ [ 90, CCW], [ -90, CCW], @@ -162,7 +169,7 @@ tri_y = [ ] -tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] +tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y] def variableName(variable): for variableName, value in list(globals().items()): From 27b16375598957121c98d5e0752271deb1561a07 Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Mon, 23 Nov 2015 22:00:56 +0200 Subject: [PATCH 007/109] Mixers: add mixer for H configuration --- ROMFS/px4fmu_common/mixers/quad_h.main.mix | 26 ++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/quad_h.main.mix diff --git a/ROMFS/px4fmu_common/mixers/quad_h.main.mix b/ROMFS/px4fmu_common/mixers/quad_h.main.mix new file mode 100644 index 0000000000..4a177d2ff0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_h.main.mix @@ -0,0 +1,26 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the H configuration. All controls +are mixed 100%. + +R: 4h 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 99921db5cfbb5bcde4a4bf6134a895dbb2a25a2a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 15:34:51 -1000 Subject: [PATCH 008/109] Temporary (or not) fix calibration failure due Due to the uOrb publishing rate difference beteen the mpu9250 driver and the mpu6000 driver on the ICM-20608-G. The latter driver has an integrator output the limit the publishing rate --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 1fcd48b088..a18a13dfa0 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -137,7 +137,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } for (unsigned s = 0; s < max_gyros; s++) { - if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 4) { mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } From fff75f60296a5eb06deb73375ca40db9f39e6b31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Nov 2015 11:48:42 +0100 Subject: [PATCH 009/109] Travis CI: Workaround for Homebrew Emacs fail --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2839e52437..e55e2cd152 100644 --- a/.travis.yml +++ b/.travis.yml @@ -59,7 +59,7 @@ before_install: ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 - && brew update + && brew update; brew update && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends From b7dfa3a9d0a592e178d3f57e8c3966bccbf22afc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 22:58:14 +0100 Subject: [PATCH 010/109] made inav compile with c++ --- src/modules/position_estimator_inav/CMakeLists.txt | 9 ++++++--- .../{inertial_filter.c => inertial_filter.cpp} | 0 ...nav_main.c => position_estimator_inav_main.cpp} | 14 +++++++------- ...params.c => position_estimator_inav_params.cpp} | 0 4 files changed, 13 insertions(+), 10 deletions(-) rename src/modules/position_estimator_inav/{inertial_filter.c => inertial_filter.cpp} (100%) rename src/modules/position_estimator_inav/{position_estimator_inav_main.c => position_estimator_inav_main.cpp} (99%) rename src/modules/position_estimator_inav/{position_estimator_inav_params.c => position_estimator_inav_params.cpp} (100%) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 60429f5d22..2f14727509 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -30,18 +30,21 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(MODULE_CFLAGS ) if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() + px4_add_module( MODULE modules__position_estimator_inav MAIN position_estimator_inav STACK 1200 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS - position_estimator_inav_main.c - position_estimator_inav_params.c - inertial_filter.c + position_estimator_inav_main.cpp + position_estimator_inav_params.cpp + inertial_filter.cpp DEPENDS platforms__common ) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.cpp similarity index 100% rename from src/modules/position_estimator_inav/inertial_filter.c rename to src/modules/position_estimator_inav/inertial_filter.cpp diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp similarity index 99% rename from src/modules/position_estimator_inav/position_estimator_inav_main.c rename to src/modules/position_estimator_inav/position_estimator_inav_main.cpp index d8f29f1082..7316da727f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -95,7 +95,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distan static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s -__EXPORT int position_estimator_inav_main(int argc, char *argv[]); +extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); int position_estimator_inav_thread_main(int argc, char *argv[]); @@ -390,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); - px4_pollfd_struct_t fds_init[1] = { - { .fd = sensor_combined_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds_init[1]; + fds_init[0].fd = sensor_combined_sub; + fds_init[0].events = POLLIN; /* wait for initial baro value */ bool wait_baro = true; @@ -434,9 +434,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - px4_pollfd_struct_t fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; + px4_pollfd_struct_t fds[1]; + fds[0].fd = vehicle_attitude_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.cpp similarity index 100% rename from src/modules/position_estimator_inav/position_estimator_inav_params.c rename to src/modules/position_estimator_inav/position_estimator_inav_params.cpp From a87ffe9bf3e7dbc81859e7e7297acfdfb2f81d1a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Nov 2015 23:17:44 +0100 Subject: [PATCH 011/109] added terrain estimator to inav --- .../position_estimator_inav_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 7316da727f..f97b7e944e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -74,6 +74,7 @@ #include #include +#include #include "position_estimator_inav_params.h" #include "inertial_filter.h" @@ -397,6 +398,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial baro value */ bool wait_baro = true; + TerrainEstimator *terrain_estimator = new TerrainEstimator(); + thread_running = true; while (wait_baro && !thread_should_exit) { @@ -1228,6 +1231,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } + /* run terrain estimator */ + terrain_estimator->predict(dt, &att, &sensor, &lidar); + terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); + if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1318,6 +1325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.eph = eph; global_pos.epv = epv; + if (terrain_estimator->is_valid()) { + global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); + global_pos.terrain_alt_valid = true; + } else { + global_pos.terrain_alt_valid = false; + } + if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); From 67dd28e2c4e4e6fb39c345e74c40759baa3f5500 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 10:30:18 +0100 Subject: [PATCH 012/109] update distance sensor separate from flow --- .../position_estimator_inav_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index f97b7e944e..57a118a9e4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -526,9 +526,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_check(optical_flow_sub, &updated); orb_check(distance_sensor_sub, &updated2); + /* update lidar separately, needed by terrain estimator */ + if (updated2) { + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); + } + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; From 6bec773423747f24fa815fdbea7eb9c279dd8e33 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 24 Nov 2015 13:37:06 +0100 Subject: [PATCH 013/109] changed isfinite to PX4_ISFINITE --- .../position_estimator_inav_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 57a118a9e4..72b250ebe9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1048,7 +1048,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += R_gps[j][i] * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1088,7 +1088,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1113,7 +1113,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } - if (isfinite(c)) { + if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } @@ -1121,7 +1121,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1148,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } - if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1167,7 +1167,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); @@ -1214,7 +1214,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); From 78ace92530894ac767714b05c75021d27787b6a9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 24 Nov 2015 16:57:19 +0100 Subject: [PATCH 014/109] reset position hold flag --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a26a65d468..6776d78871 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1141,6 +1141,16 @@ MulticopterPositionControl::task_main() _run_pos_control = true; _run_alt_control = true; + // reset the horizontal and vertical position hold flags for non-manual modes + // or if position is not controlled + if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { + _pos_hold_engaged = false; + } + + if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { + _alt_hold_engaged = false; + } + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ From 483cb1115711d252cb8e7b3bf06acf06734fce9c Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 23 Oct 2015 11:18:09 -0400 Subject: [PATCH 015/109] Fix comparison error in RC+GPS triggered FTS failsafe check This bug would cause loss of RC + loss of GPS to trigger a FTS when flying in non-manual modes with a good data link --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49e6459b03..851529bb54 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2320,7 +2320,7 @@ int commander_thread_main(int argc, char *argv[]) if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || - status.main_state !=vehicle_status_s::MAIN_STATE_STAB || + status.main_state ==vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || From 56a68094865939006c7e709ac42152e03660ec21 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 24 Nov 2015 18:05:42 -0500 Subject: [PATCH 016/109] HIL calculate TAS from IAS --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..bf79b6e09f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1325,8 +1325,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature); airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = ias; From e5b2c652e22d174ea4f3b2f3ddd5c5877e359a33 Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 25 Nov 2015 10:28:19 +0100 Subject: [PATCH 017/109] channel airspeed meas over control state for q estimator --- .../attitude_estimator_q_main.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 4b4fba54c8..f6037564e7 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ private: int _params_sub = -1; int _vision_sub = -1; int _mocap_sub = -1; + int _airspeed_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; orb_advert_t _ctrl_state_pub = nullptr; @@ -161,6 +163,8 @@ private: att_pos_mocap_s _mocap = {}; Vector<3> _mocap_hdg; + airspeed_s _airspeed = {}; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -292,6 +296,8 @@ void AttitudeEstimatorQ::task_main() _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -473,6 +479,14 @@ void AttitudeEstimatorQ::task_main() _mocap_hdg = Rmoc.transposed() * v; } + // Update airspeed + bool airspeed_updated = false; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + } + // Check for timeouts on data if (_ext_hdg_mode == 1) { _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); @@ -586,6 +600,9 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + /* Airspeed - take airspeed measurement directly here as no wind is estimated */ + ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); From 1f500413117a850fb669b2bc4d9a8bdbab20b2dc Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:23:01 +0200 Subject: [PATCH 018/109] added flag indicating if vtol is doing a transition Conflicts: msg/vtol_vehicle_status.msg --- msg/vehicle_status.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 60ab34e90d..b35d0cab60 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -113,6 +113,7 @@ uint32 system_id # system id, inspired by MAVLink's system ID field uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter +bool vtol_in_transition # True if VTOL is doing a transition bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode From d55ccd96c69f35880bc7927b10a8d5768591088b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:24:57 +0200 Subject: [PATCH 019/109] let vtol attitude control module publish attitude setpoint during transition Conflicts: src/modules/commander/commander.cpp src/modules/fw_att_control/fw_att_control_main.cpp src/modules/mc_pos_control/mc_pos_control_main.cpp --- src/modules/commander/commander.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 851529bb54..20efb4db3c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1599,7 +1599,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + /* Make sure that this is only adjusted if vehicle really is of type vtol */ if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6776d78871..53eff5977f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1610,7 +1610,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, - * in this case the attitude setpoint is published by the mavlink app + * in this case the attitude setpoint is published by the mavlink app. Also do not publish + * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate + * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || From 0fa8a5286b152c8b37075c0c6a973ed0a28bb218 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Aug 2015 16:25:27 +0200 Subject: [PATCH 020/109] tiltrotor: publish attitude setpoint when doing a transition Conflicts: src/modules/vtol_att_control/vtol_att_control_main.h --- src/modules/vtol_att_control/tiltrotor.cpp | 25 +++++++++++++++++++ .../vtol_att_control_main.cpp | 18 ++++++++++--- .../vtol_att_control/vtol_att_control_main.h | 2 ++ src/modules/vtol_att_control/vtol_type.h | 4 +++ 4 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 270aa065a9..c4fa37eb2c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _flag_was_in_trans_mode = false; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); @@ -292,6 +294,12 @@ void Tiltrotor::update_fw_state() void Tiltrotor::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { @@ -354,6 +362,23 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0; + _v_att_sp->pitch_body = 0; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->thrust = _throttle_transition; + + math::Matrix<3,3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + _v_att_sp->R_valid = true; + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + _v_att_sp->q_d_valid = true; + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } void Tiltrotor::update_external_state() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0248a20727..5725139ef4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -59,7 +59,6 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), - _v_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -75,13 +74,13 @@ VtolAttitudeControl::VtolAttitudeControl() : _actuators_0_pub(nullptr), _actuators_1_pub(nullptr), _vtol_vehicle_status_pub(nullptr), - _v_rates_sp_pub(nullptr) + _v_rates_sp_pub(nullptr), + _v_att_sp_pub(nullptr) { memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); - memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -439,6 +438,17 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } +void VtolAttitudeControl::publish_transition_att_sp() +{ + if (_v_att_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + } else { + /* advertise and publish */ + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -451,7 +461,6 @@ void VtolAttitudeControl::task_main() fflush(stdout); /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -599,6 +608,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); + publish_transition_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c10f7b0dd7..2e68d5d85d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -151,6 +151,7 @@ private: orb_advert_t _actuators_1_pub; orb_advert_t _vtol_vehicle_status_pub; orb_advert_t _v_rates_sp_pub; + orb_advert_t _v_att_sp_pub; //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint @@ -215,6 +216,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); + void publish_transition_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8557110c0..4345f506a1 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -116,6 +116,10 @@ protected: float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _yaw_transition; // yaw angle in which transition will take place + float _throttle_transition; // throttle value used for the transition phase + bool _flag_was_in_trans_mode; // true if mode has just switched to transition + }; #endif From 6c5638062a818e264fbccc978fd169c6f44883df Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 20 Aug 2015 12:38:49 +0300 Subject: [PATCH 021/109] tailsitter auto transition test! Conflicts: src/modules/vtol_att_control/tailsitter.cpp --- src/modules/vtol_att_control/tailsitter.cpp | 306 ++++++++++++++++-- src/modules/vtol_att_control/tailsitter.h | 102 +++++- .../vtol_att_control/tailsitter_params.c | 56 ++++ 3 files changed, 433 insertions(+), 31 deletions(-) create mode 100644 src/modules/vtol_att_control/tailsitter_params.c diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7368870847..5bae1e0f93 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,23 +31,49 @@ * ****************************************************************************/ -/** -* @file tailsitter.cpp -* -* @author Roman Bapst -* -*/ + /** + * @file tailsitter.cpp + * + * @author Roman Bapst + * @author David Vorsin + * + */ #include "tailsitter.h" #include "vtol_att_control_main.h" -Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : - VtolType(att_controller), - _airspeed_tot(0), - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) -{ + #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition + #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition + #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 + #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW + #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC +Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : +/**VtolType(att_controller),*/ +VtolType(attc), +_roll_weight_mc(1.0f), +_yaw_weight_mc(1.0f), +_min_front_trans_dur(0.5f) +_airspeed_tot(0), +_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), +_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + + _flag_was_in_trans_mode = false; + + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + } Tailsitter::~Tailsitter() @@ -55,23 +81,153 @@ Tailsitter::~Tailsitter() } +int +Taulsitter::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tiltrotor.front_trans_dur, &v); + _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + + /* vtol front transition phase 2 duration */ + param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); + _params_tiltrotor.front_trans_dur_p2 = v; + + /* vtol duration of a back transition */ + param_get(_params_handles_tiltrotor.back_trans_dur, &v); + _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tiltrotor.airspeed_trans, &v); + _params_tiltrotor.airspeed_trans = v; + + /* vtol airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); + _params_tiltrotor.airspeed_blend_start = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); + _params_tiltrotor.elevons_mc_lock = l; + + /* avoid parameters which will lead to zero division in the transition code */ + _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + + if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + } + + return OK; +} + void Tailsitter::update_vtol_state() { - // simply switch between the two modes - if (!_attc->is_fixed_wing_requested()) { - _vtol_mode = ROTARY_WING; + parameters_update(); - } else { - _vtol_mode = FIXED_WING; + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (_manual_control_sp->aux1 < 0.0f) { // user switchig to MC mode + + // plane is in multicopter mode + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + break; + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + case TRANSITION_BACK: + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { + _vtol_schedule.flight_mode = MC_MODE; + } + break; + } + } else { // user switchig to FW mode + + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + case FW_MODE: + break; + case TRANSITION_FRONT_P1: + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + break; + case TRANSITION_FRONT_P2: + // check if we have reached pitch angle to switch to FW mode + if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + _vtol_schedule.flight_mode = FW_MODE; + } + break; + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; + } + } + + // map tailsitter specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } + + void Tailsitter::update_mc_state() { - if (!flag_idle_mc) { + // set idle speed for rotary wing mode + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } void Tailsitter::update_fw_state() @@ -80,13 +236,124 @@ void Tailsitter::update_fw_state() set_idle_fw(); flag_idle_mc = false; } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; } void Tailsitter::update_transition_state() { + if (!_flag_was_in_trans_mode) { + // save desired heading for transition and last thrust value + _yaw_transition = _v_att->yaw; + _pitch_transition_start = _v_att->pitch; + _throttle_transition_start = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _flag_was_in_trans_mode = true; + } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { + _v_att_sp->pitch_body = _pitch_transition_start - + fabsf(PITCH_TRANSITION_FRONT_P1 / _params_tiltrotor.front_trans_dur) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + } + + /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ + if (_airspeed->true_airspeed_m_s <= _params_tiltrotor.airspeed_trans) ) { + _v_att_sp->thrust = _throttle_transition_start + (THROTTLE_TRANSITION_MAX * _throttle_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + /** if the limit reached stop adding thrust */ + if (_v_att_sp->thrust >= (THROTTLE_TRANSITION_MAX * _throttle_transition_start) ) { + _v_att_sp->thrust = (THROTTLE_TRANSITION_MAX * _throttle_transition_start); + } + } + + /** ^ else condition nescesary for publishing values ? ^ */ + + // disable mc yaw control once the plane has picked up speed + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + }else { + _mc_yaw_weight = 1.0f; + } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down + + /** FW motor is switched */ + /** is thrust assignment nescesary? */ + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) / _params_tiltrotor.front_trans_dur_p2) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + } + + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ + + _mc_roll_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_pitch_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } + + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { + _v_att_sp->pitch_body = _pitch_transition_start + + fabsf(PITCH_TRANSITION_BACK / _params_tiltrotor.back_trans_dur) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + } + + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + + /** keep yaw disabled */ + _mc_yaw_weight = 0.0f; + + // set zero throttle for backtransition otherwise unwanted moments will be created + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + + } + + + + + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + + // compute desired attitude and thrust setpoint for the transition + + _v_att_sp->timestamp = hrt_absolute_time(); + _v_att_sp->roll_body = 0; + _v_att_sp->yaw_body = _yaw_transition; + _v_att_sp->R_valid = true; + + math::Matrix<3,3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); + + math::Quaternion q_sp; + q_sp.from_dcm(R_sp); + memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } + void Tailsitter::update_external_state() { @@ -110,8 +377,7 @@ void Tailsitter::calc_tot_airspeed() _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; } -void -Tailsitter::scale_mc_output() +void Tailsitter::scale_mc_output() { // scale around tuning airspeed float airspeed; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index ec8d007567..d54748cc75 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,18 +31,21 @@ * ****************************************************************************/ -/** -* @file tiltrotor.h -* -* @author Roman Bapst -* -*/ + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * @author David Vorsin + * + */ #ifndef TAILSITTER_H #define TAILSITTER_H #include "vtol_type.h" -#include +#include /** is it necsacery? **/ +#include +#include class Tailsitter : public VtolType { @@ -51,21 +54,98 @@ public: Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); + /** + * Update vtol state. + */ void update_vtol_state(); + + /** + * Update multicopter state. + */ void update_mc_state(); + + /** + * Update fixed wing state. + */ void update_fw_state(); + + /** + * Update transition state. + */ void update_transition_state(); + + /** + * Update external state. + */ void update_external_state(); private: - void fill_actuator_outputs(); - void calc_tot_airspeed(); - void scale_mc_output(); - float _airspeed_tot; + + struct { + float front_trans_dur; /**< duration of first part of front transition */ + float front_trans_dur_p2; + float back_trans_dur; /**< duration of back transition */ + float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ + float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ + int elevons_mc_lock; /**< lock elevons in multicopter mode */ + + } _params_tailsitter; + + struct { + param_t front_trans_dur; + param_t front_trans_dur_p2; + param_t back_trans_dur; + param_t airspeed_trans; + param_t airspeed_blend_start; + param_t elevons_mc_lock; + + } _params_handles_tailsitter; + + enum vtol_mode { + MC_MODE = 0, /**< vtol is in multicopter mode */ + TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ + TRANSITION_FRONT_P2, /**< vtol is in front transition part 2 mode */ + TRANSITION_BACK, /**< vtol is in back transition mode */ + FW_MODE /**< vtol is in fixed wing mode */ + }; + + struct { + vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ + hrt_abstime transition_start; /**< absoulte time at which front transition started */ + }_vtol_schedule; + + float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ + float _roll_weight_mc; /**< multicopter desired roll moment weight */ + float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ + + /** not sure about it yet ?! **/ + const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + + + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + /** + * Speed estimation for propwash controlled surfaces. + */ + void calc_tot_airspeed(); + + + /** is this one still needed? */ + void scale_mc_output(); + + /** + * Write control values to actuator output topics. + */ + void fill_actuator_outputs(); + + /** + * Update parameters. + */ + int parameters_update(); + }; #endif diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c new file mode 100644 index 0000000000..c72d3957c8 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 tailsitter_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + * @author David Vorsin + */ + +#include + +/** + * Duration of front transition phase 2 + * + * Time in seconds it should take for the rotors to rotate forward completely from the point + * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + * + * @min 0.1 + * @max 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + + From c39935f248e0a522f4b12dbba36b70d6eced0fa9 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 20 Aug 2015 13:36:48 +0300 Subject: [PATCH 022/109] mixer for transition not final yet --- src/modules/vtol_att_control/tailsitter.cpp | 86 ++++++++++----------- 1 file changed, 42 insertions(+), 44 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5bae1e0f93..108e46aad8 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -325,8 +325,8 @@ void Tailsitter::update_transition_state() /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - // set zero throttle for backtransition otherwise unwanted moments will be created - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + // set throttle value same as started + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition_start; } @@ -416,50 +416,48 @@ void Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch (_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + switch(_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + } + break; + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + case TRANSITION: + // in transition engines are mixed by weight (FRONT_P2 , BACK) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon - } - - break; - - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - - case TRANSITION: - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + break; + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } From 093d4fdd15469f071bf6ac74c0ac028a6d1241f1 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:16:34 +0300 Subject: [PATCH 023/109] development of autonomous transition --- src/modules/vtol_att_control/tailsitter.cpp | 98 ++++++++++----------- 1 file changed, 49 insertions(+), 49 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 108e46aad8..2fd5e60382 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -48,13 +48,14 @@ #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC -Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : -/**VtolType(att_controller),*/ +Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), +_airspeed_tot(0.0f), _roll_weight_mc(1.0f), +_pitch_weight_mc(1.0f), _yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f) -_airspeed_tot(0), +_min_front_trans_dur(0.5f), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { @@ -67,12 +68,12 @@ _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinit _flag_was_in_trans_mode = false; - _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); - _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); - _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); - _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); - _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + _params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); + _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); } @@ -82,40 +83,40 @@ Tailsitter::~Tailsitter() } int -Taulsitter::parameters_update() +Tailsitter::parameters_update() { float v; int l; /* vtol duration of a front transition */ - param_get(_params_handles_tiltrotor.front_trans_dur, &v); - _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + param_get(_params_handles_tailsitter.front_trans_dur, &v); + _params_tailsitter.front_trans_dur = math::constrain(v,1.0f,5.0f); /* vtol front transition phase 2 duration */ - param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); - _params_tiltrotor.front_trans_dur_p2 = v; + param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); + _params_tailsitter.front_trans_dur_p2 = v; /* vtol duration of a back transition */ - param_get(_params_handles_tiltrotor.back_trans_dur, &v); - _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + param_get(_params_handles_tailsitter.back_trans_dur, &v); + _params_tailsitter.back_trans_dur = math::constrain(v,0.0f,5.0f); /* vtol airspeed at which it is ok to switch to fw mode */ - param_get(_params_handles_tiltrotor.airspeed_trans, &v); - _params_tiltrotor.airspeed_trans = v; + param_get(_params_handles_tailsitter.airspeed_trans, &v); + _params_tailsitter.airspeed_trans = v; /* vtol airspeed at which we start blending mc/fw controls */ - param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); - _params_tiltrotor.airspeed_blend_start = v; + param_get(_params_handles_tailsitter.airspeed_blend_start, &v); + _params_tailsitter.airspeed_blend_start = v; /* vtol lock elevons in multicopter */ - param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); - _params_tiltrotor.elevons_mc_lock = l; + param_get(_params_handles_tailsitter.elevons_mc_lock, &l); + _params_tailsitter.elevons_mc_lock = l; /* avoid parameters which will lead to zero division in the transition code */ - _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); + _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); - if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { - _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; + if ( _params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f ) { + _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; } return OK; @@ -168,7 +169,7 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) { + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } @@ -246,9 +247,9 @@ void Tailsitter::update_transition_state() { if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value - _yaw_transition = _v_att->yaw; - _pitch_transition_start = _v_att->pitch; - _throttle_transition_start = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _yaw_transition = _v_att_sp->yaw_body; + _pitch_transition_start = _v_att_sp->pitch_body; + _throttle_transition = _v_att_sp->thrust; _flag_was_in_trans_mode = true; } @@ -257,16 +258,16 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start - - fabsf(PITCH_TRANSITION_FRONT_P1 / _params_tiltrotor.front_trans_dur) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); } /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ - if (_airspeed->true_airspeed_m_s <= _params_tiltrotor.airspeed_trans) ) { - _v_att_sp->thrust = _throttle_transition_start + (THROTTLE_TRANSITION_MAX * _throttle_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + /** if the limit reached stop adding thrust */ - if (_v_att_sp->thrust >= (THROTTLE_TRANSITION_MAX * _throttle_transition_start) ) { - _v_att_sp->thrust = (THROTTLE_TRANSITION_MAX * _throttle_transition_start); + if (_v_att_sp->thrust >= ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition) ) { + _v_att_sp->thrust = ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } } @@ -282,23 +283,21 @@ void Tailsitter::update_transition_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down - /** FW motor is switched */ - /** is thrust assignment nescesary? */ + /** no motor switching */ /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) / _params_tiltrotor.front_trans_dur_p2) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - _mc_roll_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); - _mc_pitch_weight = 1.0f - 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + _mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -314,19 +313,19 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { _v_att_sp->pitch_body = _pitch_transition_start + - fabsf(PITCH_TRANSITION_BACK / _params_tiltrotor.back_trans_dur) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); } /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); /** keep yaw disabled */ _mc_yaw_weight = 0.0f; // set throttle value same as started - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition_start; + //_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition; } @@ -340,7 +339,7 @@ void Tailsitter::update_transition_state() // compute desired attitude and thrust setpoint for the transition _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0; + _v_att_sp->roll_body = 0.0f; _v_att_sp->yaw_body = _yaw_transition; _v_att_sp->R_valid = true; @@ -429,7 +428,7 @@ void Tailsitter::fill_actuator_outputs() } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =0; //pitch elevon } break; case FIXED_WING: @@ -455,6 +454,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + //_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: // not yet implemented, we are switching brute force at the moment From 9e28365bbbc501c559aff444538f53ec60545628 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:16:55 +0300 Subject: [PATCH 024/109] adding pitch weight for tailsitters --- src/modules/vtol_att_control/tailsitter.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index d54748cc75..1b2caca676 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -118,10 +118,11 @@ private: float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ float _roll_weight_mc; /**< multicopter desired roll moment weight */ + float _pitch_weight_mc; /**< multicopter desired pitch moment weight */ float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ /** not sure about it yet ?! **/ - const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ /** should this anouncement stay? **/ From 28cc7521ff8f1c82e909cfce56147d395af58e30 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:19:20 +0300 Subject: [PATCH 025/109] unnecessary parameter --- src/modules/vtol_att_control/tailsitter_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index c72d3957c8..52801d9e8c 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -50,7 +50,7 @@ * @min 0.1 * @max 2 * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + +PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ From c129ebb0ea54ec0b62a7eff39353fe6cdcaa6e61 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:19:59 +0300 Subject: [PATCH 026/109] adding poll for att_sp and make the MC attitude controller work in transition --- .../vtol_att_control_main.cpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5725139ef4..142c2a26ff 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #include "vtol_att_control_main.h" @@ -59,6 +60,7 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), + _v_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -81,6 +83,7 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -270,6 +273,36 @@ VtolAttitudeControl::vehicle_airspeed_poll() } } +/** +* Check for attitude set points update. +*/ +void +VtolAttitudeControl::vehicle_attitude_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +/** +* Check for attitude update. +*/ +void +VtolAttitudeControl::vehicle_attitude_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_att_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + } +} + /** * Check for battery updates. */ @@ -464,6 +497,7 @@ void VtolAttitudeControl::task_main() _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -535,6 +569,8 @@ void VtolAttitudeControl::task_main() vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. + vehicle_attitude_setpoint_poll();//Check for changes in attitude set points + vehicle_attitude_poll(); //Check for changes in attitude actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output vehicle_rates_sp_mc_poll(); @@ -591,6 +627,7 @@ void VtolAttitudeControl::task_main() } else if (_vtol_type->get_mode() == TRANSITION) { // vehicle is doing a transition _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition bool got_new_data = false; From 0d58429565d223f13b8a63bae77aad26be53b743 Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:20:25 +0300 Subject: [PATCH 027/109] poll on att_sp and att --- src/modules/vtol_att_control/vtol_att_control_main.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2e68d5d85d..9485d23d4f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -41,6 +41,7 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler + * @author David Vorsin * */ #ifndef VTOL_ATT_CONTROL_MAIN_H @@ -209,6 +210,8 @@ private: void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. + void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_battery_poll(); // Check for battery updates void vehicle_cmd_poll(); void parameters_update_poll(); //Check if parameters have changed From 2bf460cdfe9af1ef70ed18463b3fae77d575ddeb Mon Sep 17 00:00:00 2001 From: davidvor Date: Wed, 26 Aug 2015 17:21:00 +0300 Subject: [PATCH 028/109] adding variables for tailsitter transition --- src/modules/vtol_att_control/vtol_type.h | 31 ++++++++++++------------ 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 4345f506a1..5c112ac207 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -91,22 +91,22 @@ protected: VtolAttitudeControl *_attc; mode _vtol_mode; - struct vehicle_attitude_s *_v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; - struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s *_armed; //actuator arming status + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status struct vehicle_local_position_s *_local_pos; - struct airspeed_s *_airspeed; // airspeed - struct battery_status_s *_batt_status; // battery status + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status struct Params *_params; @@ -117,7 +117,8 @@ protected: float _mc_yaw_weight; // weight for multicopter attitude controller yaw output float _yaw_transition; // yaw angle in which transition will take place - float _throttle_transition; // throttle value used for the transition phase + float _throttle_transition; // throttle value used for the transition phase + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) bool _flag_was_in_trans_mode; // true if mode has just switched to transition }; From 30bb05487aba847cffc8e1aa46be946efbffa359 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:06:31 +0300 Subject: [PATCH 029/109] mixer setup for tests --- ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index 4fd323353a..bd332a2688 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0 #mixer for the elevons M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 5000 5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 10000 10000 0 -10000 10000 -S: 1 1 -10000 -10000 0 -10000 10000 +S: 1 0 5000 5000 0 -10000 10000 +S: 1 1 -5000 -5000 0 -10000 10000 From 2642b3fbead3d849e1b107dcacb03f6d4d870a7d Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:07:13 +0300 Subject: [PATCH 030/109] limit for pitch angle set point during transition --- src/modules/vtol_att_control/tailsitter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 2fd5e60382..5528abe59f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -259,6 +259,10 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1-0.2f; + } } /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ From 2721330040698610c83adfe6737b16636eac1691 Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 27 Aug 2015 14:07:26 +0300 Subject: [PATCH 031/109] errors --- errors.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 errors.txt diff --git a/errors.txt b/errors.txt new file mode 100644 index 0000000000..e69de29bb2 From c32f5910e0b1bfe5f6fd1dbc7013ec9702ff1011 Mon Sep 17 00:00:00 2001 From: davidvor Date: Fri, 28 Aug 2015 13:38:15 +0300 Subject: [PATCH 032/109] name change --- src/modules/vtol_att_control/tailsitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5528abe59f..63f3e40384 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -83,7 +83,7 @@ Tailsitter::~Tailsitter() } int -Tailsitter::parameters_update() +Trailsitter::parameters_update() { float v; int l; From 059cffeb40253736a874c117e3b2384ac61471b8 Mon Sep 17 00:00:00 2001 From: davidvor Date: Fri, 28 Aug 2015 13:41:19 +0300 Subject: [PATCH 033/109] name change --- src/modules/vtol_att_control/tailsitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 63f3e40384..5528abe59f 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -83,7 +83,7 @@ Tailsitter::~Tailsitter() } int -Trailsitter::parameters_update() +Tailsitter::parameters_update() { float v; int l; From 89b01fd45a5a20f0cc5489f7b08c7a5325db04e2 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 30 Aug 2015 21:19:09 +0300 Subject: [PATCH 034/109] turning on fw controler during p2 front transiion turning on fw controler during p2 front transiion simpling the weight for testing setting output more like fw for transition --- src/modules/vtol_att_control/tailsitter.cpp | 37 +++++++++++++++------ 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 5528abe59f..d9adb297d3 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -45,7 +45,7 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 - #define PITCH_TRANSITION_FRONT_P2 -1.05f // pitch angle to switch to FW + #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : @@ -292,17 +292,30 @@ void Tailsitter::update_transition_state() /** no motor switching */ + if (flag_idle_mc) { + set_idle_fw(); + //flag_idle_mc = false; + } + /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2-0.2f; + } + } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - _mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - _mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -316,15 +329,19 @@ void Tailsitter::update_transition_state() /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start + - fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_BACK+0.2f) ) { + _v_att_sp->pitch_body = PITCH_TRANSITION_BACK+0.2f; } /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + //_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + //_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -455,10 +472,10 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //change _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - //_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: // not yet implemented, we are switching brute force at the moment From 19b960648664d29147632052180e91f8b31863d8 Mon Sep 17 00:00:00 2001 From: davidvor Date: Tue, 1 Sep 2015 12:04:03 +0300 Subject: [PATCH 035/109] simplifying transition for mc control only --- src/modules/vtol_att_control/tailsitter.cpp | 61 +++++++-------------- 1 file changed, 21 insertions(+), 40 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d9adb297d3..04139c292b 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -44,9 +44,9 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition - #define PITCH_TRANSITION_FRONT_P1 -0.78f // pitch angle to switch to TRANSITION_P2 + #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.5f // pitch angle to switch to MC + #define PITCH_TRANSITION_BACK -0.6f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), @@ -170,8 +170,8 @@ void Tailsitter::update_vtol_state() case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); } break; case TRANSITION_FRONT_P2: @@ -255,28 +255,16 @@ void Tailsitter::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P1-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1-0.2f; - } - } + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body =math::constrain( _v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1-0.2f , _pitch_transition_start ); /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - - /** if the limit reached stop adding thrust */ - if (_v_att_sp->thrust >= ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition) ) { - _v_att_sp->thrust = ((1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); - } + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } - /** ^ else condition nescesary for publishing values ? ^ */ - // disable mc yaw control once the plane has picked up speed if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; @@ -294,7 +282,7 @@ void Tailsitter::update_transition_state() if (flag_idle_mc) { set_idle_fw(); - //flag_idle_mc = false; + flag_idle_mc = false; } /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ @@ -327,26 +315,19 @@ void Tailsitter::update_transition_state() flag_idle_mc = true; } - /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_BACK+0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_BACK+0.2f; - } - - /** smoothly move control weight to MC */ - //_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - //_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - _mc_roll_weight = 0.0f; - _mc_pitch_weight = 0.0f; - + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - // set throttle value same as started - //_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] = _throttle_transition; + /** smoothly move control weight to MC */ + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + + + // throttle value is the same as started } @@ -472,9 +453,9 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //change - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight) + - (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight + + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: From 76c479170a3fdb61e21b302f20e0d3f2fa08e01e Mon Sep 17 00:00:00 2001 From: davidvor Date: Thu, 3 Sep 2015 17:19:40 +0300 Subject: [PATCH 036/109] add transition back throttle value and angular rate --- src/modules/vtol_att_control/tailsitter.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 04139c292b..d2ef6b3db2 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -46,7 +46,7 @@ #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.6f // pitch angle to switch to MC + #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC Tailsitter::Tailsitter (VtolAttitudeControl *attc) : VtolType(attc), @@ -316,8 +316,11 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK - _pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + + + _v_att_sp->thrust = _throttle_transition*0.9f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; From 945dda04ca397a800bc7592490f8981a37a93b44 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 6 Sep 2015 12:01:59 +0300 Subject: [PATCH 037/109] comments --- src/modules/vtol_att_control/tailsitter.cpp | 35 ++++++++++----------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d2ef6b3db2..c557936b15 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -127,15 +127,15 @@ void Tailsitter::update_vtol_state() parameters_update(); /* simple logic using a two way switch to perform transitions. - * after flipping the switch the vehicle will start tilting rotors, picking up - * forward speed. After the vehicle has picked up enough speed the rotors are tilted - * forward completely. For the backtransition the motors simply rotate back. + * after flipping the switch the vehicle will start tilting in MC control mode, picking up + * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. + * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - if (_manual_control_sp->aux1 < 0.0f) { // user switchig to MC mode + if (_manual_control_sp->aux1 < 0.0f) { - // plane is in multicopter mode - switch(_vtol_schedule.flight_mode) { + + switch(_vtol_schedule.flight_mode) { // user switchig to MC mode case MC_MODE: break; case FW_MODE: @@ -147,8 +147,9 @@ void Tailsitter::update_vtol_state() _vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_FRONT_P2: + // NOT USED // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + //_vtol_schedule.flight_mode = MC_MODE; break; case TRANSITION_BACK: // check if we have reached pitch angle to switch to MC mode @@ -175,10 +176,11 @@ void Tailsitter::update_vtol_state() } break; case TRANSITION_FRONT_P2: + // NOT USED // check if we have reached pitch angle to switch to FW mode - if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - _vtol_schedule.flight_mode = FW_MODE; - } + //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + // _vtol_schedule.flight_mode = FW_MODE; + //} break; case TRANSITION_BACK: // failsafe into fixed wing mode @@ -316,10 +318,10 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = -1.57f + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); - + // throttle value is decreesed _v_att_sp->thrust = _throttle_transition*0.9f; /** keep yaw disabled */ @@ -329,9 +331,6 @@ void Tailsitter::update_transition_state() _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - - // throttle value is the same as started - } @@ -449,7 +448,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle break; case TRANSITION: - // in transition engines are mixed by weight (FRONT_P2 , BACK) + // in transition engines are mixed by weight (BACK TRANSITION ONLY) _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; @@ -457,8 +456,8 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight - + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; break; case EXTERNAL: From 932883e30367b4ebf81b0d7c4f3703a3d1074d32 Mon Sep 17 00:00:00 2001 From: davidvor Date: Sun, 6 Sep 2015 12:02:33 +0300 Subject: [PATCH 038/109] errors --- errors.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 errors.txt diff --git a/errors.txt b/errors.txt deleted file mode 100644 index e69de29bb2..0000000000 From d1dc8ed432f766b24e8d324d252ba5df8e13f5ce Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 7 Sep 2015 09:21:24 +0200 Subject: [PATCH 039/109] if vtol, publish virtual attitude setpoint Conflicts: src/modules/mc_pos_control/mc_pos_control_main.cpp --- .../fw_att_control/fw_att_control_main.cpp | 20 ++++++------ .../fw_pos_control_l1_main.cpp | 22 ++++++++++--- .../mc_pos_control/mc_pos_control_main.cpp | 31 +++++++++++++------ 3 files changed, 50 insertions(+), 23 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8772ceb835..1328bca48d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -142,6 +143,7 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _attitude_setpoint_id; struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ @@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rates_sp_id(0), _actuators_id(0), + _attitude_setpoint_id(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll() if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } @@ -1021,15 +1026,12 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - - } else { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 24cf1f084f..8b4e4224eb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -166,6 +167,8 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + orb_id_t _attitude_setpoint_id; + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ @@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), +/* publication ID */ + _attitude_setpoint_id(0), + /* states */ _ctrl_state(), _att_sp(), @@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } } @@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + } else if (_attitude_setpoint_id) { /* advertise and publish */ - _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /* XXX check if radius makes sense here */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 53eff5977f..a631336273 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,8 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + orb_id_t _attitude_setpoint_id; + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + _attitude_setpoint_id(0), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), _vel_x_deriv(this, "VELD"), @@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (!_attitude_setpoint_id) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } + } } orb_check(_ctrl_state_sub, &updated); @@ -1183,10 +1195,9 @@ MulticopterPositionControl::task_main() /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } else { @@ -1615,13 +1626,13 @@ MulticopterPositionControl::task_main() * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { - if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { - } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } From 0a0a074194705329d19e08f6441a7d6e38d29535 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 7 Sep 2015 09:22:01 +0200 Subject: [PATCH 040/109] use virtual attitude setpoint Conflicts: src/modules/vtol_att_control/tailsitter.cpp src/modules/vtol_att_control/tiltrotor.cpp src/modules/vtol_att_control/vtol_att_control_main.cpp src/modules/vtol_att_control/vtol_att_control_main.h src/modules/vtol_att_control/vtol_type.h --- msg/fw_virtual_attitude_setpoint.msg | 23 +++++++++ msg/mc_virtual_attitude_setpoint.msg | 23 +++++++++ src/modules/uORB/objects_common.cpp | 8 +++- src/modules/uORB/uORB.h | 1 - src/modules/vtol_att_control/standard.cpp | 11 ++++- src/modules/vtol_att_control/tailsitter.cpp | 6 +++ src/modules/vtol_att_control/tiltrotor.cpp | 24 ++++------ .../vtol_att_control_main.cpp | 48 +++++++++++++++++-- .../vtol_att_control/vtol_att_control_main.h | 13 ++++- src/modules/vtol_att_control/vtol_type.cpp | 2 + src/modules/vtol_att_control/vtol_type.h | 18 +++---- 11 files changed, 143 insertions(+), 34 deletions(-) create mode 100644 msg/fw_virtual_attitude_setpoint.msg create mode 100644 msg/mc_virtual_attitude_setpoint.msg diff --git a/msg/fw_virtual_attitude_setpoint.msg b/msg/fw_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/fw_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/msg/mc_virtual_attitude_setpoint.msg b/msg/mc_virtual_attitude_setpoint.msg new file mode 100644 index 0000000000..7bbb670b31 --- /dev/null +++ b/msg/mc_virtual_attitude_setpoint.msg @@ -0,0 +1,23 @@ + +uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data + +float32 roll_body # body angle in NED frame +float32 pitch_body # body angle in NED frame +float32 yaw_body # body angle in NED frame + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +float32[9] R_body # Rotation matrix describing the setpoint as rotation from the current body frame +bool R_valid # Set to true if rotation matrix is valid + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control +bool q_d_valid # Set to true if quaternion vector is valid +float32[4] q_e # Attitude error in quaternion +bool q_e_valid # Set to true if quaternion error vector is valid + +float32 thrust # Thrust in Newton the power system should generate + +bool roll_reset_integral # Reset roll integral part (navigation logic change) +bool pitch_reset_integral # Reset pitch integral part (navigation logic change) +bool yaw_reset_integral # Reset yaw integral part (navigation logic change) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 8fe70d7604..1b8fb18735 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); -ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/mc_virtual_attitude_setpoint.h" +ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s); + +#include "topics/fw_virtual_attitude_setpoint.h" +ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 6ead26186b..cfcf39b408 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -366,7 +366,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; typedef uint8_t hil_state_t; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 05c1a525b1..5a329d52f9 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -186,6 +186,9 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value @@ -242,12 +245,16 @@ void Standard::update_transition_state() void Standard::update_mc_state() { - // do nothing + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Standard::update_fw_state() { - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index c557936b15..827e3ec617 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -231,6 +231,9 @@ void Tailsitter::update_mc_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_fw_state() @@ -243,6 +246,9 @@ void Tailsitter::update_fw_state() _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; + + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tailsitter::update_transition_state() diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c4fa37eb2c..24ff7ece2a 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -252,6 +252,9 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; @@ -273,6 +276,9 @@ void Tiltrotor::update_mc_state() void Tiltrotor::update_fw_state() { + // copy virtual attitude setpoint to real attitude setpoint + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -363,22 +369,8 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); - // compute desired attitude and thrust setpoint for the transition - _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0; - _v_att_sp->pitch_body = 0; - _v_att_sp->yaw_body = _yaw_transition; - _v_att_sp->thrust = _throttle_transition; - - math::Matrix<3,3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); - memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); - _v_att_sp->R_valid = true; - - math::Quaternion q_sp; - q_sp.from_dcm(R_sp); - _v_att_sp->q_d_valid = true; - memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); + // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } void Tiltrotor::update_external_state() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 142c2a26ff..80bb62eccd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -60,7 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() : //init subscription handlers _v_att_sub(-1), - _v_att_sp_sub(-1), + _v_att_sp_sub(-1), + _mc_virtual_att_sp_sub(-1), + _fw_virtual_att_sp_sub(-1), _mc_virtual_v_rates_sp_sub(-1), _fw_virtual_v_rates_sp_sub(-1), _v_control_mode_sub(-1), @@ -84,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp)); + memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); @@ -351,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for mc virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::mc_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_mc_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp); + } + +} + +/** +* Check for fw virtual attitude setpoint updates. +*/ +void +VtolAttitudeControl::fw_virtual_att_sp_poll() +{ + bool updated; + + orb_check(_fw_virtual_att_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp); + } + +} + /** * Check for command updates. */ @@ -471,7 +507,7 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -void VtolAttitudeControl::publish_transition_att_sp() +void VtolAttitudeControl::publish_att_sp() { if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ @@ -494,6 +530,9 @@ void VtolAttitudeControl::task_main() fflush(stdout); /* do subscriptions */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); + _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -566,6 +605,8 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + mc_virtual_att_sp_poll(); + fw_virtual_att_sp_poll(); vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. @@ -645,7 +686,7 @@ void VtolAttitudeControl::task_main() if (got_new_data) { _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); - publish_transition_att_sp(); + publish_att_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { @@ -653,6 +694,7 @@ void VtolAttitudeControl::task_main() _vtol_type->update_external_state(); } + publish_att_sp(); _vtol_type->fill_actuator_outputs(); /* Only publish if the proper mode(s) are enabled */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 9485d23d4f..c7c51247ae 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -63,6 +63,8 @@ #include #include #include +#include +#include #include #include #include @@ -107,6 +109,8 @@ public: struct vehicle_attitude_s *get_att() {return &_v_att;} struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;} struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} @@ -124,7 +128,6 @@ public: struct Params *get_params() {return &_params;} - private: //******************flags & handlers****************************************************** bool _task_should_exit; @@ -133,6 +136,8 @@ private: /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_att_sp_sub; + int _fw_virtual_att_sp_sub; int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription int _v_control_mode_sub; //vehicle control mode subscription @@ -156,6 +161,8 @@ private: //*******************data containers*********************************************************** struct vehicle_attitude_s _v_att; //vehicle attitude struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint @@ -204,6 +211,8 @@ private: void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. void vehicle_manual_poll(); //Check for changes in manual inputs. void arming_status_poll(); //Check for arming status updates. + void mc_virtual_att_sp_poll(); + void fw_virtual_att_sp_poll(); void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); @@ -219,7 +228,7 @@ private: void fill_mc_att_rates_sp(); void fill_fw_att_rates_sp(); void handle_command(); - void publish_transition_att_sp(); + void publish_att_sp(); }; #endif diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index f57080a2a7..0120cadc84 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); + _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); + _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_rates_sp = _attc->get_rates_sp(); _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5c112ac207..196751016c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -91,13 +91,15 @@ protected: VtolAttitudeControl *_attc; mode _vtol_mode; - struct vehicle_attitude_s *_v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint + struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode struct vtol_vehicle_status_s *_vtol_vehicle_status; struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) @@ -117,8 +119,8 @@ protected: float _mc_yaw_weight; // weight for multicopter attitude controller yaw output float _yaw_transition; // yaw angle in which transition will take place - float _throttle_transition; // throttle value used for the transition phase float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + float _throttle_transition; // throttle value used for the transition phase bool _flag_was_in_trans_mode; // true if mode has just switched to transition }; From 7127f6a92fb6060c365c1032c2609ad9d4f60c94 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 22:48:53 +0100 Subject: [PATCH 041/109] fixed code style --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tailsitter.cpp | 380 ++++++++++-------- src/modules/vtol_att_control/tailsitter.h | 18 +- .../vtol_att_control/tailsitter_params.c | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 1 + .../vtol_att_control_main.cpp | 5 + .../vtol_att_control/vtol_att_control_main.h | 37 +- 7 files changed, 250 insertions(+), 195 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5a329d52f9..8a5896999e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -254,7 +254,7 @@ void Standard::update_fw_state() // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 827e3ec617..1776dc141d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,33 +31,33 @@ * ****************************************************************************/ - /** - * @file tailsitter.cpp - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #include "tailsitter.h" #include "vtol_att_control_main.h" - #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition - #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition - #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 - #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC -Tailsitter::Tailsitter (VtolAttitudeControl *attc) : -VtolType(attc), -_airspeed_tot(0.0f), -_roll_weight_mc(1.0f), -_pitch_weight_mc(1.0f), -_yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f), +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _roll_weight_mc(1.0f), + _pitch_weight_mc(1.0f), + _yaw_weight_mc(1.0f), + _min_front_trans_dur(0.5f), -_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), -_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -74,7 +74,7 @@ _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinit _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - + } Tailsitter::~Tailsitter() @@ -90,7 +90,7 @@ Tailsitter::parameters_update() /* vtol duration of a front transition */ param_get(_params_handles_tailsitter.front_trans_dur, &v); - _params_tailsitter.front_trans_dur = math::constrain(v,1.0f,5.0f); + _params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f); /* vtol front transition phase 2 duration */ param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); @@ -98,7 +98,7 @@ Tailsitter::parameters_update() /* vtol duration of a back transition */ param_get(_params_handles_tailsitter.back_trans_dur, &v); - _params_tailsitter.back_trans_dur = math::constrain(v,0.0f,5.0f); + _params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f); /* vtol airspeed at which it is ok to switch to fw mode */ param_get(_params_handles_tailsitter.airspeed_trans, &v); @@ -115,7 +115,7 @@ Tailsitter::parameters_update() /* avoid parameters which will lead to zero division in the transition code */ _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); - if ( _params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f ) { + if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) { _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; } @@ -124,97 +124,115 @@ Tailsitter::parameters_update() void Tailsitter::update_vtol_state() { - parameters_update(); + parameters_update(); - /* simple logic using a two way switch to perform transitions. + /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. - */ + */ - if (_manual_control_sp->aux1 < 0.0f) { + if (_manual_control_sp->aux1 < 0.0f) { - - switch(_vtol_schedule.flight_mode) { // user switchig to MC mode - case MC_MODE: - break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case TRANSITION_FRONT_P1: - // failsafe into multicopter mode + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_FRONT_P2: - // NOT USED - // failsafe into multicopter mode - //_vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_BACK: - // check if we have reached pitch angle to switch to MC mode - if (_v_att->pitch >= PITCH_TRANSITION_BACK) { - _vtol_schedule.flight_mode = MC_MODE; - } - break; + } + + break; } + } else { // user switchig to FW mode - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case FW_MODE: - break; - case TRANSITION_FRONT_P1: - // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { - _vtol_schedule.flight_mode = FW_MODE; - //_vtol_schedule.transition_start = hrt_absolute_time(); - } - break; - case TRANSITION_FRONT_P2: - // NOT USED - // check if we have reached pitch angle to switch to FW mode - //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - // _vtol_schedule.flight_mode = FW_MODE; - //} - break; - case TRANSITION_BACK: - // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; - /* **LATER*** if pitch is closer to mc (-45>) - * go to transition P1 - */ - break; + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) + && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // check if we have reached pitch angle to switch to FW mode + //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + // _vtol_schedule.flight_mode = FW_MODE; + //} + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; } } // map tailsitter specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_FRONT_P2: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_BACK: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } @@ -223,7 +241,7 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_mc_state() { // set idle speed for rotary wing mode - if (!flag_idle_mc) { + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } @@ -264,19 +282,24 @@ void Tailsitter::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->pitch_body =math::constrain( _v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1-0.2f , _pitch_transition_start ); + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f , + _pitch_transition_start); /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { - _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , + (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } // disable mc yaw control once the plane has picked up speed if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; - }else { + + } else { _mc_yaw_weight = 1.0f; } @@ -294,27 +317,29 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2-0.2f; + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; } } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - + _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { @@ -323,20 +348,23 @@ void Tailsitter::update_transition_state() flag_idle_mc = true; } - /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); // throttle value is decreesed - _v_att_sp->thrust = _throttle_transition*0.9f; - + _v_att_sp->thrust = _throttle_transition * 0.9f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + } @@ -353,8 +381,8 @@ void Tailsitter::update_transition_state() _v_att_sp->yaw_body = _yaw_transition; _v_att_sp->R_valid = true; - math::Matrix<3,3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); math::Quaternion q_sp; @@ -425,49 +453,69 @@ void Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch(_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + switch (_vtol_mode) { + case ROTARY_WING: + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - if (_params->elevons_mc_lock == 1) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - } else { - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =0; //pitch elevon - } - break; - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - case TRANSITION: - // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - break; - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; //pitch elevon + } + + break; + + case FIXED_WING: + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + break; + + case TRANSITION: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 1b2caca676..de5ab0ba3e 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,13 +31,13 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #ifndef TAILSITTER_H #define TAILSITTER_H @@ -114,7 +114,7 @@ private: struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ hrt_abstime transition_start; /**< absoulte time at which front transition started */ - }_vtol_schedule; + } _vtol_schedule; float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ float _roll_weight_mc; /**< multicopter desired roll moment weight */ @@ -125,7 +125,7 @@ private: float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ - /** should this anouncement stay? **/ + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index 52801d9e8c..af38b55693 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -50,7 +50,7 @@ * @min 0.1 * @max 2 * @group VTOL Attitude Control - + PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 24ff7ece2a..3890a71843 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -306,6 +306,7 @@ void Tiltrotor::update_transition_state() _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; _flag_was_in_trans_mode = true; } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 80bb62eccd..5ab0d01b1c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -512,6 +512,7 @@ void VtolAttitudeControl::publish_att_sp() if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + } else { /* advertise and publish */ _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); @@ -740,7 +741,11 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, +<<<<<<< HEAD (px4_main_t)&VtolAttitudeControl::task_main_trampoline, +======= + (main_t)&VtolAttitudeControl::task_main_trampoline, +>>>>>>> fixed code style nullptr); if (_control_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index c7c51247ae..ccf644142c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -107,27 +107,28 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;} - struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_armed_s *get_armed() {return &_armed;} - struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} - struct airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} struct Params *get_params() {return &_params;} + private: //******************flags & handlers****************************************************** bool _task_should_exit; From 27f027b3e4fd49cd02a84c690ed925b36a788f2d Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 09:58:43 +0100 Subject: [PATCH 042/109] ported vtol module to posix --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5ab0d01b1c..b9c2d64fe2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -741,11 +741,7 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, -<<<<<<< HEAD (px4_main_t)&VtolAttitudeControl::task_main_trampoline, -======= - (main_t)&VtolAttitudeControl::task_main_trampoline, ->>>>>>> fixed code style nullptr); if (_control_task < 0) { From 330f174967ecd26793bf7ededf6e3c592b2cdde5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 15:10:56 +0100 Subject: [PATCH 043/109] fix --- src/modules/vtol_att_control/tailsitter.cpp | 3 --- src/modules/vtol_att_control/tailsitter.h | 3 --- src/modules/vtol_att_control/vtol_att_control_main.h | 3 +++ 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 1776dc141d..3977959859 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -51,9 +51,6 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : VtolType(attc), _airspeed_tot(0.0f), - _roll_weight_mc(1.0f), - _pitch_weight_mc(1.0f), - _yaw_weight_mc(1.0f), _min_front_trans_dur(0.5f), _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index de5ab0ba3e..f68fbe9106 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -117,9 +117,6 @@ private: } _vtol_schedule; float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ - float _roll_weight_mc; /**< multicopter desired roll moment weight */ - float _pitch_weight_mc; /**< multicopter desired pitch moment weight */ - float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ /** not sure about it yet ?! **/ float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index ccf644142c..0e99d58aab 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -197,6 +197,9 @@ private: param_t elevons_mc_lock; } _params_handles; + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ int _transition_command; VtolType *_vtol_type; // base class for different vtol types From 2e7c1e2b363dd6ad0ed50d71310690815dada7ce Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 3 Nov 2015 15:34:51 +0100 Subject: [PATCH 044/109] rename vtol model for SITL to tailsitter --- Tools/sitl_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7dee02868a..f32f6e585e 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -79,7 +79,7 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../${rc_script}_${program} + lldb -- mainapp ../../../../${rc_script}_${program}_${model} elif [ "$debugger" == "gdb" ] then gdb --args mainapp ../../../../${rc_script}_${program} From 990eb68bbf98a177f11c96b0cac1170d898685f9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 19 Nov 2015 15:24:17 +0100 Subject: [PATCH 045/109] allow direct switch to fw if disarmed --- src/modules/vtol_att_control/tailsitter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 3977959859..20f975b7fb 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -177,8 +177,8 @@ void Tailsitter::update_vtol_state() case TRANSITION_FRONT_P1: // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) - && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; //_vtol_schedule.transition_start = hrt_absolute_time(); } From 7cb7245a84db391e0d1872ac80e097931399bb30 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 23 Nov 2015 08:03:43 +0100 Subject: [PATCH 046/109] corrected tailsitter actuator control outputs --- src/modules/vtol_att_control/tailsitter.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 20f975b7fb..106f91a8e4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -186,12 +186,6 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_FRONT_P2: - // NOT USED - // check if we have reached pitch angle to switch to FW mode - //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - // _vtol_schedule.flight_mode = FW_MODE; - //} - break; case TRANSITION_BACK: // failsafe into fixed wing mode @@ -467,7 +461,7 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; From 8832e178b0791808967e06593c1fba78da7c71a3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 15 Aug 2015 16:20:41 +0200 Subject: [PATCH 047/109] added class for vtol landing detection --- .../land_detector/MulticopterLandDetector.cpp | 7 ++ .../land_detector/MulticopterLandDetector.h | 17 ++- .../land_detector/VtolLandDetector.cpp | 116 ++++++++++++++++++ src/modules/land_detector/VtolLandDetector.h | 94 ++++++++++++++ .../land_detector/land_detector_main.cpp | 4 + 5 files changed, 232 insertions(+), 6 deletions(-) create mode 100644 src/modules/land_detector/VtolLandDetector.cpp create mode 100644 src/modules/land_detector/VtolLandDetector.h diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index fa636e496c..2f46c21f3f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -95,6 +95,13 @@ bool MulticopterLandDetector::update() // first poll for new data from our subscriptions updateSubscriptions(); + updateParameterCache(false); + + return get_landed_state(); +} + +bool MulticopterLandDetector::get_landed_state() +{ // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d001be4e7f..3ee697a468 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -60,24 +60,29 @@ protected: /** * @brief polls all subscriptions and pulls any data that has changed **/ - void updateSubscriptions(); + virtual void updateSubscriptions(); /** * @brief Runs one iteration of the land detection algorithm **/ - bool update() override; + virtual bool update() override; /** * @brief Initializes the land detection algorithm **/ - void initialize() override; + virtual void initialize() override; - -private: /** * @brief download and update local parameter cache **/ - void updateParameterCache(const bool force); + virtual void updateParameterCache(const bool force); + + /** + * @brief get multicopter landed state + **/ + bool get_landed_state(); + +private: /** * @brief Handles for interesting parameters diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp new file mode 100644 index 0000000000..3643993b18 --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 VtolLandDetector.cpp + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#include "VtolLandDetector.h" +#include + +VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), + _paramHandle(), + _params(), + _airspeedSub(-1), + _parameterSub(-1), + _airspeed{}, + _was_in_air(false), + _airspeed_filtered(0) +{ + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); +} + +void VtolLandDetector::initialize() +{ + MulticopterLandDetector::initialize(); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + // download parameters + updateParameterCache(true); +} + +void VtolLandDetector::updateSubscriptions() +{ + MulticopterLandDetector::updateSubscriptions(); + + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool VtolLandDetector::update() +{ + updateSubscriptions(); + updateParameterCache(false); + + // this is returned from the mutlicopter land detector + bool landed = get_landed_state(); + + // for vtol we additionally consider airspeed + if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + } else { + // if airspeed does not update, set it to zero and rely on multicopter land detector + _airspeed_filtered = 0.0f; + } + + // only consider airspeed if we have been in air before to avoid false + // detections in the case of wind on the ground + if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) { + landed = false; + } + + _was_in_air = !landed; + + return landed; +} + +void VtolLandDetector::updateParameterCache(const bool force) +{ + MulticopterLandDetector::updateParameterCache(force); + + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h new file mode 100644 index 0000000000..d36140d64f --- /dev/null +++ b/src/modules/land_detector/VtolLandDetector.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 VtolLandDetector.h + * Land detection algorithm for vtol + * + * @author Roman Bapst + */ + +#ifndef __VTOL_LAND_DETECTOR_H__ +#define __VTOL_LAND_DETECTOR_H__ + +#include "MulticopterLandDetector.h" +#include + +class VtolLandDetector : public MulticopterLandDetector +{ +public: + VtolLandDetector(); + + +private: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions() override; + + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force) override; + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxAirSpeed; + } _params; + + int _airspeedSub; + int _parameterSub; + + struct airspeed_s _airspeed; + + bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */ + float _airspeed_filtered; /**< low pass filtered airspeed */ +}; + +#endif diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 0aa6e1fad2..1646bf89da 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -53,6 +53,7 @@ #include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" +#include "VtolLandDetector.h" //Function prototypes static int land_detector_start(const char *mode); @@ -113,6 +114,9 @@ static int land_detector_start(const char *mode) } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); + } else if (!strcmp(mode, "vtol")) { + land_detector_task = new VtolLandDetector(); + } else { warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; From c9526af7af78aca439c8236abff3ffa118c4fef4 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 15 Aug 2015 16:30:29 +0200 Subject: [PATCH 048/109] start correct land detector for vtol --- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 7d0c153f09..6462f6954d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -18,4 +18,4 @@ fw_pos_control_l1 start # Start Land Detector # Multicopter for now until we have something for VTOL # -land_detector start multicopter +land_detector start vtol From c6408924cf2bf2507600cb5548a3d7d864be5cd7 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 6 Nov 2015 22:58:24 +0100 Subject: [PATCH 049/109] add vtol land detector to CMakeLists.txt --- src/modules/land_detector/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 3882f246c2..9ef684d3b1 100644 --- a/src/modules/land_detector/CMakeLists.txt +++ b/src/modules/land_detector/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( LandDetector.cpp MulticopterLandDetector.cpp FixedwingLandDetector.cpp + VtolLandDetector.cpp DEPENDS platforms__common ) From b83170b7c3e062e377878831aa29173a2467303a Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Wed, 25 Nov 2015 15:25:39 +0100 Subject: [PATCH 050/109] use indicated airspeed instead of tas --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f6037564e7..d069786699 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -601,7 +601,7 @@ void AttitudeEstimatorQ::task_main() ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); /* Airspeed - take airspeed measurement directly here as no wind is estimated */ - ctrl_state.airspeed = _airspeed.true_airspeed_m_s; + ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; /* Publish to control state topic */ if (_ctrl_state_pub == nullptr) { From c23562c9ed05cc9ff8f7ab6c244ffeaeb9b68d2f Mon Sep 17 00:00:00 2001 From: Amir Date: Fri, 27 Nov 2015 22:28:41 +0100 Subject: [PATCH 051/109] GPS: Add height above the ellipsoid info Modification of the GPS message, add height above the ellipsoid info. --- msg/vehicle_gps_position.msg | 1 + src/drivers/gps/ubx.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 25c532ccaa..718144f327 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -3,6 +3,7 @@ uint64 timestamp_position # Timestamp for position information int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters (millimeters) above MSL +int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid uint64 timestamp_variance float32 s_variance_m_s # speed accuracy estimate m/s diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 643b339be3..f6c3fd7882 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -874,6 +874,7 @@ UBX::payload_rx_done(void) _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m + _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; _gps_position->timestamp_position = hrt_absolute_time(); From 2641ebb99b808fb822240715d710dcb0b70429f9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 13 Nov 2015 12:45:07 -1000 Subject: [PATCH 052/109] Build without pio TEMP - uses nuttx_next_cmake version long term --- src/firmware/nuttx/CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index be6f9b1062..649ce1e8fe 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -41,13 +41,17 @@ add_custom_target(check_weak if(NOT ${BOARD} STREQUAL "sim") + if (config_io_board) + set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin") + endif() px4_nuttx_add_romfs(OUT romfs ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + EXTRAS ${extras} ) - add_dependencies(romfs fw_io) - + if (config_io_board) + add_dependencies(romfs fw_io) + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) From d6c6cb72b2ce8eef284817b1ededb7d3be6a6db3 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:24:53 -1000 Subject: [PATCH 053/109] Support PX4_I2C_BUS_ONBOARD not defined --- src/drivers/ll40ls/ll40ls.cpp | 22 +++++++++++----------- src/drivers/px4flow/px4flow.cpp | 2 ++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8591c4dee3..51e230f189 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -97,7 +97,7 @@ LidarLite *get_dev(const bool use_i2c, const int bus) LidarLite *g_dev = nullptr; if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int); if (g_dev == nullptr) { errx(1, "i2c driver not running"); @@ -231,11 +231,12 @@ void start(const bool use_i2c, const int bus) fail: +#ifdef PX4_I2C_BUS_ONBOARD if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } - +#endif if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -255,17 +256,16 @@ fail: void stop(const bool use_i2c, const int bus) { if (use_i2c) { - if (bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) { - delete g_dev_int; - g_dev_int = nullptr; - } - - } else { + if (bus == PX4_I2C_BUS_EXPANSION) { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } + } else { + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } } } else { @@ -293,7 +293,7 @@ test(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; @@ -366,7 +366,7 @@ reset(const bool use_i2c, const int bus) const char *path; if (use_i2c) { - path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT); } else { path = LL40LS_DEVICE_PATH_PWM; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 18acccc00c..5b7f1e5557 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -694,7 +694,9 @@ start() #ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, #endif +#ifdef PX4_I2C_BUS_ONBOARD PX4_I2C_BUS_ONBOARD, +#endif -1 }; From 09f83e78e5c6cfdae035afb2030db21066ecc751 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:41:16 -1000 Subject: [PATCH 054/109] Support PX4IO_DEVICE_PATH not defined --- src/modules/gpio_led/gpio_led.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 2ff3fc2767..f8c62a88a7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { @@ -207,12 +207,16 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; +#if defined(PX4IO_DEVICE_PATH) if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } +#else + gpio_dev = PX4FMU_DEVICE_PATH; +#endif /* open GPIO device */ priv->gpio_fd = open(gpio_dev, 0); From 32ae638974684f037dcd99913fc00fd1eab21bc6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 08:52:36 -1000 Subject: [PATCH 055/109] Support GPIO_CAN2_RX not defined --- src/modules/uavcan/uavcan_main.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 00671decd2..ee79e0573b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -533,12 +533,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - #ifdef GPIO_CAN1_RX +#if defined(GPIO_CAN1_RX) stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - #endif +#endif +#if defined(GPIO_CAN2_RX) stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); +#endif +#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX) +# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX" +#endif /* * CAN driver init From 1b5db91c356dcc1a33f08fb201d8c0e375371b90 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 14:39:32 -1000 Subject: [PATCH 056/109] Added Fix me re chan 1/2 interactions --- src/drivers/stm32/drv_hrt.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index ba148c1335..475c9c9c75 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -221,6 +221,7 @@ /* * Specific registers and bits used by HRT sub-functions */ +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ #if HRT_TIMER_CHANNEL == 1 # define rCCR_HRT rCCR1 /* compare register for HRT */ # define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ @@ -294,7 +295,7 @@ static void hrt_call_invoke(void); # define GTIM_CCER_CC4NP 0 # define PPM_EDGE_FLIP # endif - +/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/ # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ From f3b7585a89482586487d9e9c35c57f0d0a090e9f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 13 Nov 2015 12:39:00 -1000 Subject: [PATCH 057/109] Wip FMUV3 --- Images/px4fmu-v3.prototype | 12 + Makefile | 3 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 178 +++ cmake/nuttx/px4_impl_nuttx.cmake | 8 + nuttx-configs/px4fmu-v3/include/board.h | 295 +++++ .../px4fmu-v3/include/nsh_romfsimg.h | 42 + nuttx-configs/px4fmu-v3/nsh/Make.defs | 177 +++ nuttx-configs/px4fmu-v3/nsh/appconfig | 52 + nuttx-configs/px4fmu-v3/nsh/defconfig | 1068 +++++++++++++++++ nuttx-configs/px4fmu-v3/nsh/setenv.sh | 67 ++ nuttx-configs/px4fmu-v3/scripts/ld.script | 159 +++ nuttx-configs/px4fmu-v3/src/Makefile | 84 ++ nuttx-configs/px4fmu-v3/src/empty.c | 4 + src/drivers/boards/px4fmu-v2/CMakeLists.txt | 2 +- src/drivers/boards/px4fmu-v3/CMakeLists.txt | 47 + src/drivers/boards/px4fmu-v3/board_config.h | 279 +++++ src/drivers/boards/px4fmu-v3/px4fmu3_init.c | 344 ++++++ src/drivers/boards/px4fmu-v3/px4fmu3_led.c | 106 ++ src/drivers/boards/px4fmu-v3/px4fmu_can.c | 144 +++ .../boards/px4fmu-v3/px4fmu_pwm_servo.c | 105 ++ src/drivers/boards/px4fmu-v3/px4fmu_spi.c | 163 +++ src/drivers/boards/px4fmu-v3/px4fmu_usb.c | 108 ++ src/drivers/drv_gpio.h | 25 +- src/drivers/drv_led.h | 2 + src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +- src/drivers/px4fmu/fmu.cpp | 13 +- src/drivers/stm32/adc/adc.cpp | 34 + src/lib/version/version.h | 4 + 28 files changed, 3524 insertions(+), 5 deletions(-) create mode 100644 Images/px4fmu-v3.prototype create mode 100644 cmake/configs/nuttx_px4fmu-v3_default.cmake create mode 100755 nuttx-configs/px4fmu-v3/include/board.h create mode 100644 nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v3/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v3/nsh/appconfig create mode 100644 nuttx-configs/px4fmu-v3/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v3/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v3/scripts/ld.script create mode 100644 nuttx-configs/px4fmu-v3/src/Makefile create mode 100644 nuttx-configs/px4fmu-v3/src/empty.c create mode 100644 src/drivers/boards/px4fmu-v3/CMakeLists.txt create mode 100644 src/drivers/boards/px4fmu-v3/board_config.h create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_init.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu3_led.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_usb.c diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v3.prototype new file mode 100644 index 0000000000..ada86b9a93 --- /dev/null +++ b/Images/px4fmu-v3.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 11, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv3 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv3", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index b0f49eda3e..61582fc296 100644 --- a/Makefile +++ b/Makefile @@ -130,6 +130,9 @@ px4fmu-v1_default: px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) +px4fmu-v3_default: + $(call cmake-build,nuttx_px4fmu-v3_default) + px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake new file mode 100644 index 0000000000..82f4c1b9a7 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -0,0 +1,178 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/stm32/adc + drivers/stm32/tone_alarm + drivers/led + drivers/px4fmu + drivers/boards/px4fmu-v3 + drivers/rgbled + drivers/mpu9250 + drivers/hmc5883 + drivers/ms5611 + drivers/mb12xx + drivers/srf02 + drivers/sf0x + drivers/ll40ls + drivers/trone + drivers/gps + drivers/pwm_out_sim + drivers/hott + drivers/hott/hott_telemetry + drivers/hott/hott_sensors + drivers/blinkm + drivers/airspeed + drivers/ets_airspeed + drivers/meas_airspeed + drivers/frsky_telemetry + modules/sensors + #drivers/mkblctrl + drivers/px4flow + drivers/oreoled + drivers/gimbal + drivers/pwm_input + drivers/camera_trigger + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/pwm + systemcmds/esc_calib + systemcmds/reboot + #systemcmds/topic_listener + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/mtd + systemcmds/dumpfile + systemcmds/ver + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control + + # + # Logging + # + modules/sdlog2 + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + modules/dataman + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/conversion + lib/launchdetection + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + #examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_extra_libs + ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver + ) + +set(config_io_extra_libs + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index af7391f45c..e54276b099 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -458,6 +458,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4fmu-v3") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v3/include/board.h new file mode 100755 index 0000000000..54e21bbfe7 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/board.h @@ -0,0 +1,295 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v3/nsh/Make.defs new file mode 100644 index 0000000000..6177f4c36d --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/Make.defs @@ -0,0 +1,177 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -Os +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wnested-externs +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v3/nsh/appconfig new file mode 100644 index 0000000000..0e18aa8ef1 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v3/nsh/defconfig new file mode 100644 index 0000000000..957578233b --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/defconfig @@ -0,0 +1,1068 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=n +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# Hot fix for lost data +CONFIG_STM32_RXDMA_BUFFER_SIZE_OVERRIDE=256 +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +# CONFIG_ARCH_IRQPRIO is not set +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=1000 +CONFIG_USERMAIN_STACKSIZE=2500 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HSECLOCK=y +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=4000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_USBDEV_TRACE is not set +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v3/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/px4fmu-v3/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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 [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v3/scripts/ld.script new file mode 100644 index 0000000000..3649227a43 --- /dev/null +++ b/nuttx-configs/px4fmu-v3/scripts/ld.script @@ -0,0 +1,159 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v3/src/Makefile new file mode 100644 index 0000000000..a93664b08b --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v3/src/empty.c new file mode 100644 index 0000000000..5de10699fb --- /dev/null +++ b/nuttx-configs/px4fmu-v3/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/px4fmu-v2/CMakeLists.txt b/src/drivers/boards/px4fmu-v2/CMakeLists.txt index 27cb595c34..19b3f02754 100644 --- a/src/drivers/boards/px4fmu-v2/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v2/CMakeLists.txt @@ -44,4 +44,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v3/CMakeLists.txt new file mode 100644 index 0000000000..e83c7fd7fd --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__boards__px4fmu-v3 + COMPILE_FLAGS + -Os + SRCS + px4fmu_can.c + px4fmu3_init.c + px4fmu_pwm_servo.c + px4fmu_spi.c + px4fmu_usb.c + px4fmu3_led.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v3/board_config.h new file mode 100644 index 0000000000..d2cea93b87 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/board_config.h @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 board_config.h + * + * PX4FMUv2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ +//{GPIO_RSSI_IN, 0, 0}, - pio Analog used as PWM +//{0, GPIO_LED_SAFETY, 0}, pio replacement +//{GPIO_SAFETY_SWITCH_IN, 0, 0}, pio replacement +//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver +//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c +//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver +//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot +//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver +//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +/* Define the Chip Selects */ + +#define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Ready interrupts */ + +#define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_HMC5983 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) +#define GPIO_DRDY_ICM_20608_G (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) + + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +#define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250) +#define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983) +#define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) +#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G) + +#define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250) +#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983) +#define GPIO_DRDY_OFF_ICM_20608_G _PIN_OFF(GPIO_DRDY_ICM_20608_G) + + +/* SPI1 off */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/* Devices on the external bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) + +// ADC defines to be used in sensors.cpp to read from a particular channel +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ + +#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) +#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c new file mode 100644 index 0000000000..a1e53d41c5 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_init.c @@ -0,0 +1,344 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +/* TODO XXX commented this out to get cmake build working */ +/*#ifdef __cplusplus*/ +/*__EXPORT int matherr(struct __exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#else*/ +/*__EXPORT int matherr(struct exception *e)*/ +/*{*/ +/*return 1;*/ +/*}*/ +/*#endif*/ + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_RED); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + +#ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + message("[boot] Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + if (ret != OK) { + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + +#endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c new file mode 100644 index 0000000000..59ed10483e --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + + + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v3/px4fmu_can.c new file mode 100644 index 0000000000..62f170957d --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c new file mode 100644 index 0000000000..600dfef412 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c new file mode 100644 index 0000000000..e0c92c1448 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + + stm32_configgpio(GPIO_DRDY_MPU9250); + stm32_configgpio(GPIO_DRDY_HMC5983); + stm32_configgpio(GPIO_DRDY_ICM_20608_G); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif + +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c new file mode 100644 index 0000000000..e4cfc27e15 --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index acddadfdbe..c5894be9e7 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,29 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +/* + * PX4FMUv3 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE /* * AeroCore GPIO numbers and configuration. @@ -121,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index dbcfde91fa..3daafc963f 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -52,6 +52,8 @@ #define LED_RED 1 /* some boards have red rather than amber */ #define LED_BLUE 0 #define LED_SAFETY 2 +#define LED_GREEN 3 + #define LED_ON _PX4_IOC(_LED_BASE, 0) #define LED_OFF _PX4_IOC(_LED_BASE, 1) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ae77a07fd0..a07cc5fd5e 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); @@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) } temperature -= voltage_diff * temp_slope; -#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 64d9dced4a..5b5dbf3a6f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -121,7 +121,7 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -232,6 +232,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, +#endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index ab458895a6..43189d29f3 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -348,6 +348,40 @@ ADC::update_system_power(void) } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active High + system_power.brick_valid = stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = 1; + + // OC pins are not supported + system_power.periph_5V_OC = 0; + system_power.hipower_5V_OC = 0; + + /* lazily publish */ + if (_to_system_power != nullptr) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } + +#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index b71ba1d0d9..0f3af2ab8e 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,6 +51,10 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#define HW_ARCH "PX4FMU_V3" +#endif + #ifdef CONFIG_ARCH_BOARD_AEROCORE #define HW_ARCH "AEROCORE" #endif From 30bc968ed6c936fcc596fa7be1cb70344b7ef8d1 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 15:06:50 -1000 Subject: [PATCH 058/109] Ran Astyle --- src/drivers/ll40ls/ll40ls.cpp | 4 ++++ src/modules/gpio_led/gpio_led.c | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 51e230f189..5e50d34934 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -232,11 +232,14 @@ void start(const bool use_i2c, const int bus) fail: #ifdef PX4_I2C_BUS_ONBOARD + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } + #endif + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -261,6 +264,7 @@ void stop(const bool use_i2c, const int bus) delete g_dev_ext; g_dev_ext = nullptr; } + } else { if (g_dev_int != nullptr) { delete g_dev_int; diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index f8c62a88a7..302405ce76 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -208,12 +208,14 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; #if defined(PX4IO_DEVICE_PATH) + if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; } else { gpio_dev = PX4FMU_DEVICE_PATH; } + #else gpio_dev = PX4FMU_DEVICE_PATH; #endif From 7fb90d751f80cfacbe25df3b6eadb36c1b37fccb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 17 Nov 2015 15:07:11 -1000 Subject: [PATCH 059/109] Ran Astyle --- src/drivers/boards/px4fmu-v3/px4fmu3_led.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c index 59ed10483e..ddb34e8a56 100644 --- a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c +++ b/src/drivers/boards/px4fmu-v3/px4fmu3_led.c @@ -72,7 +72,7 @@ static uint32_t g_ledmap[] = { __EXPORT void led_init(void) { /* Configure LED GPIOs for output */ - for (size_t l = 0; l < (sizeof(g_ledmap)/sizeof(g_ledmap[0])); l++) { + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { stm32_configgpio(g_ledmap[l]); } } From a67097731a903c256b17a162fcdec85b3167a09f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 04:50:53 -1000 Subject: [PATCH 060/109] Renamed pax4fmu-v3 to pax4fmu-v4 --- Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} | 4 ++-- Makefile | 4 ++-- ...x4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} | 2 +- cmake/nuttx/px4_impl_nuttx.cmake | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h | 0 .../{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig | 2 +- nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile | 0 nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt | 6 +++--- src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c | 0 .../{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} | 0 .../{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} | 0 .../boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c | 0 src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c | 0 src/drivers/drv_gpio.h | 4 ++-- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- src/drivers/px4fmu/fmu.cpp | 4 ++-- src/drivers/stm32/adc/adc.cpp | 4 ++-- src/lib/version/version.h | 4 ++-- src/modules/gpio_led/gpio_led.c | 6 +++--- 27 files changed, 23 insertions(+), 23 deletions(-) rename Images/{px4fmu-v3.prototype => px4fmu-v4.prototype} (68%) rename cmake/configs/{nuttx_px4fmu-v3_default.cmake => nuttx_px4fmu-v4_default.cmake} (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/board.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/include/nsh_romfsimg.h (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/Make.defs (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/appconfig (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/defconfig (99%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/nsh/setenv.sh (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/scripts/ld.script (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/Makefile (100%) rename nuttx-configs/{px4fmu-v3 => px4fmu-v4}/src/empty.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/CMakeLists.txt (96%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/board_config.h (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_can.c (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_init.c => px4fmu-v4/px4fmu_init.c} (100%) rename src/drivers/boards/{px4fmu-v3/px4fmu3_led.c => px4fmu-v4/px4fmu_led.c} (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_pwm_servo.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_spi.c (100%) rename src/drivers/boards/{px4fmu-v3 => px4fmu-v4}/px4fmu_usb.c (100%) diff --git a/Images/px4fmu-v3.prototype b/Images/px4fmu-v4.prototype similarity index 68% rename from Images/px4fmu-v3.prototype rename to Images/px4fmu-v4.prototype index ada86b9a93..24e72f868d 100644 --- a/Images/px4fmu-v3.prototype +++ b/Images/px4fmu-v4.prototype @@ -1,10 +1,10 @@ { "board_id": 11, "magic": "PX4FWv1", - "description": "Firmware for the PX4FMUv3 board", + "description": "Firmware for the PX4FMUv4 board", "image": "", "build_time": 0, - "summary": "PX4FMUv3", + "summary": "PX4FMUv4", "version": "0.1", "image_size": 0, "git_identity": "", diff --git a/Makefile b/Makefile index 61582fc296..cb7c7fbc8e 100644 --- a/Makefile +++ b/Makefile @@ -130,8 +130,8 @@ px4fmu-v1_default: px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v3_default: - $(call cmake-build,nuttx_px4fmu-v3_default) +px4fmu-v4_default: + $(call cmake-build,nuttx_px4fmu-v4_default) px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake similarity index 99% rename from cmake/configs/nuttx_px4fmu-v3_default.cmake rename to cmake/configs/nuttx_px4fmu-v4_default.cmake index 82f4c1b9a7..cc1db95eef 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -12,7 +12,7 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/boards/px4fmu-v3 + drivers/boards/px4fmu-v4 drivers/rgbled drivers/mpu9250 drivers/hmc5883 diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index e54276b099..79de5c3939 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -458,7 +458,7 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) - elseif (${BOARD} STREQUAL "px4fmu-v3") + elseif (${BOARD} STREQUAL "px4fmu-v4") set(cpu_flags -mcpu=cortex-m4 -mthumb diff --git a/nuttx-configs/px4fmu-v3/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/board.h rename to nuttx-configs/px4fmu-v4/include/board.h diff --git a/nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h similarity index 100% rename from nuttx-configs/px4fmu-v3/include/nsh_romfsimg.h rename to nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h diff --git a/nuttx-configs/px4fmu-v3/nsh/Make.defs b/nuttx-configs/px4fmu-v4/nsh/Make.defs similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/Make.defs rename to nuttx-configs/px4fmu-v4/nsh/Make.defs diff --git a/nuttx-configs/px4fmu-v3/nsh/appconfig b/nuttx-configs/px4fmu-v4/nsh/appconfig similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/appconfig rename to nuttx-configs/px4fmu-v4/nsh/appconfig diff --git a/nuttx-configs/px4fmu-v3/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig similarity index 99% rename from nuttx-configs/px4fmu-v3/nsh/defconfig rename to nuttx-configs/px4fmu-v4/nsh/defconfig index 957578233b..551a9cd4ff 100644 --- a/nuttx-configs/px4fmu-v3/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -384,7 +384,7 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V3=y +CONFIG_ARCH_BOARD_PX4FMU_V4=y CONFIG_ARCH_BOARD_CUSTOM=y CONFIG_ARCH_BOARD="" diff --git a/nuttx-configs/px4fmu-v3/nsh/setenv.sh b/nuttx-configs/px4fmu-v4/nsh/setenv.sh similarity index 100% rename from nuttx-configs/px4fmu-v3/nsh/setenv.sh rename to nuttx-configs/px4fmu-v4/nsh/setenv.sh diff --git a/nuttx-configs/px4fmu-v3/scripts/ld.script b/nuttx-configs/px4fmu-v4/scripts/ld.script similarity index 100% rename from nuttx-configs/px4fmu-v3/scripts/ld.script rename to nuttx-configs/px4fmu-v4/scripts/ld.script diff --git a/nuttx-configs/px4fmu-v3/src/Makefile b/nuttx-configs/px4fmu-v4/src/Makefile similarity index 100% rename from nuttx-configs/px4fmu-v3/src/Makefile rename to nuttx-configs/px4fmu-v4/src/Makefile diff --git a/nuttx-configs/px4fmu-v3/src/empty.c b/nuttx-configs/px4fmu-v4/src/empty.c similarity index 100% rename from nuttx-configs/px4fmu-v3/src/empty.c rename to nuttx-configs/px4fmu-v4/src/empty.c diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v4/CMakeLists.txt similarity index 96% rename from src/drivers/boards/px4fmu-v3/CMakeLists.txt rename to src/drivers/boards/px4fmu-v4/CMakeLists.txt index e83c7fd7fd..b16a991331 100644 --- a/src/drivers/boards/px4fmu-v3/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v4/CMakeLists.txt @@ -31,16 +31,16 @@ # ############################################################################ px4_add_module( - MODULE drivers__boards__px4fmu-v3 + MODULE drivers__boards__px4fmu-v4 COMPILE_FLAGS -Os SRCS px4fmu_can.c - px4fmu3_init.c + px4fmu_init.c px4fmu_pwm_servo.c px4fmu_spi.c px4fmu_usb.c - px4fmu3_led.c + px4fmu_led.c DEPENDS platforms__common ) diff --git a/src/drivers/boards/px4fmu-v3/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h similarity index 100% rename from src/drivers/boards/px4fmu-v3/board_config.h rename to src/drivers/boards/px4fmu-v4/board_config.h diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_can.c b/src/drivers/boards/px4fmu-v4/px4fmu_can.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_can.c rename to src/drivers/boards/px4fmu-v4/px4fmu_can.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_init.c rename to src/drivers/boards/px4fmu-v4/px4fmu_init.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu3_led.c b/src/drivers/boards/px4fmu-v4/px4fmu_led.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu3_led.c rename to src/drivers/boards/px4fmu-v4/px4fmu_led.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_pwm_servo.c rename to src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_spi.c rename to src/drivers/boards/px4fmu-v4/px4fmu_spi.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_usb.c b/src/drivers/boards/px4fmu-v4/px4fmu_usb.c similarity index 100% rename from src/drivers/boards/px4fmu-v3/px4fmu_usb.c rename to src/drivers/boards/px4fmu-v4/px4fmu_usb.c diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c5894be9e7..971c22b2d4 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,7 +94,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 /* * PX4FMUv3 GPIO numbers. * @@ -144,7 +144,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_PX4FMU_V3) && !defined(CONFIG_ARCH_BOARD_SITL) + !defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a07cc5fd5e..eaf68a3191 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -335,7 +335,7 @@ MEASAirspeed::cycle() void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); @@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) } temperature -= voltage_diff * temp_slope; -#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } /** diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5b5dbf3a6f..ae1320b491 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -121,7 +121,7 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) static const unsigned _max_actuators = 6; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -232,7 +232,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 43189d29f3..9c6e60947a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -348,7 +348,7 @@ ADC::update_system_power(void) } #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 system_power_s system_power; system_power.timestamp = hrt_absolute_time(); @@ -381,7 +381,7 @@ ADC::update_system_power(void) _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); } -#endif // CONFIG_ARCH_BOARD_PX4FMU_V3 +#endif // CONFIG_ARCH_BOARD_PX4FMU_V4 } uint16_t diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 0f3af2ab8e..9492cec90f 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -51,8 +51,8 @@ #define HW_ARCH "PX4FMU_V2" #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V3 -#define HW_ARCH "PX4FMU_V3" +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 +#define HW_ARCH "PX4FMU_V4" #endif #ifdef CONFIG_ARCH_BOARD_AEROCORE diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 302405ce76..531a921b83 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[]) "\t\tr2\tPX4IO RELAY2" ); #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) errx(1, "usage: gpio_led {start|stop} [-p ]\n" "\t-p \tUse specified AUX OUT pin number (default: 1)" ); @@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[]) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 char *pin_name = "PX4FMU GPIO_EXT1"; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) char pin_name[] = "AUX OUT 1"; #endif @@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[]) } #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V3) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) unsigned int n = strtoul(argv[3], NULL, 10); if (n >= 1 && n <= 6) { From 75b96732b5a0de6b17f0646948077576052a0a11 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 09:20:40 -1000 Subject: [PATCH 061/109] Extended to support PX4FMU_V4 hw --- ROMFS/px4fmu_common/init.d/rc.sensors | 53 +++++++++++++++++++-------- 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 08a044329f..7b8f20839d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -67,32 +67,55 @@ then fi fi else - # FMUv1 - if mpu6000 start + if ver hwcmp PX4FMU_V4 then - fi + # External I2C bus + if hmc5883 -C -T -X start + then + fi - if l3gd20 start - then - fi + # Internal SPI bus + if hmc5883 -C -T -S -R 8 start + then + fi - # MAG selection - if param compare SENS_EXT_MAG 2 - then - if hmc5883 -C -I start + # Internal SPI bus mpu9250 is rotated ?? + if mpu9250 -R 0 start + then + fi + + # Internal SPI bus try MPU6K on ICM-20608-G is rotated 180 deg roll, 270 deg yaw + if mpu6000 -R 14 start then fi else - # Use only external as primary - if param compare SENS_EXT_MAG 1 + # FMUv1 + if mpu6000 start then - if hmc5883 -C -X start + fi + + if l3gd20 start + then + fi + + # MAG selection + if param compare SENS_EXT_MAG 2 + then + if hmc5883 -C -I start then fi else - # auto-detect the primary, prefer external - if hmc5883 start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then + if hmc5883 -C -X start + then + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + fi fi fi fi From cb6327ebabf20a23d394e3ab35602851599bf39d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 09:23:21 -1000 Subject: [PATCH 062/109] Force USE_IO to no on FMUV4 --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 411eb0d892..44b85c35bc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -217,7 +217,12 @@ then # if param compare SYS_USE_IO 1 then - set USE_IO yes + if ver hwcmp PX4FMU_V4 + then + set USE_IO no + else + set USE_IO yes + fi else set USE_IO no fi From 46c63da8bed2ce65fe86de3ed0bff293153df4c4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:13:24 -1000 Subject: [PATCH 063/109] Added support for ICM-20608-G to MPU6000 driver --- src/drivers/mpu6000/mpu6000.cpp | 68 ++++++++++++++++++++++++++------- 1 file changed, 55 insertions(+), 13 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ab2eede349..0c902e2739 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,12 @@ #define BIT_INT_STATUS_DATA 0x01 #define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// Product ID Description for ICM2608 +// There is none + +#define ICM20608_REV_00 0 // Product ID Description for MPU6000 // high 4 bits low 4 bits @@ -208,7 +214,8 @@ class MPU6000_gyro; class MPU6000 : public device::SPI { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); + MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type); virtual ~MPU6000(); virtual int init(); @@ -242,6 +249,7 @@ protected: virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); private: + int _device_type; MPU6000_gyro *_gyro; uint8_t _product; /** product code */ @@ -326,6 +334,15 @@ private: */ int reset(); + /** + * is_icm_device + */ + bool is_icm_device() { return _device_type == 20608;} + /** + * is_mpu_device + */ + bool is_mpu_device() { return _device_type == 6000;} + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -507,8 +524,10 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } -MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : +MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + int device_type) : SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), _call{}, @@ -780,16 +799,15 @@ int MPU6000::reset() int MPU6000::probe() { - uint8_t whoami; - whoami = read_reg(MPUREG_WHOAMI); + uint8_t whoami = read_reg(MPUREG_WHOAMI); + uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; - if (whoami != MPU_WHOAMI_6000) { + if (whoami != expected) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; - } - /* look for a product ID we recognise */ + /* look for a product ID we recognize */ _product = read_reg(MPUREG_PRODUCT_ID); // verify product revision @@ -806,6 +824,7 @@ MPU6000::probe() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + case ICM20608_REV_00: DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -1485,6 +1504,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + // There is no MPUREG_PRODUCT_ID on the icm device + // so lets make dummy it up and allow the rest of the + // code to run as is + if (reg == MPUREG_PRODUCT_ID && is_icm_device()) { + return ICM20608_REV_00; + } + // general register transfer at low clock speed set_frequency(speed); @@ -2054,7 +2080,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation, int range); +void start(bool, enum Rotation, int range, int device_type); void stop(bool); void test(bool); void reset(bool); @@ -2071,7 +2097,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation, int range) +start(bool external_bus, enum Rotation rotation, int range, int device_type) { int fd; MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; @@ -2087,13 +2113,23 @@ start(bool external_bus, enum Rotation rotation, int range) /* create the driver */ if (external_bus) { #ifdef PX4_SPI_BUS_EXT - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); +# if defined(PX4_SPIDEV_EXT_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM); +# else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU; +# endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type); #else errx(0, "External SPI not available"); #endif } else { - *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); +#if defined(PX4_SPIDEV_ICM) + spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM); +#else + spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU; +#endif + *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type); } if (*g_dev_ptr == nullptr) { @@ -2337,6 +2373,7 @@ usage() warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); + warnx(" -M 6000|20608 (default 6000)"); warnx(" -R rotation"); warnx(" -a accel range (in g)"); } @@ -2347,17 +2384,22 @@ int mpu6000_main(int argc, char *argv[]) { bool external_bus = false; + int device_type = 6000; int ch; enum Rotation rotation = ROTATION_NONE; int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; break; + case 'T': + device_type = atoi(optarg); + break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; @@ -2379,7 +2421,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation, accel_range); + mpu6000::start(external_bus, rotation, accel_range, device_type); } if (!strcmp(verb, "stop")) { From 6df5aab064e656b712363bea6a683b8bf8fd332a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 12:19:35 -1000 Subject: [PATCH 064/109] px4fmu-v4 uses MPU6000 driver for ICM-20609-G --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++-- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/boards/px4fmu-v4/board_config.h | 1 + src/drivers/boards/px4fmu-v4/px4fmu_spi.c | 2 +- 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 7b8f20839d..f11e61036a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -84,8 +84,8 @@ else then fi - # Internal SPI bus try MPU6K on ICM-20608-G is rotated 180 deg roll, 270 deg yaw - if mpu6000 -R 14 start + # Internal SPI bus ICM-20608-G is rotated 180 deg roll, 270 deg yaw + if mpu6000 -R 14 -T 20608 start then fi else diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index cc1db95eef..0b873ccb98 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -14,6 +14,7 @@ set(config_module_list drivers/px4fmu drivers/boards/px4fmu-v4 drivers/rgbled + drivers/mpu6000 drivers/mpu9250 drivers/hmc5883 drivers/ms5611 diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d2cea93b87..1fa6f0f2d6 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -127,6 +127,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_ICM 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index e0c92c1448..bdfa47f62c 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -101,7 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, /* SPI select is active low, so write !selected to select the device */ switch (devid) { - case PX4_SPIDEV_GYRO: + case PX4_SPIDEV_ICM: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1); stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1); From 731daec744c1f854c2f3320c4f01048532bc08cf Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 13:58:57 -1000 Subject: [PATCH 065/109] Added missing conditional compilation control for FMUV4 --- src/drivers/px4fmu/fmu.cpp | 108 +++++++++++++++++- .../drivers/airspeedsim/meas_airspeed_sim.cpp | 2 +- 2 files changed, 104 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ae1320b491..4c8b03b44c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1277,7 +1277,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_4PWM); break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case 6: set_mode(MODE_6PWM); @@ -1490,6 +1490,79 @@ PX4FMU::sensor_reset(int ms) // stm32_configgpio(GPIO_ACCEL_DRDY); // stm32_configgpio(GPIO_EXTI_MPU_DRDY); +#endif +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 1; + } + + /* disable SPI bus */ + stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250); + stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983); + stm32_configgpio(GPIO_SPI_CS_OFF_MS5611); + stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0); + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + + stm32_configgpio(GPIO_DRDY_OFF_MPU9250); + stm32_configgpio(GPIO_DRDY_OFF_HMC5983); + stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G); + + stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0); + + /* set the sensor rail off */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_MPU9250); + stm32_configgpio(GPIO_SPI_CS_HMC5983); + stm32_configgpio(GPIO_SPI_CS_MS5611); + stm32_configgpio(GPIO_SPI_CS_ICM_20608_G); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1); + stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1516,6 +1589,31 @@ PX4FMU::peripheral_reset(int ms) /* switch the peripheral rail back on */ stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0); #endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) + + if (ms < 1) { + ms = 10; + } + + /* set the peripheral rails off */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + /* Keep Spektum on to discharge rail*/ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); +#endif } void @@ -1707,7 +1805,7 @@ fmu_new_mode(PortMode new_mode) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) servo_mode = PX4FMU::MODE_6PWM; #endif #if defined(CONFIG_ARCH_BOARD_AEROCORE) @@ -1715,7 +1813,7 @@ fmu_new_mode(PortMode new_mode) #endif break; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) case PORT_PWM4: /* select 4-pin PWM mode */ @@ -2023,7 +2121,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -2112,7 +2210,7 @@ fmu_main(int argc, char *argv[]) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c \n"); #endif exit(1); diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 89691f7483..ae43a2cd5d 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -291,7 +291,7 @@ MEASAirspeedSim::cycle() void MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); From 6585b629e1d0b38ce4f598f66f85e00fb22195f7 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Nov 2015 16:32:08 -1000 Subject: [PATCH 066/109] Set Rotations for 6 and 9 axis --- ROMFS/px4fmu_common/init.d/rc.sensors | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f11e61036a..a822b82a4a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -75,17 +75,18 @@ else fi # Internal SPI bus - if hmc5883 -C -T -S -R 8 start + if hmc5883 -C -T -S -R 0 + start then fi - # Internal SPI bus mpu9250 is rotated ?? - if mpu9250 -R 0 start + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi - # Internal SPI bus ICM-20608-G is rotated 180 deg roll, 270 deg yaw - if mpu6000 -R 14 -T 20608 start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start then fi else From 775b64595c9a0cd4fa64610c5850308918cda8d9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 04:53:51 -1000 Subject: [PATCH 067/109] Fixed hmc5983 --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a822b82a4a..ad8f55c382 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -74,9 +74,8 @@ else then fi - # Internal SPI bus - if hmc5883 -C -T -S -R 0 - start + # Internal SPI bus is rotated 90 deg yaw + if hmc5883 -C -T -S -R 2 start then fi From a631a595e550af517160d813fe795c71b81d0279 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 05:11:58 -1000 Subject: [PATCH 068/109] Added lib/terrain_estimation and lib/runway_takeoff requierd from rebase on master --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 0b873ccb98..850b1a55ea 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -115,6 +115,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config From fa3cccc96a7bd8b3f8f91ebd02bdc3f43e7811d6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 Nov 2015 02:22:40 -1000 Subject: [PATCH 069/109] Start mpu6000 driver before mpu9250 -> need to change cal code --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 ++++---- src/modules/commander/gyro_calibration.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ad8f55c382..a7bc6b0815 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -79,13 +79,13 @@ else then fi - # Internal SPI bus mpu9250 is rotated 90 deg yaw - if mpu9250 -R 2 start + # Internal SPI bus ICM-20608-G is rotated 90 deg yaw + if mpu6000 -R 2 -T 20608 start then fi - # Internal SPI bus ICM-20608-G is rotated 90 deg yaw - if mpu6000 -R 2 -T 20608 start + # Internal SPI bus mpu9250 is rotated 90 deg yaw + if mpu9250 -R 2 start then fi else diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a18a13dfa0..1fcd48b088 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -137,7 +137,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } for (unsigned s = 0; s < max_gyros; s++) { - if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 4) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } From ba4fdf197e38dd0f367a2cd00d7c5405e1b0ef70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:06 +0100 Subject: [PATCH 070/109] Move S.BUS and DSM decoders into RC lib --- src/{modules/px4iofirmware => lib/rc}/dsm.c | 0 src/{modules/px4iofirmware => lib/rc}/sbus.c | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/{modules/px4iofirmware => lib/rc}/dsm.c (100%) rename src/{modules/px4iofirmware => lib/rc}/sbus.c (100%) diff --git a/src/modules/px4iofirmware/dsm.c b/src/lib/rc/dsm.c similarity index 100% rename from src/modules/px4iofirmware/dsm.c rename to src/lib/rc/dsm.c diff --git a/src/modules/px4iofirmware/sbus.c b/src/lib/rc/sbus.c similarity index 100% rename from src/modules/px4iofirmware/sbus.c rename to src/lib/rc/sbus.c From d2b154cd07045106bca1284d4bdc9814e2405849 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:26 +0100 Subject: [PATCH 071/109] Build S.BUS and DSM decoders in RC lib --- src/lib/rc/CMakeLists.txt | 1 + src/lib/rc/dsm.c | 37 ++++++++++++++------------ src/lib/rc/dsm.h | 52 ++++++++++++++++++++++++++++++++++++ src/lib/rc/sbus.c | 35 ++++++++++--------------- src/lib/rc/sbus.h | 55 +++++++++++++++++++++++++++++++++++++++ src/lib/rc/sumd.h | 2 +- 6 files changed, 144 insertions(+), 38 deletions(-) create mode 100644 src/lib/rc/dsm.h create mode 100644 src/lib/rc/sbus.h diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index 686f398e27..dd253a3f0a 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS st24.c sumd.c + sbus.c DEPENDS platforms__common ) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index fc6fbf82b8..7f446685d7 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -46,9 +46,16 @@ #include #include +#include "dsm.h" #include -#include "px4io.h" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + #include +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + #include +#endif #define DSM_FRAME_SIZE 16 /** + */ + +#pragma once + +#include + +__BEGIN_DECLS + +__EXPORT int dsm_init(const char *device); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT void dsm_bind(uint16_t cmd, int pulses); + +__END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 11f336a286..39642f0c1e 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -43,15 +43,9 @@ #include #include -#include - +#include "sbus.h" #include -#define DEBUG -#include "px4io.h" -#include "protocol.h" -#include "debug.h" - #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 #define SBUS_FLAGS_BYTE 23 @@ -77,8 +71,6 @@ #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) -static int sbus_fd = -1; - static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; @@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_ bool *sbus_frame_drop, uint16_t max_channels); int -sbus_init(const char *device) +sbus_init(const char *device, bool singlewire) { - if (sbus_fd < 0) { - sbus_fd = open(device, O_RDWR | O_NONBLOCK); - } + int sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -108,21 +98,24 @@ sbus_init(const char *device) t.c_cflag |= (CSTOPB | PARENB); tcsetattr(sbus_fd, TCSANOW, &t); + if (singlewire) { + /* only defined in configs capable of IOCTL */ + #ifdef SBUS_SERIAL_PORT + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + #endif + } + /* initialise the decoder */ partial_frame_count = 0; last_rx_time = hrt_absolute_time(); - debug("S.Bus: ready"); - - } else { - debug("S.Bus: open failed"); } return sbus_fd; } void -sbus1_output(uint16_t *values, uint16_t num_values) +sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) { uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; @@ -161,14 +154,14 @@ sbus1_output(uint16_t *values, uint16_t num_values) } } void -sbus2_output(uint16_t *values, uint16_t num_values) +sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) { char b = 'B'; write(sbus_fd, &b, 1); } bool -sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } /* decode switch channels if data fields are wide enough */ - if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { + if (max_values > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h new file mode 100644 index 0000000000..afe7a66e52 --- /dev/null +++ b/src/lib/rc/sbus.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 sbus.h + * + * RC protocol definition for S.BUS + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +__BEGIN_DECLS + +__EXPORT int sbus_init(const char *device, bool singlewire); +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels); +__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); +__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); + +__END_DECLS diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index ba9e830b91..65c669f1ed 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -101,7 +101,7 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value); __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); */ -int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); From 2d5b02e967840f0a3808c0efda7395e2fae741bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:14:53 +0100 Subject: [PATCH 072/109] Update IOv1 --- src/drivers/boards/px4io-v1/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index e05e030713..f83f18e503 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -65,6 +65,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *************************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From 83ed9e10967593baca4edb180554739b9a039352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:06 +0100 Subject: [PATCH 073/109] Update IOv2 config --- src/drivers/boards/px4io-v2/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index e9b17e7dc7..d97f3b203c 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,6 +79,8 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) + /* Safety switch button *******************************************************/ #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) From 10a3954232fb44061c6574c57fb459b286e84e93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:23 +0100 Subject: [PATCH 074/109] IO firmware: Depend on external RC lib --- src/modules/px4iofirmware/CMakeLists.txt | 5 +++-- src/modules/px4iofirmware/controls.c | 9 ++++++--- src/modules/px4iofirmware/mixer.cpp | 11 +++++++---- src/modules/px4iofirmware/px4io.h | 8 -------- src/modules/px4iofirmware/registers.c | 1 + 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 3319bec6a2..854bbdcc80 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -64,6 +64,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ") include_directories( ${include_dirs} ${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer + . ) link_directories(${link_dirs}) add_definitions(${definitions}) @@ -71,11 +72,9 @@ add_definitions(${definitions}) set(srcs adc.c controls.c - dsm.c px4io.c registers.c safety.c - sbus.c ../systemlib/up_cxxinitialize.c ../systemlib/perf_counter.c mixer.cpp @@ -86,6 +85,8 @@ set(srcs ../systemlib/pwm_limit/pwm_limit.c ../../lib/rc/st24.c ../../lib/rc/sumd.c + ../../lib/rc/sbus.c + ../../lib/rc/dsm.c ../../drivers/stm32/drv_hrt.c ../../drivers/stm32/drv_pwm_servo.c ../../drivers/boards/${config_io_board}/px4io_init.c diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 65d49a388a..25874868b1 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -46,6 +46,8 @@ #include #include #include +#include +#include #include "px4io.h" @@ -60,7 +62,8 @@ static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; -static int _dsm_fd; +static int _dsm_fd = -1; +int _sbus_fd = -1; static uint16_t rc_value_override = 0; @@ -158,7 +161,7 @@ controls_init(void) _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ - sbus_init("/dev/ttyS2"); + _sbus_fd = sbus_init("/dev/ttyS2", false); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { @@ -240,7 +243,7 @@ controls_tick() perf_begin(c_gather_sbus); bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); if (sbus_updated) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0a0962367d..4ff95bd5af 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -71,6 +72,8 @@ static bool should_arm_nothrottle = false; static bool should_always_enable_pwm = false; static volatile bool in_mixer = false; +extern int _sbus_fd; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -292,10 +295,10 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, 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); + sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } } else if (mixer_servos_armed && should_always_enable_pwm) { @@ -306,11 +309,11 @@ mixer_tick(void) /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT); + sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); } } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 51ed9c6e40..2197b7b776 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -224,14 +224,6 @@ extern uint16_t adc_measure(unsigned channel); */ extern void controls_init(void); extern void controls_tick(void); -extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); -extern void dsm_bind(uint16_t cmd, int pulses); -extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, - uint16_t max_channels); -extern void sbus1_output(uint16_t *values, uint16_t num_values); -extern void sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 18f5db5645..a3351bcbd5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -47,6 +47,7 @@ #include #include #include +#include #include "px4io.h" #include "protocol.h" From c2c6f3c9584bd585dc6f8d855ed22cc0bffa9de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 10:15:59 +0100 Subject: [PATCH 075/109] FMU driver: Add S.BUS and DSM FDs, not active yet --- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4c8b03b44c..9e1ea99861 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -141,6 +141,8 @@ private: orb_advert_t _outputs_pub; unsigned _num_outputs; int _class_instance; + int _sbus_fd; + int _dsm_fd; volatile bool _initialized; bool _servo_armed; @@ -285,6 +287,8 @@ PX4FMU::PX4FMU() : _outputs_pub(nullptr), _num_outputs(0), _class_instance(0), + _sbus_fd(-1), + _dsm_fd(-1), _initialized(false), _servo_armed(false), _pwm_on(false), @@ -318,6 +322,14 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + +#ifdef DSM_SERIAL_PORT + _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + /* only enable this during development */ _debug_enabled = false; } From b137a24f306cb731eb76dd0074718e2835e73c3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:54:39 +0100 Subject: [PATCH 076/109] Free timer 3 --- nuttx-configs/px4fmu-v4/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 551a9cd4ff..96cd833d45 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -241,12 +241,12 @@ CONFIG_STM32_SPI2=y CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM1=y # CONFIG_STM32_TIM2 is not set -CONFIG_STM32_TIM3=y +# CONFIG_STM32_TIM3 is not set CONFIG_STM32_TIM4=y # CONFIG_STM32_TIM5 is not set # CONFIG_STM32_TIM6 is not set # CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM8=y CONFIG_STM32_TIM9=y CONFIG_STM32_TIM10=y CONFIG_STM32_TIM11=y From abfd471fed6904ff74837f525d5e65f19e1324bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 11:57:40 +0100 Subject: [PATCH 077/109] v4 board config: Move HRT to TIM3 --- src/drivers/boards/px4fmu-v4/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 1fa6f0f2d6..3500d35e61 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -208,11 +208,11 @@ __BEGIN_DECLS #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER 3 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 4 /* use capture/compare channel */ -#define HRT_PPM_CHANNEL 2 /* use capture/compare channel 2 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF3|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN7) +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 From 09ef7fa9ca7a97c4b7da3e79b220aadd340c5d5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:08:56 +0100 Subject: [PATCH 078/109] DSM: Fix code style --- src/lib/rc/dsm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 7f446685d7..c068cdae0c 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,11 +50,11 @@ #include #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 - #include +#include #endif #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - #include +#include #endif #define DSM_FRAME_SIZE 16 /** Date: Wed, 25 Nov 2015 12:09:08 +0100 Subject: [PATCH 079/109] S.BUS: Fix code style --- src/lib/rc/sbus.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 39642f0c1e..1ba1ff298e 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -100,9 +100,9 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ - #ifdef SBUS_SERIAL_PORT +#ifdef SBUS_SERIAL_PORT ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - #endif +#endif } /* initialise the decoder */ @@ -161,7 +161,8 @@ sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values) } bool -sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) +sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels) { ssize_t ret; hrt_abstime now; From f101ad6e853d54950e0e5bd660964cf1366df449 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:09:22 +0100 Subject: [PATCH 080/109] S.BUS header: Fix code style --- src/lib/rc/sbus.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index afe7a66e52..1404a9eabd 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -47,7 +47,8 @@ __BEGIN_DECLS __EXPORT int sbus_init(const char *device, bool singlewire); -__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, +__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); From 38724d9e1209fece818a6dda2b6132184ee6bbc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 25 Nov 2015 12:10:42 +0100 Subject: [PATCH 081/109] Sumd: Fix code style --- src/lib/rc/sumd.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index 65c669f1ed..03d7364186 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -102,7 +102,7 @@ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_ uint16_t *channels, uint16_t max_chan_count); */ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS From 46631b5c8ad0c5e5ae653c7af5ffc87cea36d85f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2015 16:20:18 +1100 Subject: [PATCH 082/109] mpu6000: work around apparent ICM20608 bug undocumented register 0x11 sometimes starts with value 0, which gives an offset on the Y accel axis of 2.7m/s/s. It sometimes boots with 0xc9, which gives a zero offset. Force it to 0xc9 to get consistently correct behaviour --- src/drivers/mpu6000/mpu6000.cpp | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0c902e2739..109728fecf 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -151,6 +151,15 @@ #define MPU_WHOAMI_6000 0x68 #define ICM_WHOAMI_20608 0xaf +// ICM2608 specific registers + +/* this is an undocumented register which + if set incorrectly results in getting a 2.7m/s/s offset + on the Y axis of the accelerometer +*/ +#define MPUREG_ICM_UNDOC1 0x11 +#define MPUREG_ICM_UNDOC1_VALUE 0xc9 + // Product ID Description for ICM2608 // There is none @@ -301,7 +310,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 9 +#define MPU6000_NUM_CHECKED_REGISTERS 10 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -486,7 +495,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG + MPUREG_INT_PIN_CFG, + MPUREG_ICM_UNDOC1 }; @@ -789,6 +799,10 @@ int MPU6000::reset() write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); + if (is_icm_device()) { + write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); + } + // Oscillator set // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); @@ -1669,6 +1683,11 @@ MPU6000::check_registers(void) */ uint8_t v; + // the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented) + if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) { + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; + } + if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* From 05840535f183b8b1f12e72869f9fe9face51df03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 14:14:41 +0100 Subject: [PATCH 083/109] Fix math tests --- src/systemcmds/tests/CMakeLists.txt | 2 +- src/systemcmds/tests/test_hrt.c | 2 +- src/systemcmds/tests/test_time.c | 3 ++- src/systemcmds/tests/tests_main.c | 8 +++++--- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 25cad8505c..e9cb903477 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -58,7 +58,6 @@ set(srcs test_rc.c test_conv.cpp test_mount.c - test_eigen.cpp ) if(${OS} STREQUAL "nuttx") @@ -73,6 +72,7 @@ px4_add_module( STACK 60000 COMPILE_FLAGS -Wframe-larger-than=6000 + -Wno-float-equal -O0 SRCS ${srcs} DEPENDS diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index 8bc0888122..fe3c58af6a 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -37,7 +37,7 @@ ****************************************************************************/ #include - +#include #include #include diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index d89f90c683..c67f11f467 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -37,7 +37,8 @@ ****************************************************************************/ #include - +#include +#include #include #include diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e16e0adb56..7c7cbeb28f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -50,11 +50,11 @@ #include #include - -//#include - #include +// Not using Eigen at the moment +#define TESTS_EIGEN_DISABLE + #include "tests.h" /**************************************************************************** @@ -113,7 +113,9 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif +#ifndef TESTS_EIGEN_DISABLE {"eigen", test_eigen, OPT_NOJIGTEST}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From bb2d3a36b7626d91fb2a72503921c99043a05fe5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 14:15:04 +0100 Subject: [PATCH 084/109] Remove old build system artifact --- src/systemcmds/tests/module.mk | 51 ---------------------------------- 1 file changed, 51 deletions(-) delete mode 100644 src/systemcmds/tests/module.mk diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk deleted file mode 100644 index 7e31442525..0000000000 --- a/src/systemcmds/tests/module.mk +++ /dev/null @@ -1,51 +0,0 @@ -# -# Assorted tests and the like -# - -MODULE_COMMAND = tests -MODULE_STACKSIZE = 60000 -MAXOPTIMIZATION = -O0 - -SRCS = test_adc.c \ - test_bson.c \ - test_float.c \ - test_gpio.c \ - test_hott_telemetry.c \ - test_hrt.c \ - test_int.c \ - test_jig_voltages.c \ - test_led.c \ - test_sensors.c \ - test_servo.c \ - test_sleep.c \ - test_uart_baudchange.c \ - test_uart_console.c \ - test_uart_loopback.c \ - test_uart_send.c \ - test_mixer.cpp \ - test_mathlib.cpp \ - test_file.c \ - test_file2.c \ - tests_main.c \ - test_param.c \ - test_ppm_loopback.c \ - test_rc.c \ - test_conv.cpp \ - test_mount.c \ - test_eigen.cpp - -ifeq ($(PX4_TARGET_OS), nuttx) -SRCS += test_time.c - -EXTRACXXFLAGS = -Wframe-larger-than=6000 -else -EXTRACXXFLAGS = -endif - -EXTRACXXFLAGS += -Wno-float-equal - -# Flag is only valid for GCC, not clang -ifneq ($(USE_GCC), 0) -EXTRACXXFLAGS += -Wno-double-promotion -Wno-error=logical-op -endif - From 5500ad16d8119b1713620cc3fdb098cbaa36ee0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:25:30 +0100 Subject: [PATCH 085/109] Fix RC --- src/lib/rc/CMakeLists.txt | 1 + src/lib/rc/dsm.c | 14 ++++++++------ src/lib/rc/dsm.h | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index dd253a3f0a..15bbd71bb9 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( st24.c sumd.c sbus.c + dsm.c DEPENDS platforms__common ) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index c068cdae0c..5d7186ec1b 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -68,6 +68,9 @@ static unsigned dsm_partial_frame_count; /**< Count of bytes received for curren static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values); + /** * Attempt to decode a single channel raw channel datum * @@ -265,7 +268,6 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(GPIO_USART1_RX_SPEKTRUM) -#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else if (dsm_fd < 0) { @@ -332,8 +334,8 @@ dsm_bind(uint16_t cmd, int pulses) * @param[out] num_values pointer to number of raw channel values returned * @return true=DSM frame successfully decoded, false=no update */ -static bool -dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values) { /* debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", @@ -379,7 +381,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) { + if (channel >= max_values) { continue; } @@ -478,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) { ssize_t ret; hrt_abstime now; @@ -528,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by * decode it. */ dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values); + return dsm_decode(now, values, num_values, max_values); } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 9129a389d8..80614d066e 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -46,7 +46,7 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); -__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); +__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); __EXPORT void dsm_bind(uint16_t cmd, int pulses); __END_DECLS From 6bc6eda2951d52bbe3ba8d2ac3455fb81968431f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:28:01 +0100 Subject: [PATCH 086/109] Fixed drivers --- src/drivers/boards/px4fmu-v4/board_config.h | 2 + src/drivers/px4fmu/fmu.cpp | 51 ++++++++++++++++++++- src/modules/px4iofirmware/controls.c | 2 +- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 3500d35e61..9a89a7e962 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,6 +214,8 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ + /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL 2 diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9e1ea99861..452cf1fc2c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -71,6 +71,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -799,6 +804,45 @@ PX4FMU::cycle() update_pwm_rev_mask(); } + bool rc_updated = false; + +#ifdef SBUS_SERIAL_PORT + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (sbus_updated) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } +#endif + +#ifdef DSM_SERIAL_PORT + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data @@ -824,6 +868,12 @@ PX4FMU::cycle() _rc_in.rc_lost_frame_count = 0; _rc_in.rc_total_frame_count = 0; + rc_updated = true; + } + +#endif + + if (rc_updated) { /* lazily advertise on first publication */ if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); @@ -833,7 +883,6 @@ PX4FMU::cycle() } } -#endif work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 25874868b1..fa8d5ad48a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -77,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff; From 193cf888f587292706a0e7578011fe2bc0f9a5c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Nov 2015 20:28:18 +0100 Subject: [PATCH 087/109] FMUv4: Enable RC lib --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 850b1a55ea..ecfeaa36a9 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -109,6 +109,7 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter + lib/rc lib/ecl lib/external_lgpl lib/geo From a3eeafebeb3821806b88c08e0c9957e05a53f575 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Nov 2015 21:31:00 +0100 Subject: [PATCH 088/109] Enable UART6 and enable S.BUS. Tested to work. --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + nuttx-configs/px4fmu-v4/include/board.h | 4 +++ nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- src/drivers/boards/px4fmu-v4/board_config.h | 4 +-- src/drivers/px4fmu/fmu.cpp | 25 +++++++++++-------- .../attitude_estimator_q_main.cpp | 4 +++ src/modules/sensors/sensors.cpp | 2 +- 7 files changed, 27 insertions(+), 15 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index ecfeaa36a9..858c95dd05 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -58,6 +58,7 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/tests # # General system control diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 54e21bbfe7..2f7c1189f3 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -215,6 +215,9 @@ #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + #define GPIO_UART7_RX GPIO_UART7_RX_1 #define GPIO_UART7_TX GPIO_UART7_TX_1 @@ -222,6 +225,7 @@ /* UART RX DMA configurations */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* * CAN diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 96cd833d45..cfe3de4403 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -258,7 +258,7 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_UART4=y # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set +CONFIG_STM32_USART6=y CONFIG_STM32_UART7=y CONFIG_STM32_UART8=y # CONFIG_STM32_IWDG is not set diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 9a89a7e962..ab94498514 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,7 +214,7 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) -#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -225,7 +225,7 @@ __BEGIN_DECLS #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 452cf1fc2c..fbf97c9b68 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -327,12 +327,9 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif -#ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); -#endif - -#ifdef DSM_SERIAL_PORT - _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#ifdef GPIO_SBUS_INV + // this board has a GPIO to control SBUS inversion + stm32_configgpio(GPIO_SBUS_INV); #endif /* only enable this during development */ @@ -648,6 +645,15 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + + #ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); + #endif + _initialized = true; } @@ -839,14 +845,11 @@ PX4FMU::cycle() } #endif -#ifdef DSM_SERIAL_PORT - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); -#endif - #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d069786699..8ca0c74e32 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -206,6 +206,8 @@ private: AttitudeEstimatorQ::AttitudeEstimatorQ() : + _vel_prev(0, 0, 0), + _pos_acc(0, 0, 0), _voter_gyro(3), _voter_accel(3), _voter_mag(3), @@ -725,6 +727,8 @@ bool AttitudeEstimatorQ::update(float dt) corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; } + _q.normalize(); + // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..12f882ed99 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -846,7 +846,7 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; From a953f83a45afcad5211f981b3b732024eed332b9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 27 Nov 2015 15:14:24 +0100 Subject: [PATCH 089/109] XRacer: start telemetry on telem2 --- ROMFS/px4fmu_common/init.d/rcS | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 44b85c35bc..a1105ca2a4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -542,8 +542,12 @@ then # Exit from nsh to free port for mavlink set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 set MAVLINK_F "-r 1200" + # Avoid using ttyS1 for MAVLink on FMUv4 + if ver hwcmp PX4FMU_V4 + then + set MAVLINK_F "-r 1200 -d /dev/ttyS2" + fi fi fi From b196d29662f9257e442efde18467dcd95a30820f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 00:58:51 +0100 Subject: [PATCH 090/109] FMU driver: Output and read at 400 Hz --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fbf97c9b68..bdd7f4d212 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define CONTROL_INPUT_DROP_LIMIT_MS 2 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev From d407f0dfe7271a3214e14ef808c69c894cc14c8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 09:12:57 +0100 Subject: [PATCH 091/109] Fixed FMU code style --- src/drivers/px4fmu/fmu.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bdd7f4d212..cf62ca337f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -649,10 +649,10 @@ PX4FMU::cycle() _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); #endif - #ifdef DSM_SERIAL_PORT - // XXX rather than opening it we need to cycle between protocols until one is locked in - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); - #endif +#ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif _initialized = true; } @@ -843,13 +843,14 @@ PX4FMU::cycle() rc_updated = true; } + #endif #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && - ppm_decoded_channels > 3) { + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; From d07c69d3298206b10836a4e57d5155858875f13b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:16:21 +0100 Subject: [PATCH 092/109] MAVLink: Output RC inputs faster --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b04eea1a0..e4fe7ef473 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1750,7 +1750,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("VFR_HUD", 20.0f); configure_stream("ATTITUDE", 100.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); - configure_stream("RC_CHANNELS", 5.0f); + configure_stream("RC_CHANNELS", 10.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); From 05d248480adade0dc78678821e7968e4fbba213a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:37:59 +0100 Subject: [PATCH 093/109] Initialize FMU actuator control feedback struct --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index cf62ca337f..42f4fbf433 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -594,7 +594,7 @@ PX4FMU::update_pwm_rev_mask() void PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; outputs.noutputs = numvalues; outputs.timestamp = hrt_absolute_time(); From 6cc1bb7ec818b857b4d4cccaaf5a2a25a63ec087 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:39:56 +0100 Subject: [PATCH 094/109] Enable RX DMA for UART6 --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index cfe3de4403..0b653b9eb5 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -306,7 +306,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set From 151402748e037012947f0f233f30ca320e2f9560 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 14:55:03 +0100 Subject: [PATCH 095/109] S.BUS input: Be less sensitive on timing --- src/lib/rc/sbus.c | 5 ++--- src/lib/rc/sbus.h | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index 1ba1ff298e..f5330b720d 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -174,8 +174,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails * The minimum frame spacing is 7ms; with 25 bytes at 100000bps * frame transmission time is ~2ms. * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, + * If an interval of more than 4ms passes between calls, * the first byte we read will be the first byte of a frame. * * In the case where byte(s) are dropped from a frame, this also @@ -184,7 +183,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails */ now = hrt_absolute_time(); - if ((now - last_rx_time) > 3000) { + if ((now - last_rx_time) > 4000) { if (partial_frame_count > 0) { sbus_frame_drops++; partial_frame_count = 0; diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 1404a9eabd..04b91dc126 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -47,6 +47,24 @@ __BEGIN_DECLS __EXPORT int sbus_init(const char *device, bool singlewire); + +/** + * Parse serial bytes on the S.BUS bus + * + * The S.bus protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * If an interval of more than 4ms (7 ms frame spacing plus margin) + * passes between calls, the first byte we read will be the first + * byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); From fa197ee490297b39c6ccff47233ffa515306785d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 14:55:25 +0100 Subject: [PATCH 096/109] FMU driver: Run slightly faster to accomodate S.BUS --- src/drivers/px4fmu/fmu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 42f4fbf433..a5997f861c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_MS 2 +#define CONTROL_INPUT_DROP_LIMIT_US 1500 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev @@ -887,7 +887,7 @@ PX4FMU::cycle() } } - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); + work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US)); } void PX4FMU::work_stop() From f8ccac69bc4c6dd18c15e6edbb14501f2cd7444f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:06 +0100 Subject: [PATCH 097/109] FMUv1: Remove unneeded code --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index dea7678014..4e969692bc 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -145,20 +145,6 @@ static struct spi_dev_s *spi3; #include -#if 0 -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif -#endif - __EXPORT int nsh_archinitialize(void) { int result; From 57ce3cfd0107279b91bde03ac00911ba7ec43a2f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:19 +0100 Subject: [PATCH 098/109] FMUv2: Remove unneeded code --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 116a6ff556..687110fa40 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio; #include -/* TODO XXX commented this out to get cmake build working */ -/*#ifdef __cplusplus*/ -/*__EXPORT int matherr(struct __exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#else*/ -/*__EXPORT int matherr(struct exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#endif*/ - __EXPORT int nsh_archinitialize(void) { From cb286d0ae688d9182b9e68020e8eb05f8644de99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:03:50 +0100 Subject: [PATCH 099/109] FMUv4: Initialize complete board correctly --- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 23 +++++++--------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index a1e53d41c5..7966f57165 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -211,19 +211,6 @@ static struct sdio_dev_s *sdio; #include -/* TODO XXX commented this out to get cmake build working */ -/*#ifdef __cplusplus*/ -/*__EXPORT int matherr(struct __exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#else*/ -/*__EXPORT int matherr(struct exception *e)*/ -/*{*/ -/*return 1;*/ -/*}*/ -/*#endif*/ - __EXPORT int nsh_archinitialize(void) { @@ -231,9 +218,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ - // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ - // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ @@ -243,6 +228,12 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_VDD_BRICK_VALID); stm32_configgpio(GPIO_GPIO5_OUTPUT); + stm32_configgpio(GPIO_SBUS_INV); + stm32_configgpio(GPIO_8266_GPIO0); + stm32_configgpio(GPIO_SPEKTRUM_POWER); + stm32_configgpio(GPIO_8266_PD); + stm32_configgpio(GPIO_8266_RST); + /* configure the high-resolution time/callout interface */ hrt_init(); From 94eff8d9a857c209ca81e46626561e58e5117517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:05:01 +0100 Subject: [PATCH 100/109] Commander: Fix scheduling so its not running at higher prio than control apps --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20efb4db3c..ec47196763 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -300,7 +300,7 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT + 40, 3600, commander_thread_main, (char * const *)&argv[0]); From 00e9804b5ad5dd5d5d5c7ab714aeafa7c2300494 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:05:39 +0100 Subject: [PATCH 101/109] Mixer: Fix dependencies --- src/modules/systemlib/mixer/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/CMakeLists.txt b/src/modules/systemlib/mixer/CMakeLists.txt index 20187eb97f..e9abd7a2a0 100644 --- a/src/modules/systemlib/mixer/CMakeLists.txt +++ b/src/modules/systemlib/mixer/CMakeLists.txt @@ -38,7 +38,7 @@ add_custom_command(OUTPUT mixer_multirotor.generated.h > mixer_multirotor.generated.h) add_custom_target(mixer_gen - DEPENDS mixer_multirotor.generated.h) + DEPENDS mixer_multirotor.generated.h multi_tables.py) px4_add_module( MODULE modules__systemlib__mixer @@ -51,5 +51,6 @@ px4_add_module( mixer_multirotor.generated.h DEPENDS platforms__common + mixer_gen ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From a4aa844151b07c993d814ab9024ffabe40a7a52f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:06:54 +0100 Subject: [PATCH 102/109] FMU driver: Slightly increase run interval to save load --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a5997f861c..28abf37ff2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -94,7 +94,7 @@ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side */ -#define CONTROL_INPUT_DROP_LIMIT_US 1500 +#define CONTROL_INPUT_DROP_LIMIT_US 1800 #define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev From 22c7f72a1c48b72be4bb32eec65a897348466b83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:12:26 +0100 Subject: [PATCH 103/109] FMUv4 config: Fix periph 3.3V en signal, fix typo on 8266 RST signal --- src/drivers/boards/px4fmu-v4/board_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index ab94498514..a29c961f5a 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -224,13 +224,13 @@ __BEGIN_DECLS #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) -#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) /**************************************************************************************************** * Public Types From f99e14144e1abae33650817fbc84c702520a56bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:08 +0100 Subject: [PATCH 104/109] Q estimator: Increase stack size as needed --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 8ca0c74e32..8c0d601bdf 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -264,7 +264,7 @@ int AttitudeEstimatorQ::start() _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2100, + 2400, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); From 3515e6ae918d1e29a656ead9e99ba79f9704957d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:26 +0100 Subject: [PATCH 105/109] INAV: Increase stack size as needed --- .../position_estimator_inav/position_estimator_inav_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 72b250ebe9..cc2c73f05b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -154,7 +154,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300, position_estimator_inav_thread_main, (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); return 0; From 66e9abc7741c567766a3ffd3719f3709fd699b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 17:38:48 +0100 Subject: [PATCH 106/109] SDLOG2: increase stack size as needed --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f15ec8969a..afc96bacc0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -311,7 +311,7 @@ int sdlog2_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 3000, + 3300, sdlog2_thread_main, (char * const *)argv); From ce1b35d024e71b63398bb73c6ebc970a572db59a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:41:18 +0100 Subject: [PATCH 107/109] Q estimator: Fix scope of sensor voting scheme --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 8c0d601bdf..d48b92f0ab 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -333,6 +333,10 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; + int best_gyro = 0; + int best_accel = 0; + int best_mag = 0; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data @@ -369,8 +373,6 @@ void AttitudeEstimatorQ::task_main() } } - int best_gyro, best_accel, best_mag; - // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); From 1fc774bbf881c49138500073c28197dedb036b08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Nov 2015 18:06:39 +0100 Subject: [PATCH 108/109] Q estimator: Increase phase margin --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d48b92f0ab..09ccfc5afa 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -211,9 +211,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 18.0f), - _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250.0f, 10.0f) + _lp_roll_rate(250.0f, 30.0f), + _lp_pitch_rate(250.0f, 30.0f), + _lp_yaw_rate(250.0f, 20.0f) { _voter_mag.set_timeout(200000); From 3ce6ee57e41d28fc5bb47dbd97c65a53c56e9117 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 30 Nov 2015 15:44:36 -1000 Subject: [PATCH 109/109] Set nunmber of UAVCAN interfaces on HW from top level cmake file --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++ cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 ++ src/modules/uavcan/CMakeLists.txt | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index c3a3586f66..62f96b12d4 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 2) + set(config_module_list # # Board support modules diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 858c95dd05..720b9e74b8 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 1) + set(config_module_list # # Board support modules diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index ce348ec910..18ddc0f261 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -41,7 +41,7 @@ add_definitions( -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 - -DUAVCAN_STM32_NUM_IFACES=2 + -DUAVCAN_STM32_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_STM32_TIMER_NUMBER=5 )