aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: merge WPNAV into NTUN dataflash message

Browse Source
This commit is contained in:
Randy Mackay 2013-05-19 22:53:35 +09:00
parent 072f3dbe30
commit 4ad395e7be
3 changed files with 30 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
ArduCopter/Log.pde
View File

@ -278,27 +278,37 @@ struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t wp_distance; uint32_t wp_distance;
int16_t wp_bearing; int16_t wp_bearing;
float lat_error; float pos_error_x;
float lon_error; float pos_error_y;
int16_t nav_pitch; float desired_velocity_x;
int16_t nav_roll; float desired_velocity_y;
int16_t lat_speed; float velocity_x;
int16_t lon_speed; float velocity_y;
float desired_accel_x;
float desired_accel_y;
int32_t desired_roll;
int32_t desired_pitch;
}; };
// Write an Nav Tuning packet // Write an Nav Tuning packet
static void Log_Write_Nav_Tuning() static void Log_Write_Nav_Tuning()
{ {
Vector3f velocity = inertial_nav.get_velocity();
struct log_Nav_Tuning pkt = { struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG), LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
wp_distance : wp_distance, wp_distance : wp_distance,
wp_bearing : (int16_t) (wp_bearing/100), wp_bearing : (int16_t) (wp_bearing/100),
lat_error : lat_error, pos_error_x : wp_nav.dist_error.x,
lon_error : lon_error, pos_error_y : wp_nav.dist_error.y,
nav_pitch : (int16_t) nav_pitch, desired_velocity_x : wp_nav.desired_vel.x,
nav_roll : (int16_t) nav_roll, desired_velocity_y : wp_nav.desired_vel.y,
lat_speed : (int16_t) inertial_nav.get_latitude_velocity(), velocity_x : velocity.x,
lon_speed : (int16_t) inertial_nav.get_longitude_velocity() velocity_y : velocity.y,
desired_accel_x : wp_nav.desired_accel.x,
desired_accel_y : wp_nav.desired_accel.y,
desired_roll : wp_nav.get_desired_roll(),
desired_pitch : wp_nav.get_desired_pitch()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -729,41 +739,6 @@ static void Log_Write_Error(uint8_t sub_system, uint8_t error_code)
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
struct PACKED log_WPNAV {
LOG_PACKET_HEADER;
float pos_error_x;
float pos_error_y;
float desired_velocity_x;
float desired_velocity_y;
float velocity_x;
float velocity_y;
float desired_accel_x;
float desired_accel_y;
int32_t desired_roll;
int32_t desired_pitch;
};
// Write an WPNAV packet
static void Log_Write_WPNAV()
{
Vector3f velocity = inertial_nav.get_velocity();
struct log_WPNAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_WPNAV_MSG),
pos_error_x : wp_nav.dist_error.x,
pos_error_y : wp_nav.dist_error.y,
desired_velocity_x : wp_nav.desired_vel.x,
desired_velocity_y : wp_nav.desired_vel.y,
velocity_x : velocity.x,
velocity_y : velocity.y,
desired_accel_x : wp_nav.desired_accel.x,
desired_accel_y : wp_nav.desired_accel.y,
desired_roll : wp_nav.get_desired_roll(),
desired_pitch : wp_nav.get_desired_pitch()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
static const struct LogStructure log_structure[] PROGMEM = { static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES, LOG_COMMON_STRUCTURES,
{ LOG_CURRENT_MSG, sizeof(log_Current), { LOG_CURRENT_MSG, sizeof(log_Current),
@ -786,7 +761,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "hhBccffee", "Dx,Dy,SQual,X,Y,Lat,Lng,Roll,Pitch" }, "OF", "hhBccffee", "Dx,Dy,SQual,X,Y,Lat,Lng,Roll,Pitch" },
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Ecffcccc", "WPDist,TargBrg,LatErr,LngErr,NavPtch,NavRll,LatSpd,LngSpd" }, "NTUN", "Ecffffffffee", "WPDst,WPBrg,PErX,PErY,DVelX,DVelY,VelX,VelY,DAcX,DAcY,DRol,DPit" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "hcefhhhhh", "ThrIn,SonAlt,BarAlt,WPAlt,NavThr,AngBst,CRate,ThrOut,DCRate" }, "CTUN", "hcefhhhhh", "ThrIn,SonAlt,BarAlt,WPAlt,NavThr,AngBst,CRate,ThrOut,DCRate" },
{ LOG_COMPASS_MSG, sizeof(log_Compass), { LOG_COMPASS_MSG, sizeof(log_Compass),
@ -823,8 +798,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
"CAM", "ILLeccC", "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" }, "CAM", "ILLeccC", "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" },
{ LOG_ERROR_MSG, sizeof(log_Error), { LOG_ERROR_MSG, sizeof(log_Error),
"ERR", "BB", "Subsys,ECode" }, "ERR", "BB", "Subsys,ECode" },
{ LOG_WPNAV_MSG, sizeof(log_WPNAV),
"WNAV", "ffffffffee", "PErrX,PErrY,DVelX,DVelY,VelX,VelY,DAccX,DAccY,DRoll,DPtch" },
}; };
// Read the DataFlash log memory // Read the DataFlash log memory

1
ArduCopter/defines.h
View File

@ -281,7 +281,6 @@ enum ap_message {
#define LOG_DATA_INT32_MSG 0x16 #define LOG_DATA_INT32_MSG 0x16
#define LOG_DATA_UINT32_MSG 0x17 #define LOG_DATA_UINT32_MSG 0x17
#define LOG_DATA_FLOAT_MSG 0x18 #define LOG_DATA_FLOAT_MSG 0x18
#define LOG_WPNAV_MSG 0x19
#define LOG_INDEX_MSG 0xF0 #define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50 #define MAX_NUM_LOGS 50

14
ArduCopter/navigation.pde
View File

@ -23,11 +23,6 @@ static void update_navigation()
// run the navigation controllers // run the navigation controllers
update_nav_mode(); update_nav_mode();
// update log
if ((g.log_bitmask & MASK_LOG_NTUN) && motors.armed()) {
Log_Write_Nav_Tuning();
}
} }
} }
@ -165,18 +160,19 @@ static void update_nav_mode()
case NAV_LOITER: case NAV_LOITER:
// call loiter controller // call loiter controller
wp_nav.update_loiter(); wp_nav.update_loiter();
// log to dataflash
Log_Write_WPNAV();
break; break;
case NAV_WP: case NAV_WP:
// call waypoint controller // call waypoint controller
wp_nav.update_wpnav(); wp_nav.update_wpnav();
// log to dataflash
Log_Write_WPNAV();
break; break;
} }
// log to dataflash
if ((g.log_bitmask & MASK_LOG_NTUN) && nav_mode != NAV_NONE) {
Log_Write_Nav_Tuning();
}
/* /*
// To-Do: check that we haven't broken toy mode // To-Do: check that we haven't broken toy mode
case TOY_A: case TOY_A: