diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index fcf026e615..69ff06547b 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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_OPTFLOW) cliSerial->printf_P(PSTR(" OPTFLOW")); 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")); } @@ -138,7 +137,6 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(RCOUT); TARG(OPTFLOW); TARG(COMPASS); - TARG(INAV); TARG(CAMERA); #undef TARG } @@ -263,39 +261,39 @@ static void Log_Write_Optflow() struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; - uint32_t wp_distance; - int16_t wp_bearing; - float pos_error_x; - float pos_error_y; - float desired_velocity_x; - float desired_velocity_y; - float velocity_x; - float velocity_y; + uint32_t time_ms; + float desired_pos_x; + float desired_pos_y; + float pos_x; + float pos_y; + float desired_vel_x; + float desired_vel_y; + float vel_x; + float vel_y; float desired_accel_x; float desired_accel_y; - int32_t desired_roll; - int32_t desired_pitch; }; // Write an Nav Tuning packet 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(); struct log_Nav_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG), - wp_distance : wp_distance, - wp_bearing : (int16_t) (wp_bearing/100), - 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() + time_ms : hal.scheduler->millis(), + desired_pos_x : desired_position.x, + desired_pos_y : desired_position.y, + pos_x : position.x, + pos_y : position.y, + desired_vel_x : wp_nav.desired_vel.x, + desired_vel_y : wp_nav.desired_vel.y, + vel_x : velocity.x, + vel_y : velocity.y, + desired_accel_x : wp_nav.desired_accel.x, + desired_accel_y : wp_nav.desired_accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -479,41 +477,6 @@ static void Log_Write_Attitude() 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 { LOG_PACKET_HEADER; uint8_t mode; @@ -717,7 +680,7 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_OPTFLOW_MSG, sizeof(log_Optflow), "OF", "hhBccee", "Dx,Dy,SQual,X,Y,Roll,Pitch" }, { 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), "CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, { 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" }, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), "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), "MODE", "Mh", "Mode,ThrCrs" }, { 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_Compass() {} 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, uint16_t value){} static void Log_Write_Data(uint8_t id, int32_t value){} diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index a35eb2403c..f9cbd3429e 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -360,7 +360,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @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 GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 51cbc3aa81..0df3116b43 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -972,7 +972,6 @@ MASK_LOG_CURRENT | \ MASK_LOG_RCOUT | \ MASK_LOG_COMPASS | \ - MASK_LOG_INAV | \ MASK_LOG_CAMERA #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 773049b43b..89d93f85b9 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -252,7 +252,7 @@ #define LOG_EVENT_MSG 0x0D #define LOG_PID_MSG 0x0E // deprecated #define LOG_COMPASS_MSG 0x0F -#define LOG_INAV_MSG 0x11 +#define LOG_INAV_MSG 0x11 // deprecated #define LOG_CAMERA_MSG 0x12 #define LOG_ERROR_MSG 0x13 #define LOG_DATA_INT16_MSG 0x14 @@ -280,7 +280,7 @@ #define MASK_LOG_OPTFLOW (1<<11) #define MASK_LOG_PID (1<<12) // deprecated #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) // DATA - event logging diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde index fe20639f7a..ba29c47745 100644 --- a/ArduCopter/inertia.pde +++ b/ArduCopter/inertia.pde @@ -3,18 +3,8 @@ // read_inertia - read inertia in from accelerometers static void read_inertia() { - static uint8_t log_counter_inav = 0; - // inertial altitude estimates 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