mirror of https://github.com/ArduPilot/ardupilot
190 lines
5.4 KiB
C++
190 lines
5.4 KiB
C++
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/*
|
|
driver for UPixel UPFLOW optical flow sensor
|
|
|
|
UPFLOW serial packet description
|
|
byte0: header0 (0xFE)
|
|
byte1: header1 (0x0A)
|
|
byte2: x-motion low byte;
|
|
byte3: x-motion high byte;
|
|
byte4: y-motion low byte;
|
|
byte5: y-motion high byte;
|
|
byte6: dt low byte;
|
|
byte7: dt high byte;
|
|
byte8: reserved;
|
|
byte9: reserved;
|
|
byte10: surface quality
|
|
byte11: hardware version
|
|
byte12:checksum
|
|
byte13:footer (0x55)
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_OpticalFlow_UPFLOW.h"
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
#include <utility>
|
|
#include "OpticalFlow.h"
|
|
#include <stdio.h>
|
|
|
|
#define UPFLOW_HEADER0 (uint8_t)0xFE
|
|
#define UPFLOW_HEADER1 (uint8_t)0x0A
|
|
#define UPFLOW_FOOTER (uint8_t)0x55
|
|
#define UPFLOW_PIXEL_SCALING (1e-4)
|
|
#define UPFLOW_TIMEOUT_SEC 0.3f
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
// constructor
|
|
AP_OpticalFlow_UPFLOW::AP_OpticalFlow_UPFLOW(OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) :
|
|
OpticalFlow_backend(_frontend),
|
|
uart(_uart)
|
|
{
|
|
}
|
|
|
|
// detect the device
|
|
AP_OpticalFlow_UPFLOW *AP_OpticalFlow_UPFLOW::detect(OpticalFlow &_frontend)
|
|
{
|
|
AP_SerialManager *serial_manager = AP::serialmanager().get_singleton();
|
|
if (serial_manager == nullptr) {
|
|
return nullptr;
|
|
}
|
|
|
|
// look for first serial driver with protocol defined as OpticalFlow
|
|
AP_HAL::UARTDriver *uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_OpticalFlow, 0);
|
|
if (uart == nullptr) {
|
|
return nullptr;
|
|
}
|
|
|
|
// we have found a serial port so use it
|
|
AP_OpticalFlow_UPFLOW *sensor = new AP_OpticalFlow_UPFLOW(_frontend, uart);
|
|
return sensor;
|
|
}
|
|
|
|
// initialise the sensor
|
|
void AP_OpticalFlow_UPFLOW::init()
|
|
{
|
|
// sanity check uart
|
|
if (uart == nullptr) {
|
|
return;
|
|
}
|
|
// open serial port with baud rate of 19200
|
|
uart->begin(19200);
|
|
}
|
|
|
|
// read latest values from sensor and fill in x,y and totals.
|
|
void AP_OpticalFlow_UPFLOW::update(void)
|
|
{
|
|
// sanity check uart
|
|
if (uart == nullptr) {
|
|
return;
|
|
}
|
|
|
|
// record gyro values as long as they are being used
|
|
// the sanity check of dt below ensures old gyro values are not used
|
|
if (gyro_sum_count >= 1000) {
|
|
gyro_sum.zero();
|
|
gyro_sum_count = 0;
|
|
}
|
|
const Vector3f& gyro = AP::ahrs().get_gyro();
|
|
gyro_sum.x += gyro.x;
|
|
gyro_sum.y += gyro.y;
|
|
gyro_sum_count++;
|
|
|
|
|
|
bool phrased = false;
|
|
// read any available characters in the serial buffer
|
|
uint32_t nbytes = MIN(uart->available(), 1024u);
|
|
while (nbytes-- > 0) {
|
|
int16_t r = uart->read();
|
|
if (r < 0) {
|
|
break;
|
|
}
|
|
uint8_t c = (uint8_t)r;
|
|
if (recv_count == 0) {
|
|
//Header0
|
|
if (c == UPFLOW_HEADER0) {
|
|
recv_count++;
|
|
sum = 0;
|
|
}
|
|
} else if (recv_count == 1) {
|
|
//Header1
|
|
if (c != UPFLOW_HEADER1) {
|
|
recv_count = 0;
|
|
} else {
|
|
recv_count++;
|
|
}
|
|
} else if (recv_count < 12) {
|
|
//actual data
|
|
((uint8_t*)&updata)[recv_count - 2] = c;
|
|
sum ^= c;
|
|
recv_count++;
|
|
} else if (recv_count == 12) {
|
|
//checksum
|
|
if (sum != c) {
|
|
recv_count = 0;
|
|
} else {
|
|
recv_count++;
|
|
}
|
|
} else { //footer
|
|
if (c == UPFLOW_FOOTER) {
|
|
phrased=true;
|
|
}
|
|
recv_count = 0;
|
|
}
|
|
}
|
|
|
|
// return without updating state if no readings
|
|
if (phrased == false) {
|
|
return;
|
|
}
|
|
|
|
struct OpticalFlow::OpticalFlow_state state {};
|
|
|
|
state.surface_quality = updata.quality;
|
|
|
|
float dt = updata.integration_timespan * 1.0e-6;
|
|
|
|
// sanity check dt
|
|
if (is_positive(dt) && (dt < UPFLOW_TIMEOUT_SEC)) {
|
|
// calculate flow values
|
|
const Vector2f flowScaler = _flowScaler();
|
|
Vector2f flowScaleFactor = Vector2f(1.0f, 1.0f) + flowScaler * 0.001f;
|
|
|
|
// copy flow rates to state structure
|
|
state.flowRate = Vector2f((float)(-updata.flow_x_integral) * flowScaleFactor.x,
|
|
(float)(-updata.flow_y_integral) * flowScaleFactor.y);
|
|
state.flowRate *= UPFLOW_PIXEL_SCALING / dt;
|
|
|
|
// copy average body rate to state structure
|
|
state.bodyRate = Vector2f{gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count};
|
|
|
|
// we only apply yaw to flowRate as body rate comes from AHRS
|
|
_applyYaw(state.flowRate);
|
|
} else {
|
|
// first frame received in some time so cannot calculate flow values
|
|
state.flowRate.zero();
|
|
state.bodyRate.zero();
|
|
}
|
|
|
|
_update_frontend(state);
|
|
|
|
// reset gyro sum
|
|
gyro_sum.zero();
|
|
gyro_sum_count = 0;
|
|
}
|