Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: fixed dataflash logs to be useful 

added sonar and fixed other messages
This commit is contained in:
Andrew Tridgell 2013-04-19 10:23:57 +10:00
parent 1c046fa49f
commit a8d6fa3107
7 changed files with 139 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/APMrover2.pde
View File

@ -376,6 +376,8 @@ static struct {
// have we detected an obstacle? // have we detected an obstacle?
uint8_t detected_count; uint8_t detected_count;
float turn_angle; float turn_angle;
uint16_t sonar1_distance_cm;
uint16_t sonar2_distance_cm;
// time when we last detected an obstacle, in milliseconds // time when we last detected an obstacle, in milliseconds
uint32_t detected_time_ms; uint32_t detected_time_ms;
@ -428,7 +430,7 @@ static float current_total1;
// Navigation control variables // Navigation control variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// The instantaneous desired steering angle. Hundredths of a degree // The instantaneous desired steering angle. Hundredths of a degree
static int32_t nav_steer; static int32_t nav_steer_cd;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Waypoint distances // Waypoint distances
@ -636,7 +638,7 @@ static void fast_loop()
# if HIL_MODE == HIL_MODE_DISABLED # if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_IMU) if (g.log_bitmask & MASK_LOG_IMU)
Log_Write_IMU(); Log_Write_IMU();
@ -707,7 +709,7 @@ static void medium_loop()
medium_loopCounter++; medium_loopCounter++;
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
@ -717,7 +719,7 @@ static void medium_loop()
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS) if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); Log_Write_GPS();
break; break;
// This case controls the slow loop // This case controls the slow loop

2
APMrover2/GCS_Mavlink.pde
View File

@ -240,7 +240,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
int16_t bearing = nav_bearing / 100; int16_t bearing = nav_bearing / 100;
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_steer / 1.0e2, nav_steer_cd / 1.0e2,
0, 0,
bearing, bearing,
target_bearing / 100, target_bearing / 100,

137
APMrover2/Log.pde
View File

@ -48,6 +48,7 @@ print_log_menu(void)
PLOG(IMU); PLOG(IMU);
PLOG(CMD); PLOG(CMD);
PLOG(CURRENT); PLOG(CURRENT);
PLOG(SONAR);
#undef PLOG #undef PLOG
} }
@ -135,6 +136,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(IMU); TARG(IMU);
TARG(CMD); TARG(CMD);
TARG(CURRENT); TARG(CURRENT);
TARG(SONAR);
#undef TARG #undef TARG
} }
@ -182,13 +184,13 @@ struct log_Attitute {
}; };
// Write an attitude packet. Total length : 10 bytes // Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) static void Log_Write_Attitude()
{ {
struct log_Attitute pkt = { struct log_Attitute pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
roll : log_roll, roll : (int16_t)ahrs.roll_sensor,
pitch : log_pitch, pitch : (int16_t)ahrs.pitch_sensor,
yaw : log_yaw yaw : (uint16_t)ahrs.yaw_sensor
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -394,7 +396,8 @@ struct log_Nav_Tuning {
float wp_distance; float wp_distance;
uint16_t target_bearing_cd; uint16_t target_bearing_cd;
uint16_t nav_bearing_cd; uint16_t nav_bearing_cd;
int16_t nav_gain_scheduler; int16_t nav_gain_scalar;
int8_t throttle;
}; };
// Write a navigation tuning packet. Total length : 18 bytes // Write a navigation tuning packet. Total length : 18 bytes
@ -406,7 +409,8 @@ static void Log_Write_Nav_Tuning()
wp_distance : wp_distance, wp_distance : wp_distance,
target_bearing_cd : (uint16_t)target_bearing, target_bearing_cd : (uint16_t)target_bearing,
nav_bearing_cd : (uint16_t)nav_bearing, nav_bearing_cd : (uint16_t)nav_bearing,
nav_gain_scheduler : (int16_t)(nav_gain_scaler*1000) nav_gain_scalar : (int16_t)(nav_gain_scaler*1000),
throttle : (int8_t)(100 * g.channel_throttle.norm_output())
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -417,14 +421,68 @@ static void Log_Read_Nav_Tuning()
struct log_Nav_Tuning pkt; struct log_Nav_Tuning pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt)); DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f\n"), cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f, %d\n"),
(float)pkt.yaw/100.0f, (float)pkt.yaw/100.0f,
pkt.wp_distance, pkt.wp_distance,
(float)(pkt.target_bearing_cd/100.0f), (float)(pkt.target_bearing_cd/100.0f),
(float)(pkt.nav_bearing_cd/100.0f), (float)(pkt.nav_bearing_cd/100.0f),
(float)(pkt.nav_gain_scheduler/100.0f)); (float)(pkt.nav_gain_scalar/100.0f),
(int)pkt.throttle);
} }
struct log_Sonar {
LOG_PACKET_HEADER;
int16_t nav_steer;
uint16_t sonar1_distance;
uint16_t sonar2_distance;
uint16_t detected_count;
int8_t turn_angle;
uint16_t turn_time;
uint16_t ground_speed;
int8_t throttle;
};
// Write a sonar packet
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Sonar()
{
uint16_t turn_time = 0;
if (obstacle.turn_angle != 0) {
turn_time = hal.scheduler->millis() - (obstacle.detected_time_ms + g.sonar_turn_time*1000);
}
struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
nav_steer : (int16_t)nav_steer_cd,
sonar1_distance : (uint16_t)sonar.distance_cm(),
sonar2_distance : (uint16_t)sonar2.distance_cm(),
detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time,
ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * g.channel_throttle.norm_output())
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
#endif
// Read a sonar packet
static void Log_Read_Sonar()
{
struct log_Sonar pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("SONR, %d, %u, %u, %u, %d, %u, %u, %d\n"),
(int)pkt.nav_steer,
(unsigned)pkt.sonar1_distance,
(unsigned)pkt.sonar2_distance,
(unsigned)pkt.detected_count,
(int)pkt.turn_angle,
(unsigned)pkt.turn_time,
(unsigned)pkt.ground_speed,
(int)pkt.throttle);
}
struct log_Mode { struct log_Mode {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint8_t mode; uint8_t mode;
@ -452,31 +510,30 @@ static void Log_Read_Mode()
struct log_GPS { struct log_GPS {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t gps_time; uint32_t gps_time;
uint8_t gps_fix; uint8_t status;
uint8_t num_sats; uint8_t num_sats;
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t rel_altitude;
int32_t altitude; int32_t altitude;
uint32_t ground_speed; uint32_t ground_speed;
int32_t ground_course; int32_t ground_course;
int16_t hdop;
}; };
// Write an GPS packet. Total length : 30 bytes // Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS( uint32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, static void Log_Write_GPS()
uint32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats)
{ {
struct log_GPS pkt = { struct log_GPS pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), LOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
gps_time : log_Time, gps_time : g_gps->time,
gps_fix : log_Fix, status : g_gps->status(),
num_sats : log_NumSats, num_sats : g_gps->num_sats,
latitude : log_Lattitude, latitude : current_loc.lat,
longitude : log_Longitude, longitude : current_loc.lng,
rel_altitude : log_mix_alt, altitude : g_gps->altitude,
altitude : log_gps_alt, ground_speed : g_gps->ground_speed,
ground_speed : log_Ground_Speed, ground_course : g_gps->ground_course,
ground_course : log_Ground_Course hdop : g_gps->hdop
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -488,16 +545,16 @@ static void Log_Read_GPS()
DataFlash.ReadPacket(&pkt, sizeof(pkt)); DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("GPS, %ld, %u, %u, "), cliSerial->printf_P(PSTR("GPS, %ld, %u, %u, "),
(long)pkt.gps_time, (long)pkt.gps_time,
(unsigned)pkt.gps_fix, (unsigned)pkt.status,
(unsigned)pkt.num_sats); (unsigned)pkt.num_sats);
print_latlon(cliSerial, pkt.latitude); print_latlon(cliSerial, pkt.latitude);
cliSerial->print_P(PSTR(", ")); cliSerial->print_P(PSTR(", "));
print_latlon(cliSerial, pkt.longitude); print_latlon(cliSerial, pkt.longitude);
cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %lu, %ld\n"), cliSerial->printf_P(PSTR(", %4.4f, %lu, %ld, %d\n"),
(float)pkt.rel_altitude*0.01,
(float)pkt.altitude*0.01, (float)pkt.altitude*0.01,
(unsigned long)pkt.ground_speed, (unsigned long)pkt.ground_speed,
(long)pkt.ground_course); (long)pkt.ground_course,
(int)pkt.hdop);
} }
struct log_IMU { struct log_IMU {
@ -565,12 +622,20 @@ static void Log_Read_Current()
(int)pkt.current_total); (int)pkt.current_total);
} }
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
// Read the DataFlash log memory : Packet Parser // Read the DataFlash log memory : Packet Parser
static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
{ {
cliSerial->printf_P(PSTR("\n" THISFIRMWARE cliSerial->printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %u\n"), "\nFree RAM: %u\n"),
memcheck_available_memory()); (unsigned) memcheck_available_memory());
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
#if CLI_ENABLED == ENABLED
setup_show(0, NULL);
#endif
DataFlash.log_read_process(log_num, start_page, end_page, log_callback); DataFlash.log_read_process(log_num, start_page, end_page, log_callback);
} }
@ -618,6 +683,10 @@ static void log_callback(uint8_t msgid)
case LOG_GPS_MSG: case LOG_GPS_MSG:
Log_Read_GPS(); Log_Read_GPS();
break; break;
case LOG_SONAR_MSG:
Log_Read_Sonar();
break;
} }
} }
@ -629,13 +698,13 @@ static void Log_Write_Startup(uint8_t type) {}
static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {} static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
static void Log_Write_Current() {} static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {} static void Log_Write_Nav_Tuning() {}
static void Log_Write_GPS( uint32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt, static void Log_Write_GPS() {}
uint32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) {}
static void Log_Write_Performance() {} static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw) {} static void Log_Write_Attitude() {}
static void Log_Write_Control_Tuning() {} static void Log_Write_Control_Tuning() {}
static void Log_Write_IMU() {} static void Log_Write_IMU() {}
static void Log_Write_Sonar() {}
#endif // LOGGING_ENABLED #endif // LOGGING_ENABLED

8
APMrover2/Steering.pde
View File

@ -91,7 +91,7 @@ static void calc_throttle(float target_speed)
reduce target speed in proportion to turning rate, up to the reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage. SPEED_TURN_GAIN percentage.
*/ */
float steer_rate = fabsf((nav_steer/nav_gain_scaler) / (float)SERVO_MAX); float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0); steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01; float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
@ -136,12 +136,12 @@ static void calc_nav_steer()
// negative error = left turn // negative error = left turn
// positive error = right turn // positive error = right turn
nav_steer = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler); nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler);
// avoid obstacles, if any // avoid obstacles, if any
nav_steer += obstacle.turn_angle*100; nav_steer_cd += obstacle.turn_angle*100;
g.channel_steer.servo_out = nav_steer; g.channel_steer.servo_out = nav_steer_cd;
} }
/***************************************** /*****************************************

6
APMrover2/config.h
View File

@ -443,7 +443,7 @@
# define LOG_CTUN ENABLED # define LOG_CTUN ENABLED
#endif #endif
#ifndef LOG_NTUN #ifndef LOG_NTUN
# define LOG_NTUN DISABLED # define LOG_NTUN ENABLED
#endif #endif
#ifndef LOG_MODE #ifndef LOG_MODE
# define LOG_MODE ENABLED # define LOG_MODE ENABLED
@ -457,6 +457,9 @@
#ifndef LOG_CURRENT #ifndef LOG_CURRENT
# define LOG_CURRENT DISABLED # define LOG_CURRENT DISABLED
#endif #endif
#ifndef LOG_SONAR
# define LOG_SONAR ENABLED
#endif
// calculate the default log_bitmask // calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
@ -471,6 +474,7 @@
LOGBIT(MODE) | \ LOGBIT(MODE) | \
LOGBIT(IMU) | \ LOGBIT(IMU) | \
LOGBIT(CMD) | \ LOGBIT(CMD) | \
LOGBIT(SONAR) | \
LOGBIT(CURRENT) LOGBIT(CURRENT)

11
APMrover2/defines.h
View File

@ -129,14 +129,16 @@ enum gcs_severity {
#define LOG_INDEX_MSG 0xF0 #define LOG_INDEX_MSG 0xF0
#define LOG_ATTITUDE_MSG 0x01 #define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02 #define LOG_GPS_MSG 0x02
#define LOG_MODE_MSG 0X03 #define LOG_MODE_MSG 0x03
#define LOG_CONTROL_TUNING_MSG 0X04 #define LOG_CONTROL_TUNING_MSG 0x04
#define LOG_NAV_TUNING_MSG 0X05 #define LOG_NAV_TUNING_MSG 0x05
#define LOG_PERFORMANCE_MSG 0X06 #define LOG_PERFORMANCE_MSG 0x06
#define LOG_IMU_MSG 0x07 #define LOG_IMU_MSG 0x07
#define LOG_CMD_MSG 0x08 #define LOG_CMD_MSG 0x08
#define LOG_CURRENT_MSG 0x09 #define LOG_CURRENT_MSG 0x09
#define LOG_STARTUP_MSG 0x0A #define LOG_STARTUP_MSG 0x0A
#define LOG_SONAR_MSG 0x0B
#define TYPE_AIRSTART_MSG 0x00 #define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01 #define TYPE_GROUNDSTART_MSG 0x01
#define MAX_NUM_LOGS 100 #define MAX_NUM_LOGS 100
@ -151,6 +153,7 @@ enum gcs_severity {
#define MASK_LOG_IMU (1<<7) #define MASK_LOG_IMU (1<<7)
#define MASK_LOG_CMD (1<<8) #define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9) #define MASK_LOG_CURRENT (1<<9)
#define MASK_LOG_SONAR (1<<10)
// Waypoint Modes // Waypoint Modes
// ---------------- // ----------------

23
APMrover2/sensors.pde
View File

@ -58,49 +58,52 @@ static void read_sonars(void)
if (sonar2.enabled()) { if (sonar2.enabled()) {
// we have two sonars // we have two sonars
float sonar1_dist_cm = sonar.distance_cm(); obstacle.sonar1_distance_cm = sonar.distance_cm();
float sonar2_dist_cm = sonar2.distance_cm(); obstacle.sonar2_distance_cm = sonar2.distance_cm();
if (sonar1_dist_cm <= g.sonar_trigger_cm && if (obstacle.sonar1_distance_cm <= g.sonar_trigger_cm &&
sonar1_dist_cm <= sonar2_dist_cm) { obstacle.sonar2_distance_cm <= obstacle.sonar2_distance_cm) {
// we have an object on the left // we have an object on the left
if (obstacle.detected_count < 127) { if (obstacle.detected_count < 127) {
obstacle.detected_count++; obstacle.detected_count++;
} }
if (obstacle.detected_count == g.sonar_debounce) { if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(PSTR("Sonar1 obstacle %.0fcm"), gcs_send_text_fmt(PSTR("Sonar1 obstacle %.0fcm"),
sonar1_dist_cm); obstacle.sonar1_distance_cm);
} }
obstacle.detected_time_ms = hal.scheduler->millis(); obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = g.sonar_turn_angle; obstacle.turn_angle = g.sonar_turn_angle;
} else if (sonar2_dist_cm <= g.sonar_trigger_cm) { } else if (obstacle.sonar2_distance_cm <= g.sonar_trigger_cm) {
// we have an object on the right // we have an object on the right
if (obstacle.detected_count < 127) { if (obstacle.detected_count < 127) {
obstacle.detected_count++; obstacle.detected_count++;
} }
if (obstacle.detected_count == g.sonar_debounce) { if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(PSTR("Sonar2 obstacle %.0fcm"), gcs_send_text_fmt(PSTR("Sonar2 obstacle %.0fcm"),
sonar2_dist_cm); obstacle.sonar2_distance_cm);
} }
obstacle.detected_time_ms = hal.scheduler->millis(); obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = -g.sonar_turn_angle; obstacle.turn_angle = -g.sonar_turn_angle;
} }
} else { } else {
// we have a single sonar // we have a single sonar
float sonar_dist_cm = sonar.distance_cm(); obstacle.sonar1_distance_cm = sonar.distance_cm();
if (sonar_dist_cm <= g.sonar_trigger_cm) { obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm <= g.sonar_trigger_cm) {
// obstacle detected in front // obstacle detected in front
if (obstacle.detected_count < 127) { if (obstacle.detected_count < 127) {
obstacle.detected_count++; obstacle.detected_count++;
} }
if (obstacle.detected_count == g.sonar_debounce) { if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(PSTR("Sonar obstacle %.0fcm"), gcs_send_text_fmt(PSTR("Sonar obstacle %.0fcm"),
sonar_dist_cm); obstacle.sonar1_distance_cm);
} }
obstacle.detected_time_ms = hal.scheduler->millis(); obstacle.detected_time_ms = hal.scheduler->millis();
obstacle.turn_angle = g.sonar_turn_angle; obstacle.turn_angle = g.sonar_turn_angle;
} }
} }
Log_Write_Sonar();
// no object detected - reset after the turn time // no object detected - reset after the turn time
if (obstacle.detected_count >= g.sonar_debounce && if (obstacle.detected_count >= g.sonar_debounce &&
hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) {