ardupilot/libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.cpp

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;
}