mirror of https://github.com/ArduPilot/ardupilot
174 lines
5.2 KiB
C++
174 lines
5.2 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/>.
|
|
*/
|
|
|
|
/*
|
|
* AP_IRLock_I2C.cpp
|
|
*
|
|
* Based on AP_IRLock_PX4 by MLandes
|
|
*
|
|
* See: http://irlock.com/pages/serial-communication-protocol
|
|
*/
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_IRLock_I2C.h"
|
|
#include <stdio.h>
|
|
#include <utility>
|
|
#include <AP_HAL/I2CDevice.h>
|
|
#include <AP_Common/Semaphore.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define IRLOCK_I2C_ADDRESS 0x54
|
|
|
|
#define IRLOCK_SYNC 0xAA55AA55
|
|
|
|
void AP_IRLock_I2C::init(int8_t bus)
|
|
{
|
|
if (bus < 0) {
|
|
// default to i2c external bus
|
|
bus = 1;
|
|
}
|
|
dev = std::move(hal.i2c_mgr->get_device(bus, IRLOCK_I2C_ADDRESS));
|
|
if (!dev) {
|
|
return;
|
|
}
|
|
|
|
// read at 50Hz
|
|
printf("Starting IRLock on I2C\n");
|
|
|
|
dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_IRLock_I2C::read_frames, void));
|
|
}
|
|
|
|
/*
|
|
synchronise with frame start. We expect 0xAA55AA55 at the start of
|
|
a frame
|
|
*/
|
|
bool AP_IRLock_I2C::sync_frame_start(void)
|
|
{
|
|
uint32_t sync_word;
|
|
if (!dev->transfer(nullptr, 0, (uint8_t *)&sync_word, 4)) {
|
|
return false;
|
|
}
|
|
|
|
// record sensor successfully responded to I2C request
|
|
_last_read_ms = AP_HAL::millis();
|
|
|
|
uint8_t count=40;
|
|
while (count-- && sync_word != IRLOCK_SYNC && sync_word != 0) {
|
|
uint8_t sync_byte;
|
|
if (!dev->transfer(nullptr, 0, &sync_byte, 1)) {
|
|
return false;
|
|
}
|
|
if (sync_byte == 0) {
|
|
break;
|
|
}
|
|
sync_word = (sync_word>>8) | (uint32_t(sync_byte)<<24);
|
|
}
|
|
return sync_word == IRLOCK_SYNC;
|
|
}
|
|
|
|
/*
|
|
converts IRLOCK pixels to a position on a normal plane 1m in front of the lens
|
|
based on a characterization of IR-LOCK with the standard lens, focused such that 2.38mm of threads are exposed
|
|
*/
|
|
void AP_IRLock_I2C::pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y)
|
|
{
|
|
ret_x = (-0.00293875727162397f*pix_x + 0.470201163459835f)/(4.43013552642296e-6f*((pix_x - 160.0f)*(pix_x - 160.0f)) +
|
|
4.79331390531725e-6f*((pix_y - 100.0f)*(pix_y - 100.0f)) - 1.0f);
|
|
ret_y = (-0.003056843086277f*pix_y + 0.3056843086277f)/(4.43013552642296e-6f*((pix_x - 160.0f)*(pix_x - 160.0f)) +
|
|
4.79331390531725e-6f*((pix_y - 100.0f)*(pix_y - 100.0f)) - 1.0f);
|
|
}
|
|
|
|
/*
|
|
read a frame from sensor
|
|
*/
|
|
bool AP_IRLock_I2C::read_block(struct frame &irframe)
|
|
{
|
|
if (!dev->transfer(nullptr, 0, (uint8_t*)&irframe, sizeof(irframe))) {
|
|
return false;
|
|
}
|
|
|
|
// record sensor successfully responded to I2C request
|
|
_last_read_ms = AP_HAL::millis();
|
|
|
|
/* check crc */
|
|
uint32_t crc = irframe.signature + irframe.pixel_x + irframe.pixel_y + irframe.pixel_size_x + irframe.pixel_size_y;
|
|
if (crc != irframe.checksum) {
|
|
// printf("bad crc 0x%04x 0x%04x\n", crc, irframe.checksum);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void AP_IRLock_I2C::read_frames(void)
|
|
{
|
|
if (!sync_frame_start()) {
|
|
return;
|
|
}
|
|
struct frame irframe;
|
|
|
|
if (!read_block(irframe)) {
|
|
return;
|
|
}
|
|
|
|
int16_t corner1_pix_x = irframe.pixel_x - irframe.pixel_size_x/2;
|
|
int16_t corner1_pix_y = irframe.pixel_y - irframe.pixel_size_y/2;
|
|
int16_t corner2_pix_x = irframe.pixel_x + irframe.pixel_size_x/2;
|
|
int16_t corner2_pix_y = irframe.pixel_y + irframe.pixel_size_y/2;
|
|
|
|
float corner1_pos_x, corner1_pos_y, corner2_pos_x, corner2_pos_y;
|
|
pixel_to_1M_plane(corner1_pix_x, corner1_pix_y, corner1_pos_x, corner1_pos_y);
|
|
pixel_to_1M_plane(corner2_pix_x, corner2_pix_y, corner2_pos_x, corner2_pos_y);
|
|
|
|
{
|
|
WITH_SEMAPHORE(sem);
|
|
|
|
/* convert to angles */
|
|
_target_info.timestamp = AP_HAL::millis();
|
|
_target_info.pos_x = 0.5f*(corner1_pos_x+corner2_pos_x);
|
|
_target_info.pos_y = 0.5f*(corner1_pos_y+corner2_pos_y);
|
|
_target_info.pos_z = 1.0f;
|
|
}
|
|
|
|
#if 0
|
|
// debugging
|
|
static uint32_t lastt;
|
|
if (_target_info.timestamp - lastt > 2000) {
|
|
lastt = _target_info.timestamp;
|
|
printf("pos_x:%.5f pos_y:%.5f size_x:%.6f size_y:%.5f\n",
|
|
_target_info.pos_x, _target_info.pos_y,
|
|
(corner2_pos_x-corner1_pos_x), (corner2_pos_y-corner1_pos_y));
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// retrieve latest sensor data - returns true if new data is available
|
|
bool AP_IRLock_I2C::update()
|
|
{
|
|
bool new_data = false;
|
|
if (!dev) {
|
|
return false;
|
|
}
|
|
WITH_SEMAPHORE(sem);
|
|
|
|
if (_last_update_ms != _target_info.timestamp) {
|
|
new_data = true;
|
|
}
|
|
_last_update_ms = _target_info.timestamp;
|
|
_flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
|
|
|
|
// return true if new data found
|
|
return new_data;
|
|
}
|