aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/px4flow/px4flow.cpp14
-rw-r--r--src/modules/mavlink/mavlink_main.cpp2
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp40
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp46
-rw-r--r--src/modules/mavlink/mavlink_receiver.h2
-rw-r--r--src/modules/sdlog2/sdlog2.c13
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h18
-rw-r--r--src/modules/uORB/topics/optical_flow.h1
8 files changed, 80 insertions, 56 deletions
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index c3660a967..daaf02a77 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -116,6 +116,7 @@ typedef struct i2c_integral_frame {
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t ground_distance;
+ int16_t gyro_temperature;
uint8_t qual;
} __attribute__((packed));
struct i2c_integral_frame f_integral;
@@ -456,16 +457,16 @@ PX4FLOW::collect()
int ret = -EIO;
/* read from the sensor */
- uint8_t val[46] = { 0 };
+ uint8_t val[47] = { 0 };
perf_begin(_sample_perf);
if (PX4FLOW_REG == 0x00) {
- ret = transfer(nullptr, 0, &val[0], 45); // read 45 bytes (22+23 : frame1 + frame2)
+ ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
}
if (PX4FLOW_REG == 0x16) {
- ret = transfer(nullptr, 0, &val[0], 23); // read 23 bytes (only frame2)
+ ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
}
if (ret < 0) {
@@ -500,7 +501,8 @@ PX4FLOW::collect()
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
| val[39] << 8 | val[38];
f_integral.ground_distance = val[43] << 8 | val[42];
- f_integral.qual = val[44];
+ f_integral.gyro_temperature = val[45] << 8 | val[44];
+ f_integral.qual = val[46];
}
if (PX4FLOW_REG == 0x16) {
@@ -513,7 +515,8 @@ PX4FLOW::collect()
f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
f_integral.ground_distance = val[21] << 8 | val[20];
- f_integral.qual = val[22];
+ f_integral.gyro_temperature = val[23] << 8 | val[22];
+ f_integral.qual = val[24];
}
@@ -530,6 +533,7 @@ PX4FLOW::collect()
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+ report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 4448f8d6f..3c1d360c2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1403,7 +1403,7 @@ 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", 5.0f);
+ configure_stream("OPTICAL_FLOW_RAD", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf..d955b5f76 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1786,33 +1786,32 @@ protected:
}
};
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
public:
const char *get_name() const
{
- return MavlinkStreamOpticalFlow::get_name_static();
+ return MavlinkStreamOpticalFlowRad::get_name_static();
}
static const char *get_name_static()
{
- return "OPTICAL_FLOW";
+ return "OPTICAL_FLOW_RAD";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_OPTICAL_FLOW;
+ return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamOpticalFlow(mavlink);
+ return new MavlinkStreamOpticalFlowRad(mavlink);
}
unsigned get_size()
{
- return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+ return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
@@ -1820,11 +1819,11 @@ private:
uint64_t _flow_time;
/* do not allow top copying this class */
- MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
- MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+ MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
protected:
- explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+ explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
_flow_time(0)
{}
@@ -1834,18 +1833,23 @@ protected:
struct optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) {
- mavlink_optical_flow_t msg;
+ mavlink_optical_flow_rad_t msg;
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
- msg.flow_x = flow.flow_raw_x;
- msg.flow_y = flow.flow_raw_y;
- msg.flow_comp_m_x = flow.flow_comp_x_m;
- msg.flow_comp_m_y = flow.flow_comp_y_m;
+ msg.integrated_x = flow.pixel_flow_x_integral;
+ msg.integrated_y = flow.pixel_flow_y_integral;
+ msg.integrated_xgyro = flow.gyro_x_rate_integral;
+ msg.integrated_ygyro = flow.gyro_y_rate_integral;
+ msg.integrated_zgyro = flow.gyro_z_rate_integral;
+ msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
- msg.ground_distance = flow.ground_distance_m;
+ msg.integration_time_us = flow.integration_timespan;
+ msg.sensor_id = flow.sensor_id;
+ msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+ msg.temperature = flow.gyro_temperature;
- _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
}
}
};
@@ -2151,7 +2155,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
- new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+ new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index e8d783847..bee58f89b 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_command_int(msg);
break;
- case MAVLINK_MSG_ID_OPTICAL_FLOW:
- handle_message_optical_flow(msg);
+ case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+ handle_message_optical_flow_rad(msg);
break;
case MAVLINK_MSG_ID_SET_MODE:
@@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
}
void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
{
/* optical flow */
- mavlink_optical_flow_t flow;
- mavlink_msg_optical_flow_decode(msg, &flow);
+ mavlink_optical_flow_rad_t flow;
+ mavlink_msg_optical_flow_rad_decode(msg, &flow);
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
- f.timestamp = hrt_absolute_time();
- f.flow_timestamp = flow.time_usec;
- f.flow_raw_x = flow.flow_x;
- f.flow_raw_y = flow.flow_y;
- f.flow_comp_x_m = flow.flow_comp_m_x;
- f.flow_comp_y_m = flow.flow_comp_m_y;
- f.ground_distance_m = flow.ground_distance;
+ f.timestamp = flow.time_usec;
+ f.integration_timespan = flow.integration_time_us;
+ f.pixel_flow_x_integral = flow.integrated_x;
+ f.pixel_flow_y_integral = flow.integrated_y;
+ f.gyro_x_rate_integral = flow.integrated_xgyro;
+ f.gyro_y_rate_integral = flow.integrated_ygyro;
+ f.gyro_z_rate_integral = flow.integrated_zgyro;
+ f.time_since_last_sonar_update = flow.time_delta_distance_us;
+ f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
+ f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -389,15 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
struct optical_flow_s f;
memset(&f, 0, sizeof(f));
- f.timestamp = hrt_absolute_time();
- f.flow_timestamp = flow.time_usec;
- f.flow_raw_x = flow.flow_x;
- f.flow_raw_y = flow.flow_y;
- f.flow_comp_x_m = flow.flow_comp_m_x;
- f.flow_comp_y_m = flow.flow_comp_m_y;
- f.ground_distance_m = flow.ground_distance;
+ f.timestamp = flow.time_usec;
+ f.integration_timespan = flow.integration_time_us;
+ f.pixel_flow_x_integral = flow.integrated_x;
+ f.pixel_flow_y_integral = flow.integrated_y;
+ f.gyro_x_rate_integral = flow.integrated_xgyro;
+ f.gyro_y_rate_integral = flow.integrated_ygyro;
+ f.gyro_z_rate_integral = flow.integrated_zgyro;
+ f.time_since_last_sonar_update = flow.time_delta_distance_us;
+ f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
+ f.gyro_temperature = flow.temperature;
if (_flow_pub < 0) {
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -413,7 +419,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
- r.distance = flow.ground_distance;
+ r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e5f2c6a73..a057074a7 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,7 +112,7 @@ private:
void handle_message(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
- void handle_message_optical_flow(mavlink_message_t *msg);
+ void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 9bde37432..54c7b0c83 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1507,11 +1507,14 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
- log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
- log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
- log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
- log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
- log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+ log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
+ log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
+ log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
+ log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
+ log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
+ log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
+ log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
+ log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d49fc0c79..5264ff1c8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -200,13 +200,19 @@ struct log_ARSP_s {
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
- int16_t flow_raw_x;
- int16_t flow_raw_y;
- float flow_comp_x;
- float flow_comp_y;
- float distance;
- uint8_t quality;
+ uint64_t timestamp;
uint8_t sensor_id;
+ float pixel_flow_x_integral;
+ float pixel_flow_y_integral;
+ float gyro_x_rate_integral;
+ float gyro_y_rate_integral;
+ float gyro_z_rate_integral;
+ float ground_distance_m;
+ uint32_t integration_timespan;
+ uint32_t time_since_last_sonar_update;
+ uint16_t frame_count_since_last_readout;
+ int16_t gyro_temperature;
+ uint8_t quality;
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index f3731d213..d3dc46ee0 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -66,6 +66,7 @@ struct optical_flow_s {
uint32_t integration_timespan; /**<accumulation timespan in microseconds */
uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
+ int16_t gyro_temperature;/**< Temperature * 100 in centi-degrees Celsius */
uint8_t quality; /**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 18:47:53 +0000