@ -123,6 +123,8 @@ static void camera_init(CameraState *s, int camera_id, int camera_num,
s - > max_gain = max_gain ; s - > max_gain = max_gain ;
s - > fps = fps ; s - > fps = fps ;
s - > self_recover = 0 ;
zsock_t * ops_sock = zsock_new_push ( " >inproc://cameraops " ) ; zsock_t * ops_sock = zsock_new_push ( " >inproc://cameraops " ) ;
assert ( ops_sock ) ; assert ( ops_sock ) ;
s - > ops_sock = zsock_resolve ( ops_sock ) ; s - > ops_sock = zsock_resolve ( ops_sock ) ;
@ -1745,8 +1747,13 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
avg_focus + = s - > focus [ i ] ; avg_focus + = s - > focus [ i ] ;
} }
} }
// self recover override
if ( s - > self_recover > 1 ) {
s - > focus_err = 200 * ( ( s - > self_recover % 2 = = 0 ) ? 1 : - 1 ) ; // far for even numbers, close for odd
s - > self_recover - = 2 ;
return ;
}
//printf("\n");
if ( good_count < 4 ) { if ( good_count < 4 ) {
s - > focus_err = nan ( " " ) ; s - > focus_err = nan ( " " ) ;
return ; return ;
@ -1770,8 +1777,8 @@ static void do_autofocus(CameraState *s) {
float err = s - > focus_err ; float err = s - > focus_err ;
float sag = ( s - > last_sag_acc_z / 9.8 ) * 128 ; float sag = ( s - > last_sag_acc_z / 9.8 ) * 128 ;
const int dac_up = s - > device = = DEVICE_LP3 ? 634 : 456 ; const int dac_up = s - > device = = DEVICE_LP3 ? LP3_AF_DAC_UP : OP3T_AF_DAC_UP ;
const int dac_down = s - > device = = DEVICE_LP3 ? 366 : 224 ; const int dac_down = s - > device = = DEVICE_LP3 ? LP3_AF_DAC_DOWN : OP3T_AF_DAC_DOWN ;
if ( ! isnan ( err ) ) { if ( ! isnan ( err ) ) {
// learn lens_true_pos
// learn lens_true_pos
@ -1980,43 +1987,6 @@ static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) {
} ; } ;
} }
static bool acceleration_from_sensor_sock ( void * sock , float * vs ) {
int err ;
bool ret = false ;
zmq_msg_t msg ;
err = zmq_msg_init ( & msg ) ;
assert ( err = = 0 ) ;
err = zmq_msg_recv ( & msg , sock , 0 ) ;
assert ( err > = 0 ) ;
void * data = zmq_msg_data ( & msg ) ;
size_t size = zmq_msg_size ( & msg ) ;
auto amsg = kj : : heapArray < capnp : : word > ( size / sizeof ( capnp : : word ) + 1 ) ;
memcpy ( amsg . begin ( ) , data , size ) ;
capnp : : FlatArrayMessageReader cmsg ( amsg ) ;
auto event = cmsg . getRoot < cereal : : Event > ( ) ;
if ( event . which ( ) = = cereal : : Event : : SENSOR_EVENTS ) {
auto sensor_events = event . getSensorEvents ( ) ;
for ( auto sensor_event : sensor_events ) {
if ( sensor_event . which ( ) = = cereal : : SensorEventData : : ACCELERATION ) {
auto v = sensor_event . getAcceleration ( ) . getV ( ) ;
if ( v . size ( ) < 3 ) {
continue ; //wtf
}
for ( int j = 0 ; j < 3 ; j + + ) {
vs [ j ] = v [ j ] ;
}
ret = true ;
break ;
}
}
}
zmq_msg_close ( & msg ) ;
return ret ;
}
static void ops_term ( ) { static void ops_term ( ) {
zsock_t * ops_sock = zsock_new_push ( " >inproc://cameraops " ) ; zsock_t * ops_sock = zsock_new_push ( " >inproc://cameraops " ) ;
assert ( ops_sock ) ; assert ( ops_sock ) ;
@ -2036,21 +2006,20 @@ static void* ops_thread(void* arg) {
zsock_t * cameraops = zsock_new_pull ( " @inproc://cameraops " ) ; zsock_t * cameraops = zsock_new_pull ( " @inproc://cameraops " ) ;
assert ( cameraops ) ; assert ( cameraops ) ;
zsock_t * sensor_sock = zsock_new_sub ( " >tcp://127.0.0.1:8003 " , " " ) ;
assert ( sensor_sock ) ;
zsock_t * terminate = zsock_new_sub ( " >inproc://terminate " , " " ) ; zsock_t * terminate = zsock_new_sub ( " >inproc://terminate " , " " ) ;
assert ( terminate ) ; assert ( terminate ) ;
zpoller_t * poller = zpoller_new ( cameraops , sensor_sock , terminate , NULL ) ; zpoller_t * poller = zpoller_new ( cameraops , terminate , NULL ) ;
assert ( poller ) ; assert ( poller ) ;
while ( ! do_exit ) { SubMaster sm ( { " sensorEvents " } ) ; // msgq submaster
while ( ! do_exit ) {
// zmq handling
zsock_t * which = ( zsock_t * ) zpoller_wait ( poller , - 1 ) ; zsock_t * which = ( zsock_t * ) zpoller_wait ( poller , - 1 ) ;
if ( which = = terminate | | which = = NULL ) { if ( which = = terminate ) {
break ; break ;
} } else if ( which ! = NULL ) {
void * sockraw = zsock_resolve ( which ) ; void * sockraw = zsock_resolve ( which ) ;
if ( which = = cameraops ) { if ( which = = cameraops ) {
@ -2080,22 +2049,38 @@ static void* ops_thread(void* arg) {
} }
zmq_msg_close ( & msg ) ; zmq_msg_close ( & msg ) ;
}
}
// msgq handling
if ( sm . update ( 0 ) > 0 ) {
float vals [ 3 ] = { 0.0 } ;
bool got_accel = false ;
} else if ( which = = sensor_sock ) { auto sensor_events = sm [ " sensorEvents " ] . getSensorEvents ( ) ;
float vs [ 3 ] = { 0.0 } ; for ( auto sensor_event : sensor_events ) {
bool got_accel = acceleration_from_sensor_sock ( sockraw , vs ) ; if ( sensor_event . which ( ) = = cereal : : SensorEventData : : ACCELERATION ) {
auto v = sensor_event . getAcceleration ( ) . getV ( ) ;
if ( v . size ( ) < 3 ) {
continue ; //wtf
}
for ( int j = 0 ; j < 3 ; j + + ) {
vals [ j ] = v [ j ] ;
}
got_accel = true ;
break ;
}
}
uint64_t ts = nanos_since_boot ( ) ; uint64_t ts = nanos_since_boot ( ) ;
if ( got_accel & & ts - s - > rear . last_sag_ts > 10000000 ) { // 10 ms
if ( got_accel & & ts - s - > rear . last_sag_ts > 10000000 ) { // 10 ms
s - > rear . last_sag_ts = ts ; s - > rear . last_sag_ts = ts ;
s - > rear . last_sag_acc_z = - vs [ 2 ] ; s - > rear . last_sag_acc_z = - val s [ 2 ] ;
} }
} }
} }
zpoller_destroy ( & poller ) ; zpoller_destroy ( & poller ) ;
zsock_destroy ( & cameraops ) ; zsock_destroy ( & cameraops ) ;
zsock_destroy ( & sensor_sock ) ;
zsock_destroy ( & terminate ) ; zsock_destroy ( & terminate ) ;
return NULL ; return NULL ;
ZwX1616
5 years ago
committed by
GitHub