diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 539ca5b8df..5797d577cb 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -376,6 +376,8 @@ static struct { // have we detected an obstacle? uint8_t detected_count; float turn_angle; + uint16_t sonar1_distance_cm; + uint16_t sonar2_distance_cm; // time when we last detected an obstacle, in milliseconds uint32_t detected_time_ms; @@ -428,7 +430,7 @@ static float current_total1; // Navigation control variables //////////////////////////////////////////////////////////////////////////////// // The instantaneous desired steering angle. Hundredths of a degree -static int32_t nav_steer; +static int32_t nav_steer_cd; //////////////////////////////////////////////////////////////////////////////// // Waypoint distances @@ -636,7 +638,7 @@ static void fast_loop() # if HIL_MODE == HIL_MODE_DISABLED 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) Log_Write_IMU(); @@ -707,7 +709,7 @@ static void medium_loop() medium_loopCounter++; #if HIL_MODE != HIL_MODE_ATTITUDE 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) Log_Write_Control_Tuning(); @@ -717,7 +719,7 @@ static void medium_loop() Log_Write_Nav_Tuning(); 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; // This case controls the slow loop diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 62fb373700..6b16d0edc1 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -240,7 +240,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) int16_t bearing = nav_bearing / 100; mavlink_msg_nav_controller_output_send( chan, - nav_steer / 1.0e2, + nav_steer_cd / 1.0e2, 0, bearing, target_bearing / 100, diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index a01cf6d9de..c5915b596d 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -48,6 +48,7 @@ print_log_menu(void) PLOG(IMU); PLOG(CMD); PLOG(CURRENT); + PLOG(SONAR); #undef PLOG } @@ -135,6 +136,7 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(IMU); TARG(CMD); TARG(CURRENT); + TARG(SONAR); #undef TARG } @@ -182,13 +184,13 @@ struct log_Attitute { }; // 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 = { LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - roll : log_roll, - pitch : log_pitch, - yaw : log_yaw + roll : (int16_t)ahrs.roll_sensor, + pitch : (int16_t)ahrs.pitch_sensor, + yaw : (uint16_t)ahrs.yaw_sensor }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -394,7 +396,8 @@ struct log_Nav_Tuning { float wp_distance; uint16_t target_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 @@ -406,7 +409,8 @@ static void Log_Write_Nav_Tuning() wp_distance : wp_distance, target_bearing_cd : (uint16_t)target_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)); } @@ -417,14 +421,68 @@ static void Log_Read_Nav_Tuning() struct log_Nav_Tuning pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f\n"), - (float)pkt.yaw/100.0f, - pkt.wp_distance, - (float)(pkt.target_bearing_cd/100.0f), - (float)(pkt.nav_bearing_cd/100.0f), - (float)(pkt.nav_gain_scheduler/100.0f)); + cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f, %d\n"), + (float)pkt.yaw/100.0f, + pkt.wp_distance, + (float)(pkt.target_bearing_cd/100.0f), + (float)(pkt.nav_bearing_cd/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 { LOG_PACKET_HEADER; uint8_t mode; @@ -452,31 +510,30 @@ static void Log_Read_Mode() struct log_GPS { LOG_PACKET_HEADER; uint32_t gps_time; - uint8_t gps_fix; + uint8_t status; uint8_t num_sats; int32_t latitude; int32_t longitude; - int32_t rel_altitude; int32_t altitude; uint32_t ground_speed; int32_t ground_course; + int16_t hdop; }; // 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, - uint32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) +static void Log_Write_GPS() { struct log_GPS pkt = { LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), - gps_time : log_Time, - gps_fix : log_Fix, - num_sats : log_NumSats, - latitude : log_Lattitude, - longitude : log_Longitude, - rel_altitude : log_mix_alt, - altitude : log_gps_alt, - ground_speed : log_Ground_Speed, - ground_course : log_Ground_Course + gps_time : g_gps->time, + status : g_gps->status(), + num_sats : g_gps->num_sats, + latitude : current_loc.lat, + longitude : current_loc.lng, + altitude : g_gps->altitude, + ground_speed : g_gps->ground_speed, + ground_course : g_gps->ground_course, + hdop : g_gps->hdop }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -488,16 +545,16 @@ static void Log_Read_GPS() DataFlash.ReadPacket(&pkt, sizeof(pkt)); cliSerial->printf_P(PSTR("GPS, %ld, %u, %u, "), (long)pkt.gps_time, - (unsigned)pkt.gps_fix, + (unsigned)pkt.status, (unsigned)pkt.num_sats); print_latlon(cliSerial, pkt.latitude); cliSerial->print_P(PSTR(", ")); print_latlon(cliSerial, pkt.longitude); - cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %lu, %ld\n"), - (float)pkt.rel_altitude*0.01, + cliSerial->printf_P(PSTR(", %4.4f, %lu, %ld, %d\n"), (float)pkt.altitude*0.01, (unsigned long)pkt.ground_speed, - (long)pkt.ground_course); + (long)pkt.ground_course, + (int)pkt.hdop); } struct log_IMU { @@ -565,12 +622,20 @@ static void Log_Read_Current() (int)pkt.current_total); } +static int8_t setup_show (uint8_t argc, const Menu::arg *argv); + // Read the DataFlash log memory : Packet Parser 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"), - 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); } @@ -618,6 +683,10 @@ static void log_callback(uint8_t msgid) case LOG_GPS_MSG: Log_Read_GPS(); 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_Current() {} 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, - uint32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) {} +static void Log_Write_GPS() {} static void Log_Write_Performance() {} 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_IMU() {} +static void Log_Write_Sonar() {} #endif // LOGGING_ENABLED diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 16205978b8..ee92d76904 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -91,7 +91,7 @@ static void calc_throttle(float target_speed) reduce target speed in proportion to turning rate, up to the 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); 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 // 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 - 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; } /***************************************** diff --git a/APMrover2/config.h b/APMrover2/config.h index 609cc2b08f..facd7d4213 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -443,7 +443,7 @@ # define LOG_CTUN ENABLED #endif #ifndef LOG_NTUN -# define LOG_NTUN DISABLED +# define LOG_NTUN ENABLED #endif #ifndef LOG_MODE # define LOG_MODE ENABLED @@ -457,6 +457,9 @@ #ifndef LOG_CURRENT # define LOG_CURRENT DISABLED #endif +#ifndef LOG_SONAR +# define LOG_SONAR ENABLED +#endif // calculate the default log_bitmask #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) @@ -471,6 +474,7 @@ LOGBIT(MODE) | \ LOGBIT(IMU) | \ LOGBIT(CMD) | \ + LOGBIT(SONAR) | \ LOGBIT(CURRENT) diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 0330bf737f..9700aa6879 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -129,14 +129,16 @@ enum gcs_severity { #define LOG_INDEX_MSG 0xF0 #define LOG_ATTITUDE_MSG 0x01 #define LOG_GPS_MSG 0x02 -#define LOG_MODE_MSG 0X03 -#define LOG_CONTROL_TUNING_MSG 0X04 -#define LOG_NAV_TUNING_MSG 0X05 -#define LOG_PERFORMANCE_MSG 0X06 +#define LOG_MODE_MSG 0x03 +#define LOG_CONTROL_TUNING_MSG 0x04 +#define LOG_NAV_TUNING_MSG 0x05 +#define LOG_PERFORMANCE_MSG 0x06 #define LOG_IMU_MSG 0x07 #define LOG_CMD_MSG 0x08 #define LOG_CURRENT_MSG 0x09 #define LOG_STARTUP_MSG 0x0A +#define LOG_SONAR_MSG 0x0B + #define TYPE_AIRSTART_MSG 0x00 #define TYPE_GROUNDSTART_MSG 0x01 #define MAX_NUM_LOGS 100 @@ -151,6 +153,7 @@ enum gcs_severity { #define MASK_LOG_IMU (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CURRENT (1<<9) +#define MASK_LOG_SONAR (1<<10) // Waypoint Modes // ---------------- diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 88ddb05d02..87f4ddf7a9 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -58,49 +58,52 @@ static void read_sonars(void) if (sonar2.enabled()) { // we have two sonars - float sonar1_dist_cm = sonar.distance_cm(); - float sonar2_dist_cm = sonar2.distance_cm(); - if (sonar1_dist_cm <= g.sonar_trigger_cm && - sonar1_dist_cm <= sonar2_dist_cm) { + obstacle.sonar1_distance_cm = sonar.distance_cm(); + obstacle.sonar2_distance_cm = sonar2.distance_cm(); + if (obstacle.sonar1_distance_cm <= g.sonar_trigger_cm && + obstacle.sonar2_distance_cm <= obstacle.sonar2_distance_cm) { // we have an object on the left if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { gcs_send_text_fmt(PSTR("Sonar1 obstacle %.0fcm"), - sonar1_dist_cm); + obstacle.sonar1_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); 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 if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { gcs_send_text_fmt(PSTR("Sonar2 obstacle %.0fcm"), - sonar2_dist_cm); + obstacle.sonar2_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); obstacle.turn_angle = -g.sonar_turn_angle; } } else { // we have a single sonar - float sonar_dist_cm = sonar.distance_cm(); - if (sonar_dist_cm <= g.sonar_trigger_cm) { + obstacle.sonar1_distance_cm = sonar.distance_cm(); + obstacle.sonar2_distance_cm = 0; + if (obstacle.sonar1_distance_cm <= g.sonar_trigger_cm) { // obstacle detected in front if (obstacle.detected_count < 127) { obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { gcs_send_text_fmt(PSTR("Sonar obstacle %.0fcm"), - sonar_dist_cm); + obstacle.sonar1_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); obstacle.turn_angle = g.sonar_turn_angle; } } + Log_Write_Sonar(); + // no object detected - reset after the turn time if (obstacle.detected_count >= g.sonar_debounce && hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) {