| 
						
						
							
								
							
						
						
					 | 
					 | 
					@ -10,12 +10,12 @@ | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <cassert> | 
					 | 
					 | 
					 | 
					#include <cassert> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <cerrno> | 
					 | 
					 | 
					 | 
					#include <cerrno> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <chrono> | 
					 | 
					 | 
					 | 
					#include <chrono> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					 | 
					#include <cmath> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <cstdint> | 
					 | 
					 | 
					 | 
					#include <cstdint> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <cstdio> | 
					 | 
					 | 
					 | 
					#include <cstdio> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <cstdlib> | 
					 | 
					 | 
					 | 
					#include <cstdlib> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <future> | 
					 | 
					 | 
					 | 
					#include <future> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <thread> | 
					 | 
					 | 
					 | 
					#include <thread> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <unordered_map> | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include <libusb-1.0/libusb.h> | 
					 | 
					 | 
					 | 
					#include <libusb-1.0/libusb.h> | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
	
		
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
					 | 
					@ -26,7 +26,6 @@ | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/common/timing.h" | 
					 | 
					 | 
					 | 
					#include "selfdrive/common/timing.h" | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/common/util.h" | 
					 | 
					 | 
					 | 
					#include "selfdrive/common/util.h" | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/hardware/hw.h" | 
					 | 
					 | 
					 | 
					#include "selfdrive/hardware/hw.h" | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/locationd/ublox_msg.h" | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/boardd/panda.h" | 
					 | 
					 | 
					 | 
					#include "selfdrive/boardd/panda.h" | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					#include "selfdrive/boardd/pigeon.h" | 
					 | 
					 | 
					 | 
					#include "selfdrive/boardd/pigeon.h" | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
							
								
							
						
						
					 | 
					 | 
					@ -552,38 +551,11 @@ void pigeon_thread(Panda *panda) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  std::unique_ptr<Pigeon> pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); | 
					 | 
					 | 
					 | 
					  std::unique_ptr<Pigeon> pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  std::unordered_map<char, uint64_t> last_recv_time; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  std::unordered_map<char, int64_t> cls_max_dt = { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    {(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    {(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  }; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					  while (!do_exit && panda->connected) { | 
					 | 
					 | 
					 | 
					  while (!do_exit && panda->connected) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    bool need_reset = false; | 
					 | 
					 | 
					 | 
					    bool need_reset = false; | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    bool ignition_local = ignition; | 
					 | 
					 | 
					 | 
					    bool ignition_local = ignition; | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    std::string recv = pigeon->receive(); | 
					 | 
					 | 
					 | 
					    std::string recv = pigeon->receive(); | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    // Parse message header
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    if (ignition_local && recv.length() >= 3) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        const char msg_cls = recv[2]; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        uint64_t t = nanos_since_boot(); | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        if (t > last_recv_time[msg_cls]) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					          last_recv_time[msg_cls] = t; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    // Check based on message frequency
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    for (const auto& [msg_cls, max_dt] : cls_max_dt) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls]; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      if (ignition_last && ignition_local && dt > max_dt) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt); | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        // TODO: turn on reset after verification of logs
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        // need_reset = true;
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    // Check based on null bytes
 | 
					 | 
					 | 
					 | 
					    // Check based on null bytes
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { | 
					 | 
					 | 
					 | 
					    if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      need_reset = true; | 
					 | 
					 | 
					 | 
					      need_reset = true; | 
				
			
			
		
	
	
		
		
			
				
					| 
						
						
						
							
								
							
						
					 | 
					 | 
					@ -599,12 +571,6 @@ void pigeon_thread(Panda *panda) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    if((ignition_local && !ignition_last) || need_reset) { | 
					 | 
					 | 
					 | 
					    if((ignition_local && !ignition_last) || need_reset) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      pigeon_active = true; | 
					 | 
					 | 
					 | 
					      pigeon_active = true; | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      pigeon->init(); | 
					 | 
					 | 
					 | 
					      pigeon->init(); | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      // Set receive times to current time
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
 | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      for (const auto& [msg_cls, dt] : cls_max_dt) { | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					        last_recv_time[msg_cls] = t; | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      } | 
					 | 
					 | 
					 | 
					 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					    } else if (!ignition_local && ignition_last) { | 
					 | 
					 | 
					 | 
					    } else if (!ignition_local && ignition_last) { | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      // power off on falling edge of ignition
 | 
					 | 
					 | 
					 | 
					      // power off on falling edge of ignition
 | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					      LOGD("powering off pigeon\n"); | 
					 | 
					 | 
					 | 
					      LOGD("powering off pigeon\n"); | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
					 | 
					
  |