ardupilot/libraries/AP_IRLock/AP_IRLock_I2C.cpp

174 lines
5.3 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>
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
see: https://github.com/ArduPilot/ardupilot/issues/5232 and https://gist.github.com/jschall/eac130ed9d6e5dcd9ce582f3eeeb3071
*/
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;
}