diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
new file mode 100644
index 0000000000..dd3a0cca8f
--- /dev/null
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
@@ -0,0 +1,72 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+/*
+ * AP_OpticalFlow_PX4.cpp - ardupilot library for PX4Flow sensor
+ *
+ */
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include
+#include "AP_OpticalFlow_PX4.h"
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+void AP_OpticalFlow_PX4::init(void)
+{
+ _fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+ // check for failure to open device
+ if (_fd < 0) {
+ hal.console->printf("Unable to open " PX4FLOW_DEVICE_PATH "\n");
+ return;
+ }
+
+ // if we got this far, the sensor must be healthy
+ _flags.healthy = true;
+}
+
+// update - read latest values from sensor and fill in x,y and totals.
+void AP_OpticalFlow_PX4::update(void)
+{
+ struct px4flow_report report;
+ while (::read(_fd, &report, sizeof(px4flow_report)) == sizeof(px4flow_report) && report.timestamp != _last_timestamp) {
+ _device_id = report.sensor_id;
+ _surface_quality = report.quality;
+ _raw.x = report.flow_raw_x;
+ _raw.y = report.flow_raw_y;
+ _velocity.x = report.flow_comp_x_m;
+ _velocity.y = report.flow_comp_y_m;
+ _last_timestamp = report.timestamp;
+ _last_update = hal.scheduler->millis();
+ }
+
+ // To-Do: add support for PX4Flow's sonar by retrieving ground_distance_m from report
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
new file mode 100644
index 0000000000..c40b336d79
--- /dev/null
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
@@ -0,0 +1,26 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_OpticalFlow_PX4_H
+#define AP_OpticalFlow_PX4_H
+
+#include "OpticalFlow.h"
+
+class AP_OpticalFlow_PX4 : public OpticalFlow
+{
+public:
+
+ /// constructor
+ AP_OpticalFlow_PX4(const AP_AHRS &ahrs) : OpticalFlow(ahrs) {};
+
+ // init - initialise the sensor
+ virtual void init();
+
+ // update - read latest values from sensor and fill in x,y and totals.
+ virtual void update(void);
+
+private:
+ int _fd; // file descriptor for sensor
+ uint64_t _last_timestamp; // time of last update (used to avoid processing old reports)
+};
+
+#endif