aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rwxr-xr-xTools/px_uploader.py6
-rw-r--r--makefiles/config_px4fmu-v2_default.mk1
-rw-r--r--src/drivers/drv_px4flow.h31
-rw-r--r--src/drivers/px4flow/px4flow.cpp68
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp19
-rw-r--r--src/modules/mavlink/mavlink_main.cpp8
m---------uavcan0
7 files changed, 53 insertions, 80 deletions
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index d8f4884bc..b46db00b5 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -178,9 +178,9 @@ class uploader(object):
MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
- def __init__(self, portname, baudrate):
+ def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5):
# open the port, keep the default timeout short so we can poll quickly
- self.port = serial.Serial(portname, baudrate, timeout=2.0)
+ self.port = serial.Serial(portname, baudrate)
self.otp = b''
self.sn = b''
@@ -195,7 +195,7 @@ class uploader(object):
def __recv(self, count=1):
c = self.port.read(count)
if len(c) < 1:
- raise RuntimeError("timeout waiting for data")
+ raise RuntimeError("timeout waiting for data (%u bytes)", count)
# print("recv " + binascii.hexlify(c))
return c
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index d0a40445d..5f146686c 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
MODULES += drivers/pca8574
+MODULES += drivers/px4flow
# Needs to be burned to the ground and re-written; for now,
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
index 76ec55c3e..ab640837b 100644
--- a/src/drivers/drv_px4flow.h
+++ b/src/drivers/drv_px4flow.h
@@ -46,37 +46,6 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Optical flow in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- *
- * @warning If possible the usage of the raw flow and performing rotation-compensation
- * using the autopilot angular rate estimate is recommended.
- */
-struct px4flow_report {
-
- uint64_t timestamp; /**< in microseconds since system start */
-
- int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
- int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
- float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
- float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
- float ground_distance_m; /**< Altitude / distance to ground in meters */
- uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
- uint8_t sensor_id; /**< id of the sensor emitting the flow value */
-
-};
-
-/**
- * @}
- */
-
/*
* ObjDev tag for px4flow data.
*/
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index f214b5964..60ad3c1af 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -37,7 +37,7 @@
*
* Driver for the PX4FLOW module connected via I2C.
*/
-
+
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
-//#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/optical_flow.h>
#include <board_config.h>
@@ -80,7 +80,7 @@
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
-
+
virtual int init();
-
+
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
-
+
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
-
+
protected:
virtual int probe();
@@ -136,13 +136,13 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
-
+
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
-
+
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -151,7 +151,7 @@ private:
* @return True if the device is present.
*/
int probe_address(uint8_t address);
-
+
/**
* Initialise the automatic measurement state machine and start it.
*
@@ -159,12 +159,12 @@ private:
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
-
+
/**
* Stop the automatic measurement state machine.
*/
void stop();
-
+
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@@ -179,8 +179,8 @@ private:
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
-
-
+
+
};
/*
@@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
{
// enable debug() calls
_debug_enabled = true;
-
+
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@@ -226,13 +226,13 @@ PX4FLOW::init()
goto out;
/* allocate basic report buffers */
- _reports = new RingBuffer(2, sizeof(px4flow_report));
+ _reports = new RingBuffer(2, sizeof(struct optical_flow_s));
if (_reports == nullptr)
goto out;
/* get a publish handle on the px4flow topic */
- struct px4flow_report zero_report;
+ struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
@@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
-
+
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
-
+
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
-
+
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
-
+
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
- unsigned count = buflen / sizeof(struct px4flow_report);
- struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+ unsigned count = buflen / sizeof(struct optical_flow_s);
+ struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -425,7 +425,7 @@ PX4FLOW::measure()
return ret;
}
ret = OK;
-
+
return ret;
}
@@ -433,14 +433,14 @@ int
PX4FLOW::collect()
{
int ret = -EIO;
-
+
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
-
+
perf_begin(_sample_perf);
-
+
ret = transfer(nullptr, 0, &val[0], 22);
-
+
if (ret < 0)
{
log("error reading from sensor: %d", ret);
@@ -448,7 +448,7 @@ PX4FLOW::collect()
perf_end(_sample_perf);
return ret;
}
-
+
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
@@ -466,7 +466,7 @@ PX4FLOW::collect()
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
- struct px4flow_report report;
+ struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
@@ -503,7 +503,7 @@ PX4FLOW::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
-
+
/* notify about state change */
struct subsystem_info_s info = {
true,
@@ -644,7 +644,7 @@ start()
fail:
- if (g_dev != nullptr)
+ if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
@@ -678,7 +678,7 @@ void stop()
void
test()
{
- struct px4flow_report report;
+ struct optical_flow_s report;
ssize_t sz;
int ret;
@@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "start"))
px4flow::start();
-
+
/*
* Stop the driver
*/
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 ee112a40e..517333c80 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -145,6 +145,7 @@ private:
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
+ bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
- _setpoint_valid(false)
+ _setpoint_valid(false),
+ _debug(false)
{
/* safely initialize structs */
_att = {};
@@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
- airspeed = _airspeed.true_airspeed_m_s;
+ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
+ airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@@ -792,7 +795,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@@ -815,7 +818,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@@ -828,7 +831,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@@ -852,7 +855,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
@@ -860,13 +863,13 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
- if (loop_counter % 10 == 0) {
+ if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ed7e879d3..940e64144 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1396,14 +1396,14 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
- configure_stream("OPTICAL_FLOW", 0.5f);
+ configure_stream("OPTICAL_FLOW", 20.0f);
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
- configure_stream("ATTITUDE", 15.0f);
- configure_stream("GLOBAL_POSITION_INT", 15.0f);
- configure_stream("CAMERA_CAPTURE", 1.0f);
+ configure_stream("ATTITUDE", 50.0f);
+ configure_stream("GLOBAL_POSITION_INT", 50.0f);
+ configure_stream("CAMERA_CAPTURE", 2.0f);
break;
default:
diff --git a/uavcan b/uavcan
-Subproject 75153eb6436d0cc00679056ff8e916c6d99057a
+Subproject c7872def16e82a8b318d571829fe9622e2d35ff
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-09 00:37:56 +0000