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?
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

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;
mavlink_msg_nav_controller_output_send(
chan,
nav_steer / 1.0e2,
nav_steer_cd / 1.0e2,
0,
bearing,
target_bearing / 100,

127
APMrover2/Log.pde
View File

@ -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"),
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_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 {
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
"\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

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
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;
}
/*****************************************

6
APMrover2/config.h
View File

@ -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)

11
APMrover2/defines.h
View File

@ -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
// ----------------

23
APMrover2/sensors.pde
View File

@ -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) {