|
|
@ -168,7 +168,7 @@ void OpticalFlow::update(void) |
|
|
|
|
|
|
|
|
|
|
|
// only healthy if the data is less than 0.5s old
|
|
|
|
// only healthy if the data is less than 0.5s old
|
|
|
|
// _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
|
|
|
|
// _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
|
|
|
|
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)&&(_state.surface_quality < _threshold); |
|
|
|
_flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)&&(_state.surface_quality > _threshold); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void OpticalFlow::handle_msg(const mavlink_message_t &msg) |
|
|
|
void OpticalFlow::handle_msg(const mavlink_message_t &msg) |
|
|
@ -182,17 +182,12 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg) |
|
|
|
backend->handle_msg(msg); |
|
|
|
backend->handle_msg(msg); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
#include <GCS_MAVLink/GCS.h> |
|
|
|
|
|
|
|
void OpticalFlow::update_state(const OpticalFlow_state &state) |
|
|
|
void OpticalFlow::update_state(const OpticalFlow_state &state) |
|
|
|
{ |
|
|
|
{ |
|
|
|
_state = state; |
|
|
|
_state = state; |
|
|
|
_last_update_ms = AP_HAL::millis(); |
|
|
|
_last_update_ms = AP_HAL::millis(); |
|
|
|
|
|
|
|
|
|
|
|
// static uint8_t last_quaty;
|
|
|
|
|
|
|
|
// if(abs(last_quaty - _state.surface_quality)> 30){
|
|
|
|
|
|
|
|
// last_quaty = _state.surface_quality;
|
|
|
|
|
|
|
|
// gcs().send_text(MAV_SEVERITY_INFO,"thr:%d,q:%d",_threshold,_state.surface_quality);
|
|
|
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
if(_state.surface_quality < _threshold ) |
|
|
|
if(_state.surface_quality < _threshold ) |
|
|
|
{ |
|
|
|
{ |
|
|
|
_state.surface_quality = 0; |
|
|
|
_state.surface_quality = 0; |
|
|
|