Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_OpticalFlow: Add initialization support for UPixels UPFLOW optical flow sensor #24187

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
+250 −1
Open

AP_OpticalFlow: Add initialization support for UPixels UPFLOW optical flow sensor #24187

Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3442822
AP_OpticalFlow: Add Upflow LC306 init
discodyer Jul 1, 2023
35f014f
Merge branch 'master' into master
discodyer Jul 1, 2023
2816eb1
AP_OpticalFlow: Remove unnecessary debug msgs
discodyer Oct 20, 2023
d87081c
AP_OpticalFlow: Remove unused header file
discodyer Oct 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
259 changes: 258 additions & 1 deletion libraries/AP_OpticalFlow/AP_OpticalFlow_UPFLOW.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
*/

#include "AP_OpticalFlow_UPFLOW.h"
#include <GCS_MAVLink/GCS.h>

#if AP_OPTICALFLOW_UPFLOW_ENABLED

Expand All @@ -49,9 +50,180 @@
#define UPFLOW_FOOTER (uint8_t)0x55
#define UPFLOW_PIXEL_SCALING (1e-4)
#define UPFLOW_TIMEOUT_SEC 0.3f
#define UPFLOW_SENSOR_IIC_ADDR (uint8_t)0xDC
#define UPFLOW_INIT_FAILED GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UPFLOW: Init Failed, Using LC302-3C");

extern const AP_HAL::HAL& hal;

// internal parameter configuration instructions
const static uint8_t upflow_internal_para[4] = {0x96, 0x26, 0xbc, 0x50};

// sensor parameter configuration
const static uint8_t upflow_sensor_cfg[]= {
//Address, Data
0x12, 0x80,
0x11, 0x30,
0x1b, 0x06,
0x6b, 0x43,
0x12, 0x20,
0x3a, 0x00,
0x15, 0x02,
0x62, 0x81,
0x08, 0xa0,
0x06, 0x68,
0x2b, 0x20,
0x92, 0x25,
0x27, 0x97,
0x17, 0x01,
0x18, 0x79,
0x19, 0x00,
0x1a, 0xa0,
0x03, 0x00,
0x13, 0x00,
0x01, 0x13,
0x02, 0x20,
0x87, 0x16,
0x8c, 0x01,
0x8d, 0xcc,
0x13, 0x07,
0x33, 0x10,
0x34, 0x1d,
0x35, 0x46,
0x36, 0x40,
0x37, 0xa4,
0x38, 0x7c,
0x65, 0x46,
0x66, 0x46,
0x6e, 0x20,
0x9b, 0xa4,
0x9c, 0x7c,
0xbc, 0x0c,
0xbd, 0xa4,
0xbe, 0x7c,
0x20, 0x09,
0x09, 0x03,
0x72, 0x2f,
0x73, 0x2f,
0x74, 0xa7,
0x75, 0x12,
0x79, 0x8d,
0x7a, 0x00,
0x7e, 0xfa,
0x70, 0x0f,
0x7c, 0x84,
0x7d, 0xba,
0x5b, 0xc2,
0x76, 0x90,
0x7b, 0x55,
0x71, 0x46,
0x77, 0xdd,
0x13, 0x0f,
0x8a, 0x10,
0x8b, 0x20,
0x8e, 0x21,
0x8f, 0x40,
0x94, 0x41,
0x95, 0x7e,
0x96, 0x7f,
0x97, 0xf3,
0x13, 0x07,
0x24, 0x58,
0x97, 0x48,
0x25, 0x08,
0x94, 0xb5,
0x95, 0xc0,
0x80, 0xf4,
0x81, 0xe0,
0x82, 0x1b,
0x83, 0x37,
0x84, 0x39,
0x85, 0x58,
0x86, 0xff,
0x89, 0x15,
0x8a, 0xb8,
0x8b, 0x99,
0x39, 0x98,
0x3f, 0x98,
0x90, 0xa0,
0x91, 0xe0,
0x40, 0x20,
0x41, 0x28,
0x42, 0x26,
0x43, 0x25,
0x44, 0x1f,
0x45, 0x1a,
0x46, 0x16,
0x47, 0x12,
0x48, 0x0f,
0x49, 0x0d,
0x4b, 0x0b,
0x4c, 0x0a,
0x4e, 0x08,
0x4f, 0x06,
0x50, 0x06,
0x5a, 0x56,
0x51, 0x1b,
0x52, 0x04,
0x53, 0x4a,
0x54, 0x26,
0x57, 0x75,
0x58, 0x2b,
0x5a, 0xd6,
0x51, 0x28,
0x52, 0x1e,
0x53, 0x9e,
0x54, 0x70,
0x57, 0x50,
0x58, 0x07,
0x5c, 0x28,
0xb0, 0xe0,
0xb1, 0xc0,
0xb2, 0xb0,
0xb3, 0x4f,
0xb4, 0x63,
0xb4, 0xe3,
0xb1, 0xf0,
0xb2, 0xa0,
0x55, 0x00,
0x56, 0x40,
0x96, 0x50,
0x9a, 0x30,
0x6a, 0x81,
0x23, 0x33,
0xa0, 0xd0,
0xa1, 0x31,
0xa6, 0x04,
0xa2, 0x0f,
0xa3, 0x2b,
0xa4, 0x0f,
0xa5, 0x2b,
0xa7, 0x9a,
0xa8, 0x1c,
0xa9, 0x11,
0xaa, 0x16,
0xab, 0x16,
0xac, 0x3c,
0xad, 0xf0,
0xae, 0x57,
0xc6, 0xaa,
0xd2, 0x78,
0xd0, 0xb4,
0xd1, 0x00,
0xc8, 0x10,
0xc9, 0x12,
0xd3, 0x09,
0xd4, 0x2a,
0xee, 0x4c,
0x7e, 0xfa,
0x74, 0xa7,
0x78, 0x4e,
0x60, 0xe7,
0x61, 0xc8,
0x6d, 0x70,
0x1e, 0x39,
0x98, 0x1a
};

// constructor
AP_OpticalFlow_UPFLOW::AP_OpticalFlow_UPFLOW(AP_OpticalFlow &_frontend, AP_HAL::UARTDriver *_uart) :
OpticalFlow_backend(_frontend),
Expand Down Expand Up @@ -87,6 +259,91 @@ void AP_OpticalFlow_UPFLOW::init()
}
// open serial port with baud rate of 19200
uart->begin(19200);

/*
LC306 or LC302-8B boards require initialization before use, but LC302-3C does not.
Initialization can divided into four parts

(0xAA)start --> (0xAB)internal parameter configuration
--> (0xBB)sensor parameter configuration (Loop) --> (0xDD)End
^--------------------------------------/

| command sent | returned status |
| ------------ | --------------- |
| 0xAA | none | 1. Start flag
| ------------ | --------------- |
| 0xAB | 0xAB | 2. internal parameter configuration flag
| 0x96 | | status code: 0x00 == success
| 0x26 | status code | 0x01 == IIC no response
| 0xbc | | 0x02 == XOR error
| 0x50 | | send: 0xAB, 0x96, 0x26, 0xbc, 0x50, 0x5c(BCC without 0xAB)
| BCC(0x5c) | 0xAB xor status | success response: 0xAB, 0x00, 0xAB
| ------------ | --------------- |
| 0xBB | 0xBB | 3. sensor parameter configuration
| 0xdc | | sending data in upflow_sensor_cfg
| address | status code | same status code above
| data | | send: 0xBB, 0xdc, 0xXX, 0xXX, 0xXX(BCC without 0xBB)
| BCC | 0xBB xor status | success response: 0xBB, 0x00, 0xBB
| ------------ | --------------- |
| 0xDD | none | 4. Stop flag

!: Step 3 will run repeatedly until the sending is complete.
Each time the module receives an 0xBB instruction, it will return a corresponding status.
*/

// uint32_t nbytes = 0;
uint8_t bcc_sum = 0;
uint8_t recv_buf[3];

//delay 100ms before initialization
hal.scheduler->delay(100);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not really a fan of delays in the init, this is called from the main thread. However, there are a few existing examples in rangefinder...

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for your feedback. I understand your concern about latency in the main thread, but this is to ensure the sensor is initialized correctly. It is necessary to wait 100 ms for the sensor to initialize after powering up. However, do you have any suggestions or a more elegant approach?


// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UPFLOW: Start initialization!");

// (0xAA)start configuration
uart->write((uint8_t)0xAA);

// (0xAB)internal parameter configuration
uart->write((uint8_t)0xAB);
for (int i = 0; i < 4; i++) {
uart->write(upflow_internal_para[i]);
bcc_sum ^= upflow_internal_para[i];
}
uart->write(bcc_sum);
hal.scheduler->delay(10);
for (int i = 0; i < 3; i++) {
recv_buf[i] = uart->read();
}
// if failed to receive response code, you can start over from 0xAA.
if ((recv_buf[0] ^ recv_buf[1]) != recv_buf[2]) {
UPFLOW_INIT_FAILED;
return;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What happens on this early return? We don't call init a second time, so does this mean the sensor never works ?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What happens on this early return? We don't call init a second time, so does this mean the sensor never works ?

The reason for early return is that if the user inserts the LC302-3C, the sensor will not return any data during the initialization operation, and there is no need to continue the initialization operation. LC302-3C and other sensor versions that do not require initialization will not be affected.

The UPFLOW_INIT_FAILED macro is what I've set during debugging to monitor whether the initialization is successful or not in the ground station.
And I have conducted multiple tests using the Pixhawk 2.4.8 flight controller. For LC306 and LC302-8B, they can be initialized without any issues.

}

// (0xBB)sensor parameter configuration
uint32_t cfg_cnt = 0;
while (cfg_cnt < sizeof(upflow_sensor_cfg) - 1) {
uart->write((uint8_t)0xBB);
uart->write(UPFLOW_SENSOR_IIC_ADDR);
uart->write(upflow_sensor_cfg[cfg_cnt]); // address
uart->write(upflow_sensor_cfg[cfg_cnt + 1]); // data
uart->write(UPFLOW_SENSOR_IIC_ADDR ^ upflow_sensor_cfg[cfg_cnt] ^ upflow_sensor_cfg[cfg_cnt + 1]);
hal.scheduler->delay(10);
for (int i = 0; i < 3; i++) {
recv_buf[i] = uart->read();
}
// if failed to receive response code, you can start over from 0xBB.
if ((recv_buf[0] ^ recv_buf[1]) != recv_buf[2]) {
UPFLOW_INIT_FAILED;
return;
}
cfg_cnt += 2;
}

// (0xDD)End configuration
uart->write((uint8_t)0xDD);

GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UPFLOW: initialized Done!");
discodyer marked this conversation as resolved.
Show resolved Hide resolved
}

// read latest values from sensor and fill in x,y and totals.
Expand Down Expand Up @@ -191,4 +448,4 @@ void AP_OpticalFlow_UPFLOW::update(void)
gyro_sum_count = 0;
}

#endif // AP_OPTICALFLOW_UPFLOW_ENABLED
#endif // AP_OPTICALFLOW_UPFLOW_ENABLED