aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/4008_ardrone35
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.autostart9
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.interface9
-rw-r--r--ROMFS/px4fmu_common/init.d/rcS23
-rw-r--r--nuttx-configs/px4fmu-v1/nsh/defconfig6
-rw-r--r--src/drivers/px4fmu/fmu.cpp2
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp21
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.h14
-rw-r--r--src/lib/launchdetection/LaunchDetector.cpp24
-rw-r--r--src/lib/launchdetection/LaunchDetector.h10
-rw-r--r--src/lib/launchdetection/LaunchMethod.h8
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp55
-rw-r--r--src/modules/navigator/geofence.cpp11
-rw-r--r--src/modules/navigator/geofence.h6
-rw-r--r--src/modules/position_estimator_inav/inertial_filter.c5
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c2
16 files changed, 177 insertions, 63 deletions
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
new file mode 100644
index 000000000..0f98f7b6c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -0,0 +1,35 @@
+#!nsh
+#
+# ARDrone
+#
+
+echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
+
+# Just use the default multicopter settings.
+sh /etc/init.d/rc.mc_defaults
+
+#
+# Load default params for this platform
+#
+if [ $DO_AUTOCONFIG == yes ]
+then
+ # Set all params here, then disable autoconfig
+ param set MC_ROLL_P 5.0
+ param set MC_ROLLRATE_P 0.13
+ param set MC_ROLLRATE_I 0.0
+ param set MC_ROLLRATE_D 0.0
+ param set MC_PITCH_P 5.0
+ param set MC_PITCHRATE_P 0.13
+ param set MC_PITCHRATE_I 0.0
+ param set MC_PITCHRATE_D 0.0
+ param set MC_YAW_P 1.0
+ param set MC_YAW_D 0.1
+ param set MC_YAWRATE_P 0.15
+ param set MC_YAWRATE_I 0.0
+ param set MC_YAWRATE_D 0.0
+ param set MC_YAW_FF 0.15
+fi
+
+set OUTPUT_MODE ardrone
+set USE_IO no
+set MIXER skip
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 3968af58e..7aaf7133e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -106,6 +106,15 @@ then
sh /etc/init.d/4001_quad_x
fi
+#
+# ARDrone
+#
+
+if param compare SYS_AUTOSTART 4008 8
+then
+ sh /etc/init.d/4008_ardrone
+fi
+
if param compare SYS_AUTOSTART 4010 10
then
sh /etc/init.d/4010_dji_f330
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index d25f01dde..7f793b219 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -3,7 +3,7 @@
# Script to configure control interface
#
-if [ $MIXER != none ]
+if [ $MIXER != none -a $MIXER != skip ]
then
#
# Load mixer
@@ -33,8 +33,11 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
else
- echo "[init] Mixer not defined"
- tone_alarm $TUNE_OUT_ERROR
+ if [ $MIXER != skip ]
+ then
+ echo "[init] Mixer not defined"
+ tone_alarm $TUNE_OUT_ERROR
+ fi
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 76f021e33..57299d772 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -240,6 +240,11 @@ then
fi
fi
+ if [ $OUTPUT_MODE == ardrone ]
+ then
+ set FMU_MODE gpio_serial
+ fi
+
if [ $HIL == yes ]
then
set OUTPUT_MODE hil
@@ -277,9 +282,9 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
- if [ $OUTPUT_MODE == fmu ]
+ if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then
- echo "[init] Use FMU PWM as primary output"
+ echo "[init] Use FMU as primary output"
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
@@ -294,7 +299,7 @@ then
then
set TTYS1_BUSY yes
fi
- if [ $FMU_MODE == pwm_gpio ]
+ if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
then
set TTYS1_BUSY yes
fi
@@ -351,7 +356,7 @@ then
fi
fi
else
- if [ $OUTPUT_MODE != fmu ]
+ if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
then
if fmu mode_$FMU_MODE
then
@@ -367,7 +372,7 @@ then
then
set TTYS1_BUSY yes
fi
- if [ $FMU_MODE == pwm_gpio ]
+ if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
then
set TTYS1_BUSY yes
fi
@@ -428,6 +433,14 @@ then
fi
#
+ # Start up ARDrone Motor interface
+ #
+ if [ $OUTPUT_MODE == ardrone ]
+ then
+ ardrone_interface start -d /dev/ttyS1
+ fi
+
+ #
# Fixed wing setup
#
if [ $VEHICLE_TYPE == fw ]
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 1dc96b3c3..20edc68aa 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
#
# Stack and heap information
#
-CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
@@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_WORKPRIORITY=192
CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2048
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
# CONFIG_LIB_KBDCODEC is not set
# CONFIG_LIB_SLCDCODEC is not set
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 0fbd84924..a70ff6c5c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[])
}
- fprintf(stderr, "FMU: unrecognised command, try:\n");
+ fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#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\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index d5c759b17..12f80c4a3 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -41,12 +41,16 @@
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
-CatapultLaunchMethod::CatapultLaunchMethod() :
- last_timestamp(0),
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+ SuperBlock(parent, "CAT"),
+ last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
- threshold_accel(NULL, "LAUN_CAT_A", false),
- threshold_time(NULL, "LAUN_CAT_T", false)
+ threshold_accel(this, "A"),
+ threshold_time(this, "T")
{
}
@@ -83,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
return launchDetected;
}
-void CatapultLaunchMethod::updateParams()
+
+void CatapultLaunchMethod::reset()
{
- threshold_accel.update();
- threshold_time.update();
+ integrator = 0.0f;
+ launchDetected = false;
+}
+
}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index e943f11e9..55c46ff3f 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -44,22 +44,24 @@
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
-class CatapultLaunchMethod : public LaunchMethod
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
{
public:
- CatapultLaunchMethod();
+ CatapultLaunchMethod(SuperBlock *parent);
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
- void updateParams();
+ void reset();
private:
hrt_abstime last_timestamp;
-// float threshold_accel_raw;
-// float threshold_time;
float integrator;
bool launchDetected;
@@ -69,3 +71,5 @@ private:
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index df9f2fe95..bf539701b 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -42,12 +42,16 @@
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
+namespace launchdetection
+{
+
LaunchDetector::LaunchDetector() :
- launchdetection_on(NULL, "LAUN_ALL_ON", false),
- throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+ SuperBlock(NULL, "LAUN"),
+ launchdetection_on(this, "ALL_ON"),
+ throttlePreTakeoff(this, "THR_PRE")
{
/* init all detectors */
- launchMethods[0] = new CatapultLaunchMethod();
+ launchMethods[0] = new CatapultLaunchMethod(this);
/* update all parameters of all detectors */
@@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector()
}
+void LaunchDetector::reset()
+{
+ /* Reset all detectors */
+ launchMethods[0]->reset();
+}
+
void LaunchDetector::update(float accel_x)
{
if (launchdetection_on.get() == 1) {
@@ -81,12 +91,4 @@ bool LaunchDetector::getLaunchDetected()
return false;
}
-void LaunchDetector::updateParams() {
-
- launchdetection_on.update();
- throttlePreTakeoff.update();
-
- for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
- launchMethods[i]->updateParams();
- }
}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 7c2ff826c..8066ebab3 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -45,18 +45,21 @@
#include <stdint.h>
#include "LaunchMethod.h"
-
+#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
-class __EXPORT LaunchDetector
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
{
public:
LaunchDetector();
~LaunchDetector();
+ void reset();
void update(float accel_x);
bool getLaunchDetected();
- void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -71,5 +74,6 @@ private:
};
+}
#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index bfb5f8cb4..01fa7050e 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -41,14 +41,20 @@
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
+namespace launchdetection
+{
+
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
- virtual void updateParams() = 0;
+ virtual void reset() = 0;
+
protected:
private:
};
+}
+
#endif /* LAUNCHMETHOD_H_ */
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 ed6d8792c..8ba8f2ccb 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
@@ -176,6 +176,8 @@ private:
bool launch_detected;
bool usePreTakeoffThrust;
+ bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
/* Landingslope object */
Landingslope landingslope;
@@ -184,7 +186,7 @@ private:
float target_bearing;
/* Launch detection */
- LaunchDetector launchDetector;
+ launchdetection::LaunchDetector launchDetector;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
@@ -344,6 +346,16 @@ private:
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
+
+ /*
+ * Reset takeoff state
+ */
+ int reset_takeoff_state();
+
+ /*
+ * Reset landing state
+ */
+ int reset_landing_state();
};
namespace l1_control
@@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_motor_lim(false),
land_onslope(false),
launch_detected(false),
+ last_manual(false),
usePreTakeoffThrust(false),
flare_curve_alt_last(0.0f),
launchDetector(),
@@ -976,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* no takeoff detection --> fly */
launch_detected = true;
+ warnx("launchdetection off");
}
}
@@ -1022,19 +1036,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// mission is active
_loiter_hold = false;
- /* reset land state */
+ /* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
- land_noreturn_horizontal = false;
- land_noreturn_vertical = false;
- land_stayonground = false;
- land_motor_lim = false;
- land_onslope = false;
+ reset_landing_state();
}
/* reset takeoff/launch state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
- launch_detected = false;
- usePreTakeoffThrust = false;
+ reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1131,6 +1140,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* no flight mode applies, do not publish an attitude setpoint */
setpoint = false;
+
+ /* reset landing and takeoff state */
+ if (!last_manual) {
+ reset_landing_state();
+ reset_takeoff_state();
+ }
}
if (usePreTakeoffThrust) {
@@ -1141,6 +1156,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
+ if (_control_mode.flag_control_position_enabled) {
+ last_manual = false;
+ } else {
+ last_manual = true;
+ }
+
return setpoint;
}
@@ -1291,6 +1312,22 @@ FixedwingPositionControl::task_main()
_exit(0);
}
+int FixedwingPositionControl::reset_takeoff_state()
+{
+ launch_detected = false;
+ usePreTakeoffThrust = false;
+ launchDetector.reset();
+}
+
+int FixedwingPositionControl::reset_landing_state()
+{
+ land_noreturn_horizontal = false;
+ land_noreturn_vertical = false;
+ land_stayonground = false;
+ land_motor_lim = false;
+ land_onslope = false;
+}
+
int
FixedwingPositionControl::start()
{
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 9bbaf167a..f452a85f7 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -55,11 +55,13 @@
#endif
static const int ERROR = -1;
-Geofence::Geofence() : _fence_pub(-1),
+Geofence::Geofence() :
+ SuperBlock(NULL, "GF"),
+ _fence_pub(-1),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
- param_geofence_on(NULL, "GF_ON", false)
+ param_geofence_on(this, "ON")
{
/* Load initial params */
updateParams();
@@ -292,8 +294,3 @@ int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
}
-
-void Geofence::updateParams()
-{
- param_geofence_on.update();
-}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 5b56ebc7a..9628b7271 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -41,11 +41,13 @@
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
+#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
-class Geofence {
+class Geofence : public control::SuperBlock
+{
private:
orb_advert_t _fence_pub; /**< publish fence topic */
@@ -85,8 +87,6 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
-
- void updateParams();
};
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index 13328edb4..7cd076948 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
- float ewdt = w * dt;
- if (ewdt > 1.0f)
- ewdt = 1.0f; // prevent over-correcting
- ewdt *= e;
+ float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index d6d03367b..a14354138 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
- dt = fmaxf(fminf(0.02, dt), 0.005);
+ dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
t_prev = t;
/* use GPS if it's valid and reference position initialized */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 06:14:13 +0000