Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: combine NTUN and INAV dataflash msgs

Browse Source
This commit is contained in:
Randy Mackay 2014-01-13 22:00:11 +09:00
parent 1e8a79eeaf
commit 138c2803be
5 changed files with 26 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
ArduCopter/Log.pde
View File

@ -47,7 +47,6 @@ print_log_menu(void)
if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf_P(PSTR(" RCOUT")); if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf_P(PSTR(" RCOUT"));
if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW")); if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW"));
if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf_P(PSTR(" COMPASS")); if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf_P(PSTR(" COMPASS"));
if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV"));
if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA")); if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA"));
} }
@ -138,7 +137,6 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(RCOUT); TARG(RCOUT);
TARG(OPTFLOW); TARG(OPTFLOW);
TARG(COMPASS); TARG(COMPASS);
TARG(INAV);
TARG(CAMERA); TARG(CAMERA);
#undef TARG #undef TARG
} }
@ -263,39 +261,39 @@ static void Log_Write_Optflow()
struct PACKED log_Nav_Tuning { struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t wp_distance; uint32_t time_ms;
int16_t wp_bearing; float desired_pos_x;
float pos_error_x; float desired_pos_y;
float pos_error_y; float pos_x;
float desired_velocity_x; float pos_y;
float desired_velocity_y; float desired_vel_x;
float velocity_x; float desired_vel_y;
float velocity_y; float vel_x;
float vel_y;
float desired_accel_x; float desired_accel_x;
float desired_accel_y; 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()
{ {
const Vector3f &desired_position = wp_nav.get_loiter_target();
const Vector3f &position = inertial_nav.get_position();
const Vector3f &velocity = inertial_nav.get_velocity(); const 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, time_ms : hal.scheduler->millis(),
wp_bearing : (int16_t) (wp_bearing/100), desired_pos_x : desired_position.x,
pos_error_x : wp_nav.dist_error.x, desired_pos_y : desired_position.y,
pos_error_y : wp_nav.dist_error.y, pos_x : position.x,
desired_velocity_x : wp_nav.desired_vel.x, pos_y : position.y,
desired_velocity_y : wp_nav.desired_vel.y, desired_vel_x : wp_nav.desired_vel.x,
velocity_x : velocity.x, desired_vel_y : wp_nav.desired_vel.y,
velocity_y : velocity.y, vel_x : velocity.x,
desired_accel_x : wp_nav.desired_accel.x, vel_y : velocity.y,
desired_accel_y : wp_nav.desired_accel.y, desired_accel_x : wp_nav.desired_accel.x,
desired_roll : wp_nav.get_desired_roll(), desired_accel_y : wp_nav.desired_accel.y
desired_pitch : wp_nav.get_desired_pitch()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -479,41 +477,6 @@ static void Log_Write_Attitude()
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
struct PACKED log_INAV {
LOG_PACKET_HEADER;
int16_t baro_alt;
int16_t inav_alt;
int16_t inav_climb_rate;
float accel_corr_x;
float accel_corr_y;
float accel_corr_z;
int32_t gps_lat_from_home;
int32_t gps_lon_from_home;
float inav_lat_from_home;
float inav_lon_from_home;
};
// Write an INAV packet
static void Log_Write_INAV()
{
const Vector3f &accel_corr = inertial_nav.accel_correction_ef;
struct log_INAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_INAV_MSG),
baro_alt : (int16_t)baro_alt, // 1 barometer altitude
inav_alt : (int16_t)inertial_nav.get_altitude(), // 2 accel + baro filtered altitude
inav_climb_rate : (int16_t)inertial_nav.get_velocity_z(), // 3 accel + baro based climb rate
accel_corr_x : accel_corr.x, // 4 accel correction x-axis
accel_corr_y : accel_corr.y, // 5 accel correction y-axis
accel_corr_z : accel_corr.z, // 6 accel correction z-axis
gps_lat_from_home : g_gps->latitude-home.lat, // 7 lat from home
gps_lon_from_home : g_gps->longitude-home.lng, // 8 lon from home
inav_lat_from_home : inertial_nav.get_latitude_diff(), // 9 accel based lat from home
inav_lon_from_home : inertial_nav.get_longitude_diff() // 10 accel based lon from home
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Mode { struct PACKED log_Mode {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint8_t mode; uint8_t mode;
@ -717,7 +680,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "hhBccee", "Dx,Dy,SQual,X,Y,Roll,Pitch" }, "OF", "hhBccee", "Dx,Dy,SQual,X,Y,Roll,Pitch" },
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
"NTUN", "Ecffffffffee", "WPDst,WPBrg,PErX,PErY,DVelX,DVelY,VelX,VelY,DAcX,DAcY,DRol,DPit" }, "NTUN", "Iffffffffff", "TimeMS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, "CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_COMPASS_MSG, sizeof(log_Compass), { LOG_COMPASS_MSG, sizeof(log_Compass),
@ -730,8 +693,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" }, "CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude), { LOG_ATTITUDE_MSG, sizeof(log_Attitude),
"ATT", "IccccCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw" }, "ATT", "IccccCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw" },
{ LOG_INAV_MSG, sizeof(log_INAV),
"INAV", "cccfffiiff", "BAlt,IAlt,IClb,ACorrX,ACorrY,ACorrZ,GLat,GLng,ILat,ILng" },
{ LOG_MODE_MSG, sizeof(log_Mode), { LOG_MODE_MSG, sizeof(log_Mode),
"MODE", "Mh", "Mode,ThrCrs" }, "MODE", "Mh", "Mode,ThrCrs" },
{ LOG_STARTUP_MSG, sizeof(log_Startup), { LOG_STARTUP_MSG, sizeof(log_Startup),
@ -809,7 +770,6 @@ static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) {}
static void Log_Write_Current() {} static void Log_Write_Current() {}
static void Log_Write_Compass() {} static void Log_Write_Compass() {}
static void Log_Write_Attitude() {} static void Log_Write_Attitude() {}
static void Log_Write_INAV() {}
static void Log_Write_Data(uint8_t id, int16_t value){} static void Log_Write_Data(uint8_t id, int16_t value){}
static void Log_Write_Data(uint8_t id, uint16_t value){} static void Log_Write_Data(uint8_t id, uint16_t value){}
static void Log_Write_Data(uint8_t id, int32_t value){} static void Log_Write_Data(uint8_t id, int32_t value){}

2
ArduCopter/Parameters.pde
View File

@ -360,7 +360,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: LOG_BITMASK // @Param: LOG_BITMASK
// @DisplayName: Log bitmask // @DisplayName: Log bitmask
// @Description: 2 byte bitmap of log types to enable // @Description: 2 byte bitmap of log types to enable
// @Values: 0:Disabled,830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,17214:Default+INav,-6146:NearlyAll // @Values: 0:Disabled,830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll
// @User: Standard // @User: Standard
GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),

1
ArduCopter/config.h
View File

@ -972,7 +972,6 @@
MASK_LOG_CURRENT | \ MASK_LOG_CURRENT | \
MASK_LOG_RCOUT | \ MASK_LOG_RCOUT | \
MASK_LOG_COMPASS | \ MASK_LOG_COMPASS | \
MASK_LOG_INAV | \
MASK_LOG_CAMERA MASK_LOG_CAMERA
#endif #endif

4
ArduCopter/defines.h
View File

@ -252,7 +252,7 @@
#define LOG_EVENT_MSG 0x0D #define LOG_EVENT_MSG 0x0D
#define LOG_PID_MSG 0x0E // deprecated #define LOG_PID_MSG 0x0E // deprecated
#define LOG_COMPASS_MSG 0x0F #define LOG_COMPASS_MSG 0x0F
#define LOG_INAV_MSG 0x11 #define LOG_INAV_MSG 0x11 // deprecated
#define LOG_CAMERA_MSG 0x12 #define LOG_CAMERA_MSG 0x12
#define LOG_ERROR_MSG 0x13 #define LOG_ERROR_MSG 0x13
#define LOG_DATA_INT16_MSG 0x14 #define LOG_DATA_INT16_MSG 0x14
@ -280,7 +280,7 @@
#define MASK_LOG_OPTFLOW (1<<11) #define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12) // deprecated #define MASK_LOG_PID (1<<12) // deprecated
#define MASK_LOG_COMPASS (1<<13) #define MASK_LOG_COMPASS (1<<13)
#define MASK_LOG_INAV (1<<14) #define MASK_LOG_INAV (1<<14) // deprecated
#define MASK_LOG_CAMERA (1<<15) #define MASK_LOG_CAMERA (1<<15)
// DATA - event logging // DATA - event logging

10
ArduCopter/inertia.pde
View File

@ -3,18 +3,8 @@
// read_inertia - read inertia in from accelerometers // read_inertia - read inertia in from accelerometers
static void read_inertia() static void read_inertia()
{ {
static uint8_t log_counter_inav = 0;
// inertial altitude estimates // inertial altitude estimates
inertial_nav.update(G_Dt); inertial_nav.update(G_Dt);
if (g.log_bitmask & MASK_LOG_INAV) {
log_counter_inav++;
if( log_counter_inav >= 10 ) {
log_counter_inav = 0;
Log_Write_INAV();
}
}
} }
// read_inertial_altitude - pull altitude and climb rate from inertial nav library // read_inertial_altitude - pull altitude and climb rate from inertial nav library