Skip to content

Commit

Permalink
Added camera id to ABI message
Browse files Browse the repository at this point in the history
  • Loading branch information
@guidoAI @dewagter
guidoAI authored and dewagter committed Jun 30, 2022
1 parent 644da66 commit 17cd0cf
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 12 deletions.
1 change: 1 addition & 0 deletions conf/abi.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@
<field name="flow_der_y" type="int32_t">The derotated flow calculation in the y direction (in subpixels)</field>
<field name="quality" type="float"/>
<field name="size_divergence" type="float">Divergence as determined with the size method (in 1/seconds) with LK, and Divergence (1/seconds) itself with EF</field>
<field name="camera_id" type="uint8_t">Camera ID (bottom or front)</field>
</message>

<message name="VELOCITY_ESTIMATE" id="12">
Expand Down
3 changes: 2 additions & 1 deletion sw/airborne/modules/computer_vision/opticflow_module.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,7 +150,8 @@ void opticflow_module_run(void)
opticflow_result[idx_camera].flow_der_x,
opticflow_result[idx_camera].flow_der_y,
opticflow_result[idx_camera].noise_measurement,
opticflow_result[idx_camera].div_size);
opticflow_result[idx_camera].div_size,
(uint8_t) idx_camera);
//TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result[idx_camera].noise_measurement < 0.8) {
AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID + idx_camera, now_ts,
Expand Down
19 changes: 12 additions & 7 deletions sw/airborne/modules/ctrl/optical_flow_landing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,7 +271,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev)
void vertical_ctrl_agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
// Callback function of the optical flow estimate:
void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);

// common functions for different landing strategies:
static void set_cov_div(int32_t thrust);
Expand Down Expand Up @@ -1107,14 +1107,19 @@ void vertical_ctrl_agl_cb(uint8_t sender_id UNUSED, __attribute__((unused)) uint

void vertical_ctrl_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp,
int32_t flow_x UNUSED, int32_t flow_y UNUSED,
int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED,
float quality UNUSED, float size_divergence,
uint8_t camera_id)
{

optical_flow_x = ((float)flow_der_x)/10.0;
optical_flow_y = ((float)flow_der_y)/10.0;
divergence_vision = size_divergence;
//printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
vision_time = ((float)stamp) / 1e6;
if(camera_id == 0)
{
optical_flow_x = ((float)flow_der_x)/10.0;
optical_flow_y = ((float)flow_der_y)/10.0;
divergence_vision = size_divergence;
//printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision);
vision_time = ((float)stamp) / 1e6;
}
}

////////////////////////////////////////////////////////////////////
Expand Down
12 changes: 8 additions & 4 deletions sw/airborne/modules/loggers/file_logger.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,12 +91,13 @@ int16_t OF_flow_der_x;
int16_t OF_flow_der_y;
float OF_quality;
float OF_size_divergence;
uint8_t OF_camera_id;
static abi_event OF_ev; ///< The sonar ABI event
static void logger_optical_flow_cb(uint8_t sender_id __attribute__((unused)),uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);

static void logger_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id)
{
OF_previous_stamp = OF_stamp;
OF_stamp = stamp;
Expand All @@ -106,6 +107,7 @@ static void logger_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t fl
OF_flow_der_y = flow_der_y;
OF_quality = quality;
OF_size_divergence = size_divergence;
OF_camera_id = camera_id;
}
uint32_t OF_stamp2=0;
uint32_t OF_previous_stamp2=0;
Expand All @@ -115,12 +117,13 @@ int16_t OF_flow_der_x2;
int16_t OF_flow_der_y2;
float OF_quality2;
float OF_size_divergence2;
uint8_t OF_camera_id2;
static abi_event OF_ev2; ///< The sonar ABI event
static void logger_optical_flow_cb2(uint8_t sender_id __attribute__((unused)),uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id);

static void logger_optical_flow_cb2(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence, uint8_t camera_id)
{
OF_previous_stamp2 = OF_stamp;
OF_stamp2 = stamp;
Expand All @@ -130,6 +133,7 @@ static void logger_optical_flow_cb2(uint8_t sender_id, uint32_t stamp, int32_t f
OF_flow_der_y2 = flow_der_y;
OF_quality2 = quality;
OF_size_divergence2 = size_divergence;
OF_camera_id2 = camera_id;
}

// reading RPMs:
Expand Down

0 comments on commit 17cd0cf

Please sign in to comment.