diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index d811fe5dbb..4c0b72f32a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -11,6 +11,8 @@ #include #endif +//param show PLND_type + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AC_PrecLand::var_info[] = { @@ -178,8 +180,7 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) } } -bool AC_PrecLand::target_acquired() -{ +bool AC_PrecLand::target_acquired() { if ((AP_HAL::millis()-_last_update_ms) >= 2000) { _estimator_initialized = false; _target_acquired = false; @@ -188,6 +189,17 @@ bool AC_PrecLand::target_acquired() return _target_acquired; } +void AC_PrecLand::set_target_acquired(bool _target_acquired_value) { + if (_target_acquired_value == false) { + _target_acquired = _target_acquired_value; + _estimator_initialized = false; + } + else { + _target_acquired = _target_acquired_value; + } +} + + bool AC_PrecLand::get_height_above_target_cm(int32_t& ret) { #if !HAL_WITH_UAVCAN return false; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 05cecd4b03..7e352ea8dc 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -77,6 +77,9 @@ class AC_PrecLand // returns true when the landing target has been detected bool target_acquired(); + // sets the value of _target_acquired + void set_target_acquired(bool _target_acquired_value); + // process a LANDING_TARGET mavlink message void handle_msg(mavlink_message_t* msg); diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index e38ee89edc..2ec18760c7 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -10,7 +10,7 @@ class AC_PrecLand_Backend { public: // Constructor - AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : + AC_PrecLand_Backend(AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : _frontend(frontend), _state(state) {} @@ -43,6 +43,6 @@ class AC_PrecLand_Backend int8_t get_bus(void) const { return _frontend._bus.get(); } protected: - const AC_PrecLand& _frontend; // reference to precision landing front end + AC_PrecLand& _frontend; // reference to precision landing front end AC_PrecLand::precland_state &_state; // reference to this instances state }; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 05c2122950..b64544ca9c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -1,10 +1,11 @@ + #include #include "AC_PrecLand_Companion.h" extern const AP_HAL::HAL& hal; // Constructor -AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) +AC_PrecLand_Companion::AC_PrecLand_Companion(AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : AC_PrecLand_Backend(frontend, state), _timestamp_us(0), _distance_to_target(0.0f), diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 5b45e03e09..939980482c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -1,3 +1,4 @@ + #pragma once #include @@ -13,7 +14,7 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend { public: // Constructor - AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + AC_PrecLand_Companion(AC_PrecLand& frontend, AC_PrecLand::precland_state& state); // perform any required initialisation of backend void init() override; diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index bc014a3e23..89a6abc036 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -1,23 +1,59 @@ + #include #include "AC_PrecLand_IRLock.h" +#include extern const AP_HAL::HAL& hal; // Constructor -AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) +AC_PrecLand_IRLock::AC_PrecLand_IRLock(AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : AC_PrecLand_Backend(frontend, state), irlock(), _have_los_meas(false), - _los_meas_time_ms(0) + _los_meas_time_ms(0), + _multiple_target_timestamp_log(), + _index(0) { } // init - perform initialisation of this backend -void AC_PrecLand_IRLock::init() +void AC_PrecLand_IRLock::init() { irlock.init(get_bus()); } +//multiple_target_check - Checks if multiple targets detected for 500 ms continously. If yes, sets target_acquired=False which calls contingency landing +void AC_PrecLand_IRLock::multiple_target_check() +{ + if (_index == 24) { + _index = 0; + } + _multiple_target_timestamp_log[_index] = irlock.num_targets(); + _index++; + +/* + printf("\n _multiple_target_timestamp_log: "); + for (size_t j=0; j<24; j++) { + printf("%u ", _multiple_target_timestamp_log[j]); + } +*/ + size_t count = 0; + for (size_t j=0; j<24; j++) { + if (_multiple_target_timestamp_log[j] > 1) { + count++; + } + } + uint16_t multiple_target_count_percentage = count*100/25; +// printf("\nCount: %u -- Multiple_target_count_percentage: %u", irlock.num_targets(), multiple_target_count_percentage); + if (multiple_target_count_percentage >= 80) { +// printf("\n----------------------------------------------------------------------HIGH COUNT PERCENTAGE: %u ------setting TARGET ACQUIRED FALSE -- %", multiple_target_count_percentage); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Multiple Targets Detected"); + // Send sn alert telling multiple targets detected + _frontend.set_target_acquired(false); + } +} + + // update - give chance to driver to get updates from sensor void AC_PrecLand_IRLock::update() { @@ -31,6 +67,7 @@ void AC_PrecLand_IRLock::update() irlock.get_unit_vector_body(_los_meas_body); _have_los_meas = true; _los_meas_time_ms = irlock.last_update_ms(); + multiple_target_check(); } _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index bcf78f57c2..fa46658615 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -1,3 +1,4 @@ + #pragma once #include @@ -19,7 +20,7 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend public: // Constructor - AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + AC_PrecLand_IRLock(AC_PrecLand& frontend, AC_PrecLand::precland_state& state); // perform any required initialisation of backend void init() override; @@ -37,10 +38,17 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend // return true if there is a valid los measurement available bool have_los_meas() override; + //multiple_target_check - Checks if multiple targets detected for 500 ms continously. + //If yes, sets target_acquired=False which calls contingency landing + void multiple_target_check(); + + private: AP_IRLock_I2C irlock; Vector3f _los_meas_body; // unit vector in body frame pointing towards target bool _have_los_meas; // true if there is a valid measurement from the camera uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured + uint32_t _multiple_target_timestamp_log[25]; // array of timestamps when multiple targets detected + size_t _index; }; diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.cpp b/libraries/AP_IRLock/AP_IRLock_I2C.cpp index d0af466ac1..ccce43195d 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.cpp +++ b/libraries/AP_IRLock/AP_IRLock_I2C.cpp @@ -1,14 +1,12 @@ -/* + /* 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 . */ @@ -28,12 +26,12 @@ extern const AP_HAL::HAL& hal; -#define IRLOCK_I2C_ADDRESS 0x54 +#define IRLOCK_I2C_ADDRESS 0x54 -#define IRLOCK_SYNC 0xAA55AA55 -void AP_IRLock_I2C::init(int8_t bus) -{ + +void AP_IRLock_I2C::init(int8_t bus) { + // printf("\nINIT"); if (bus < 0) { // default to i2c external bus bus = 1; @@ -42,133 +40,105 @@ void AP_IRLock_I2C::init(int8_t bus) if (!dev) { return; } - sem = hal.util->new_semaphore(); - // 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; +// // printf("Initializing IRLock on I2C\n"); + dev->register_periodic_callback(200, FUNCTOR_BIND_MEMBER(&AP_IRLock_I2C::read_frames, void)); + // printf("\nINIT END -----"); } /* 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) -{ +void AP_IRLock_I2C::pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y) { + // printf("\nPIXEL TO 1M PLANE"); + 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); + // printf("\nPIXEL TO 1M PLANE END-----"); } -/* - 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; - } +void AP_IRLock_I2C::copy_frame_from_parser() { + // printf("\nCOPY FRAME FROM PARSER"); + for (size_t i=0; i<10; i++) { + pixy_parser::pixy_blob blob; + if (!pixyObj.read_buffer(i, blob)) { + break; + } - // record sensor successfully responded to I2C request - _last_read_ms = AP_HAL::millis(); + int16_t corner1_pix_x = blob.center_x - blob.width/2; // 3. CONVERT frame + int16_t corner1_pix_y = blob.center_y - blob.height/2; + int16_t corner2_pix_x = blob.center_x + blob.width/2; + int16_t corner2_pix_y = blob.center_y + blob.height/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); - /* 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; -} + _target_info[i].pos_x = 0.5f*(corner1_pos_x+corner2_pos_x); // 4. copy frame data from pixy parser to target info array + _target_info[i].pos_y = 0.5f*(corner1_pos_y+corner2_pos_y); + _target_info[i].size_x = corner2_pos_x-corner1_pos_x; + _target_info[i].size_x = corner2_pos_y-corner1_pos_y; -void AP_IRLock_I2C::read_frames(void) -{ - if (!sync_frame_start()) { - return; - } - struct frame irframe; - - if (!read_block(irframe)) { - return; + _num_targets = i+1; +// printf("\n Num_targets in the loop: %u", _num_targets); + +// // printf("\nBLOCK:- \nX: 0x%04x - Y: 0x%04x - W: 0x%04x - H: 0x%04x\n\n\n", blob.center_x, blob.center_y, blob.width, blob.height); + // printf("\nBLOCK:- \nX: %03u - Y: %03u - W: %03u - H: %03u\n\n\n", blob.center_x, blob.center_y, blob.width, blob.height); } - 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; + _frame_timestamp = AP_HAL::millis(); // 6. update frame_timestamp_us +// printf("\nFrame Timestamp Init: %u", _frame_timestamp); - 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); + // printf("\nCOPY FRAME FROM PARSER END----"); - if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - /* 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.size_x = corner2_pos_x-corner1_pos_x; - _target_info.size_y = corner2_pos_y-corner1_pos_y; - sem->give(); - } +} + +void AP_IRLock_I2C::read_frames(void) { + // printf("\nREAD FRAMES"); + uint8_t buf[16]; + dev->transfer(nullptr, 0, buf, 16); + // printf("\nUPDATE CALL AFTER"); -#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, - _target_info.size_x, _target_info.size_y); + for (size_t i=0; i<16; i++) { + if (pixyObj.recv_byte_pixy(buf[i])) { + // printf("\nREAD FRAMES BEFORE SEMAPHORE"); + + if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { // 2. take semaphore + copy_frame_from_parser(); + sem->give(); // 7. give semaphore + // printf("\nGIVE SEMAPHORE"); + + } + } } -#endif + // printf("\nREAD FRAMES END -----"); + +// // printf("\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&------------------OUT"); +// // printf("\nBLOCK:- \nX: 0x%04x - Y: 0x%04x - W: 0x%04x - H: 0x%04x\n\n\n", blob.center_yr_x, blob.center_y, blob.width, blob.height); + } // retrieve latest sensor data - returns true if new data is available -bool AP_IRLock_I2C::update() -{ +bool AP_IRLock_I2C::update() { + // printf("\nUPDATE"); bool new_data = false; if (!dev || !sem) { - return false; + return false; } if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - if (_last_update_ms != _target_info.timestamp) { + if (_last_update_ms != _frame_timestamp) { new_data = true; } - _last_update_ms = _target_info.timestamp; - _flags.healthy = (AP_HAL::millis() - _last_read_ms < 100); + _last_update_ms = _frame_timestamp; + _flags.healthy = (AP_HAL::millis() - _last_update_ms < 100); +// printf("\nHEALTHY COMPARE STATUS: %u", _flags.healthy); +// printf("\nHEALTHY COMPARISON (AP_HAL::millis(): %u - _last_read_ms: %u < 100)", AP_HAL::millis(), _last_update_ms); sem->give(); } // return true if new data found + // printf("\nUPDATE END ----"); return new_data; -} +} \ No newline at end of file diff --git a/libraries/AP_IRLock/AP_IRLock_I2C.h b/libraries/AP_IRLock/AP_IRLock_I2C.h index 862274beb3..8c42433b4f 100644 --- a/libraries/AP_IRLock/AP_IRLock_I2C.h +++ b/libraries/AP_IRLock/AP_IRLock_I2C.h @@ -5,6 +5,7 @@ #pragma once #include "IRLock.h" +#include "pixy_parser.h" class AP_IRLock_I2C : public IRLock { @@ -15,26 +16,15 @@ class AP_IRLock_I2C : public IRLock // retrieve latest sensor data - returns true if new data is available bool update() override; -private: - AP_HAL::OwnPtr dev; - - struct PACKED frame { - uint16_t checksum; - uint16_t signature; - uint16_t pixel_x; - uint16_t pixel_y; - uint16_t pixel_size_x; - uint16_t pixel_size_y; - }; + pixy_parser pixyObj; - bool timer(void); +private: + AP_HAL::OwnPtr dev; - bool sync_frame_start(void); - bool read_block(struct frame &irframe); void read_frames(void); - + void copy_frame_from_parser(void); void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y); AP_HAL::Semaphore *sem; uint32_t _last_read_ms; -}; +}; \ No newline at end of file diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp index 5601347e2a..c6d6f31cca 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.cpp +++ b/libraries/AP_IRLock/AP_IRLock_SITL.cpp @@ -69,11 +69,11 @@ bool AP_IRLock_SITL::update() if (s == sizeof(irlock_packet) && pkt.timestamp > _last_timestamp) { // fprintf(stderr, " posx %f posy %f sizex %f sizey %f\n", pkt.pos_x, pkt.pos_y, pkt.size_x, pkt.size_y); - _target_info.timestamp = pkt.timestamp; - _target_info.pos_x = pkt.pos_x; - _target_info.pos_y = pkt.pos_y; - _target_info.size_x = pkt.size_x; - _target_info.size_y = pkt.size_y; + _target_info[0].timestamp = pkt.timestamp; + _target_info[0].pos_x = pkt.pos_x; + _target_info[0].pos_y = pkt.pos_y; + _target_info[0].size_x = pkt.size_x; + _target_info[0].size_y = pkt.size_y; _last_timestamp = pkt.timestamp; _last_update_ms = _last_timestamp; new_data = true; diff --git a/libraries/AP_IRLock/IRLock.cpp b/libraries/AP_IRLock/IRLock.cpp index 7bdddc15d3..6276539832 100644 --- a/libraries/AP_IRLock/IRLock.cpp +++ b/libraries/AP_IRLock/IRLock.cpp @@ -1,3 +1,4 @@ + /* * IRLock.cpp * @@ -17,8 +18,8 @@ bool IRLock::get_angle_to_target_rad(float &x_angle_rad, float &y_angle_rad) con } // use data from first (largest) object - x_angle_rad = atanf(_target_info.pos_x); - y_angle_rad = atanf(_target_info.pos_y); + x_angle_rad = atanf(_target_info[0].pos_x); + y_angle_rad = atanf(_target_info[0].pos_y); return true; } @@ -31,9 +32,29 @@ bool IRLock::get_unit_vector_body(Vector3f& ret) const return false; } + //Here there shold be aa check that cmpares the timestamp at which the previous velue of count was plural + //And then if even after 500 millisec timestamp it shows plural count so return false + +/* + if (current_timestamp >= prev_multiple_count_timestamp + 500ms) { + if (count > 1) { + return false; + } + } + + //REAL CODE: + if (!AP_HAL::millis() >= get_multiple_count_start_timestamp() + 500) { + if (readbuf.count > 1) { + return false; + } + } + + + ALSO CODE WRITTEN IN PIXY_PARSER.CPP : RECV_BYTE_PIXY() +*/ // use data from first (largest) object - ret.x = -_target_info.pos_y; - ret.y = _target_info.pos_x; + ret.x = -_target_info[0].pos_y; + ret.y = _target_info[0].pos_x; ret.z = 1.0f; ret /= ret.length(); return true; diff --git a/libraries/AP_IRLock/IRLock.h b/libraries/AP_IRLock/IRLock.h index 6d888b2568..42415937cd 100644 --- a/libraries/AP_IRLock/IRLock.h +++ b/libraries/AP_IRLock/IRLock.h @@ -1,3 +1,4 @@ + /* 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 @@ -37,7 +38,7 @@ class IRLock uint32_t last_update_ms() const { return _last_update_ms; } // returns the number of blocks in the current frame - size_t num_targets() const { return _flags.healthy?1:0; } + size_t num_targets() const { return _flags.healthy?_num_targets:0; } // retrieve latest sensor data - returns true if new data is available virtual bool update() = 0; @@ -58,15 +59,17 @@ class IRLock // internals uint32_t _last_update_ms; + uint32_t _frame_timestamp; // milliseconds since system start // irlock_target_info is a duplicate of the PX4Firmware irlock_s structure typedef struct { - uint32_t timestamp; // milliseconds since system start float pos_x; // x-axis distance from center of image to center of target in units of tan(theta) float pos_y; // y-axis distance from center of image to center of target in units of tan(theta) float size_x; // size of target along x-axis in units of tan(theta) float size_y; // size of target along y-axis in units of tan(theta) } irlock_target_info; - irlock_target_info _target_info; + irlock_target_info _target_info[10]; + size_t _num_targets; + }; diff --git a/libraries/AP_IRLock/pixy_parser.cpp b/libraries/AP_IRLock/pixy_parser.cpp new file mode 100644 index 0000000000..12a8f9ec4f --- /dev/null +++ b/libraries/AP_IRLock/pixy_parser.cpp @@ -0,0 +1,272 @@ + +/* + 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 . + */ + +/* + * pixy_parser.cpp + * * + */ + +#ifdef PIXY_PARSER_USE_ASSERT +#define PIXY_PARSER_ASSERT(x) {assert(x);} +#else +#define PIXY_PARSER_ASSERT(x) {} +#endif + +#include "pixy_parser.h" + +#include +#include +#include +#include +#include +#include +#include +#include + + +// Constructor - pixy_parser +pixy_parser::pixy_parser() { + pixy_len = 0; + blob_buffer_write_idx = 0; + blob_buffer[blob_buffer_write_idx].count = 0; +} + +// Method - empty_pixyBuf +void pixy_parser::empty_pixyBuf() { + for (size_t i=0; i= PIXY_PARSER_MAX_BLOBS) { + return false; + } + writebuf.blobs[writebuf.count] = received_blob; + // printf("SEE THIS---- [%u, %u, %u, %u], ", (unsigned)received_blob.center_x, (unsigned)received_blob.center_y, (unsigned)received_blob.width, (unsigned)received_blob.height); + writebuf.count++; +// print_buffer(); + return true; +} + +// Method - swap_buffer +// Blob buffer indexing swap +void pixy_parser::swap_buffer() { + blob_buffer_write_idx = (blob_buffer_write_idx+1)%2; + // printf("Swapping Main Buffer: Swapped to %u\n", blob_buffer_write_idx); +// struct blob_buffer* writebuf = &blob_buffer[blob_buffer_write_idx]; + struct blob_buffer& writebuf = blob_buffer[blob_buffer_write_idx]; + writebuf.count = 0; //This is probably wrong. But helps me get rid of an error!!!----------------------------------------------------------- +// print_buffer(); +} + +// Method - read_buffer +// - read blob i: + bool pixy_parser::read_buffer(size_t i, pixy_parser::pixy_blob& ret) { + // printf("Reading Main Buffer: Reading from %u\n", (blob_buffer_write_idx+1)%2); +// struct blob_buffer* readbuf = &blob_buffer[(blob_buffer_write_idx+1)%2]; + struct blob_buffer& readbuf = blob_buffer[(blob_buffer_write_idx+1)%2]; + if ( i >= PIXY_PARSER_MAX_BLOBS || i >= readbuf.count) { + return false; + } + ret = readbuf.blobs[i]; + return true; +} + +// Method - check_pixy_message +// Checks Validity of a inpyt byte stream +enum pixy_parser::message_validity_t pixy_parser::check_pixy_message() { +// // printf("INSIDE CHECK_PIXY_MSG:%u\n", (unsigned)pixy_len); + + if (pixy_len == 0) { + return MESSAGE_EMPTY; + } + if (pixy_len >= 1 && pixy_buf[0] != 0x55) { //checking if 1st byte of the message is 0x55 + // printf("First block not 0x55 .. Message Invalid!\n"); + return MESSAGE_INVALID; + } + if (pixy_len >= 2 && pixy_buf[1] != 0xAA) { //checking if 2nd byte of the message is 0xAA (Combined it verifies if the message is 0x55AA) + // printf("Second block not 0xAA .. Message Invalid!\n"); + return MESSAGE_INVALID; + } + if (pixy_len < 14) { // Looks for at least 14 bytes i.e. "sync+blob_data" = 2+12 bytes) + // printf("Message Length less than 14.. Message Incomplete!\n"); + return MESSAGE_INCOMPLETE; + } + + if (pixy_buf[2] == 0x55 && pixy_buf[3] == 0xAA) { + + // printf("2 syncs available and size >= 14..\n"); + + if (pixy_len >= 16) { + + // printf("PixyLen Greater than or equal to 16 with 2 syncs, calculating crc....\n"); + /* check crc */ + uint16_t crc_calculated = 0; //addition of all the message fields from 5 to 15 + for (size_t i = 6; i <= 15; i+=2) { + uint16_t word = pixy_buf[i] | (uint16_t)pixy_buf[i+1]<<8; + crc_calculated += word; + } + + uint16_t crc_provided; //Getting true crc by concatinating 4 & 5 message fields(original crc) into a single 16 bit + crc_provided = pixy_buf[4] | (uint16_t)pixy_buf[5]<<8; + if (crc_provided != crc_calculated) { + // printf("CRC Failed Message Invalid!\n"); + return MESSAGE_INVALID; + } + return MESSAGE_VALID_SOF; + } else { + return MESSAGE_INCOMPLETE; + } + + + } else { + // printf("Only 1 SYNC available and size < 16, calculating CRC....\n"); + + uint16_t crc_calculated = 0; //addition of all the message fields from 5 to 15 + for (size_t i = 4; i <= 13; i+=2) { + uint16_t word = pixy_buf[i] | (uint16_t)pixy_buf[i+1]<<8; + crc_calculated += word; + } + uint16_t crc_provided; //Getting true crc by concatinating 4 & 5 message fields(original crc) into a single 16 bit + crc_provided = pixy_buf[2] | (uint16_t)pixy_buf[3]<<8; + // printf("Crc calculated: %u Crc provided: %u\n", (unsigned)crc_calculated, (unsigned)crc_provided); + + if (crc_provided != crc_calculated) { + // printf("CRC Failed Message Invalid!\n"); + return MESSAGE_INVALID; + } + return MESSAGE_VALID_BLOCK; + } +} + +// Method - recv_byte_pixy +// Logicbase of Pixy_parser. Receives the byte from here and adds into pixyBuf if valid. +bool pixy_parser::recv_byte_pixy(uint8_t byte) { +// // printf("INSIDE recv_byte_pixy:-\n"); +// Read 2 bytes +// printf("\n---- IF_SWAP BEFORE: %d", if_swap_buffer); + bool if_swap = 0; + struct blob_buffer& writebuf = blob_buffer[blob_buffer_write_idx]; + + + // printf("The byte inputted is: %u and pixylen is %u\n", byte, (unsigned)pixy_len); + + PIXY_PARSER_ASSERT(pixy_len < PIXY_PARSER_PIXY_BUF_SIZE); + if (pixy_len >= PIXY_PARSER_PIXY_BUF_SIZE) { + // This should never happen. + empty_pixyBuf(); + } + + pixy_buf[pixy_len++] = byte; // Append byte to buffer + + enum message_validity_t validity = check_pixy_message(); // Call parser + // printf("Validity Check Call: %d (0:empty 1:invalid 2:incomplete 3:valid_SOF 4:valid_block)\n", validity); + // printf("Pixy Len: %u\n", (unsigned)pixy_len); + // printf("Pixy_buf: "); +// for (size_t i=0; i 1) { + if (!AP_HAL::millis() <= multiple_count_start_timestamp + 500) { + multiple_count_start_timestamp = AP_HAL::millis(); + } + } + +*/ + + + if (validity == MESSAGE_VALID_SOF) { + if (!(writebuf.count >= PIXY_PARSER_MAX_BLOBS)) { + pixy_blob received_blob; + received_blob.center_x = pixy_buf[8] | (uint16_t)pixy_buf[9]<<8; //Extract blob information + received_blob.center_y = pixy_buf[10] | (uint16_t)pixy_buf[11]<<8; + received_blob.width = pixy_buf[12] | (uint16_t)pixy_buf[13]<<8; + received_blob.height = pixy_buf[14] | (uint16_t)pixy_buf[15]<<8; + + if (writebuf.count != 0) { + swap_buffer(); //swap buffer +// if_swap_buffer = 1; + if_swap = 1; + + writebuf.count = 0; //Turn the count to 0 + // printf("@@@SEE THIS---- [%u, %u, %u, %u], ", (unsigned)received_blob.center_x, (unsigned)received_blob.center_y, (unsigned)received_blob.width, (unsigned)received_blob.height); + + write_buffer(received_blob); //Writing inside the buffer(the count increment happens inside the write_buffer()) +// return true; + } + else { + write_buffer(received_blob); //Writing inside the buffer(the count increment happens inside the write_buffer()) + } + } + empty_pixyBuf(); + } + + if (validity == MESSAGE_VALID_BLOCK) { ///-----------------------------how can I store the block inside a frame?----------------- + if (!(writebuf.count >= PIXY_PARSER_MAX_BLOBS)) { + pixy_blob received_blob; + received_blob.center_x = pixy_buf[6] | (uint16_t)pixy_buf[7]<<8; //Extract blob information + received_blob.center_y = pixy_buf[8] | (uint16_t)pixy_buf[9]<<8; + received_blob.width = pixy_buf[10] | (uint16_t)pixy_buf[11]<<8; + received_blob.height = pixy_buf[12] | (uint16_t)pixy_buf[13]<<8; + // printf("@@@SEE THIS---- [%u, %u, %u, %u], ", (unsigned)received_blob.center_x, (unsigned)received_blob.center_y, (unsigned)received_blob.width, (unsigned)received_blob.height); + + write_buffer(received_blob); //Writing inside the buffer + } + empty_pixyBuf(); //Emptying The pixy_buf once read the SOF into 'writebuf' + } + + if (validity == MESSAGE_INVALID) { // If message invalid, wait and read 2 bytes + pixy_len--; ///----------------------------------------QUES---------------------------------------------- + memmove(pixy_buf, pixy_buf+1, pixy_len); // This discards the first block of the pixy_buf and makes pixy_buf have everything except the fisrt block value + validity = check_pixy_message(); + + if (writebuf.count < PIXY_PARSER_MAX_BLOBS && writebuf.count != 0) { + swap_buffer(); //swap buffer +// if_swap_buffer = 1; + if_swap = 1; +// return true; + } + } + return if_swap; +} diff --git a/libraries/AP_IRLock/pixy_parser.h b/libraries/AP_IRLock/pixy_parser.h new file mode 100644 index 0000000000..a846958718 --- /dev/null +++ b/libraries/AP_IRLock/pixy_parser.h @@ -0,0 +1,71 @@ + +/* + * pixy_parser.h + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#define PIXY_PARSER_PIXY_BUF_SIZE 17 +#define PIXY_PARSER_MAX_BLOBS 10 + +class pixy_parser { + public: + typedef struct { + uint16_t center_x; + uint16_t center_y; + uint16_t width; + uint16_t height; + } pixy_blob; + + pixy_parser(); + void empty_pixyBuf(void); + void print_buffer(void); + void swap_buffer(void); + bool recv_byte_pixy(uint8_t byte); + bool read_buffer(size_t i, pixy_blob& ret); + private: + + struct blob_buffer { //Frame (full of blobs) + pixy_blob blobs[PIXY_PARSER_MAX_BLOBS]; + size_t count; + } blob_buffer[2]; + + + enum message_validity_t { + MESSAGE_EMPTY, + MESSAGE_INVALID, + MESSAGE_INCOMPLETE, + MESSAGE_VALID_SOF, + MESSAGE_VALID_BLOCK + }; + + bool write_buffer(const pixy_blob& blob1); + enum message_validity_t check_pixy_message(); + + uint8_t pixy_buf[PIXY_PARSER_PIXY_BUF_SIZE]; + size_t pixy_len; + uint8_t blob_buffer_write_idx; + + +// uint32_t multiple_count_start_timestamp; +}; + + + + + + + + + + +