fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 15:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

replaced optical_flow mavlink message with optical_flow_rad, added gyro_temperature, adapted sd2log for px4flow integral frame

Browse Source
This commit is contained in:
dominiho
2014-10-30 16:39:02 +01:00
parent aba77d062f
commit 9f64953bb9
8 changed files with 80 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+1 -1
View File
@@ -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:
src/modules/mavlink/mavlink_messages.cpp
+22 -18
View File
@@ -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),
src/modules/mavlink/mavlink_receiver.cpp
+26 -20
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -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);
src/modules/sdlog2/sdlog2.c
+8 -5
View File
@@ -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);
src/modules/sdlog2/sdlog2_messages.h
+12 -6
View File
@@ -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 --- */
src/modules/uORB/topics/optical_flow.h
+1
View File
@@ -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 */