dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

acount for SENS_FLOW_ROT in simulation

This commit is contained in:
ChristophTobler 2016-12-29 14:37:59 +01:00 committed by Lorenz Meier
parent fd91f998c4
commit 06436e753e
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/simulator/simulator_mavlink.cpp
View File

@ -43,7 +43,7 @@
#include <sys/socket.h>
#include <netinet/in.h>
#include <pthread.h>
#include <conversion/rotation.h>
#include <mathlib/mathlib.h>
extern "C" __EXPORT hrt_abstime hrt_reset(void);
@ -1047,6 +1047,14 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
flow.pixel_flow_y_integral = flow_mavlink->integrated_y;
flow.quality = flow_mavlink->quality;
/* rotate measurements according to parameter */
enum Rotation flow_rot;
param_get(param_find("SENS_FLOW_ROT"), &flow_rot);
float zeroval = 0.0f;
rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval);
rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral);
int flow_multi;
orb_publish_auto(ORB_ID(optical_flow), &_flow_pub, &flow, &flow_multi, ORB_PRIO_HIGH);