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

Sub: Match Copter changes.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-04-21 16:06:15 -07:00 committed by Andrew Tridgell
parent d0130136f0
commit 3b679808f3
5 changed files with 28 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
ArduSub/Log.cpp
View File

@ -339,6 +339,7 @@ struct PACKED log_Performance {
int16_t pm_test;
uint8_t i2c_lockup_count;
uint16_t ins_error_count;
uint32_t log_dropped;
};
// Write a performance monitoring packet
@ -352,9 +353,10 @@ void Sub::Log_Write_Performance()
max_time : perf_info_get_max_time(),
pm_test : pmTest1,
i2c_lockup_count : hal.i2c->lockup_count(),
ins_error_count : ins.error_count()
ins_error_count : ins.error_count(),
log_dropped : DataFlash.num_dropped() - perf_info_get_num_dropped(),
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
// Write an attitude packet
@ -399,21 +401,6 @@ void Sub::Log_Write_MotBatt()
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
}
struct PACKED log_Startup {
LOG_PACKET_HEADER;
uint64_t time_us;
};
// Write Startup packet
void Sub::Log_Write_Startup()
{
struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
time_us : AP_HAL::micros64()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Event {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -451,7 +438,7 @@ void Sub::Log_Write_Data(uint8_t id, int16_t value)
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
@ -473,7 +460,7 @@ void Sub::Log_Write_Data(uint8_t id, uint16_t value)
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
@ -494,7 +481,7 @@ void Sub::Log_Write_Data(uint8_t id, int32_t value)
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
@ -515,7 +502,7 @@ void Sub::Log_Write_Data(uint8_t id, uint32_t value)
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
@ -537,7 +524,7 @@ void Sub::Log_Write_Data(uint8_t id, float value)
id : id,
data_value : value
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
}
@ -557,7 +544,7 @@ void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
sub_system : sub_system,
error_code : error_code,
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
}
void Sub::Log_Write_Baro(void)
@ -685,11 +672,9 @@ const struct LogStructure Sub::log_structure[] = {
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QHHIhBH", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
"PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" },
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
"MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "Q", "TimeUS" },
{ LOG_EVENT_MSG, sizeof(log_Event),
"EV", "QB", "TimeUS,Id" },
{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),

1
ArduSub/Sub.h
View File

@ -882,6 +882,7 @@ private:
uint32_t perf_info_get_max_time();
uint32_t perf_info_get_min_time();
uint16_t perf_info_get_num_long_running();
uint32_t perf_info_get_num_dropped();
Vector3f pv_location_to_vector(const Location& loc);
Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec);
float pv_alt_above_origin(float alt_above_home_cm);

1
ArduSub/defines.h
View File

@ -268,7 +268,6 @@ enum ThrowModeState {
#define LOG_CONTROL_TUNING_MSG 0x04
#define LOG_NAV_TUNING_MSG 0x05
#define LOG_PERFORMANCE_MSG 0x06
#define LOG_STARTUP_MSG 0x0A
#define LOG_OPTFLOW_MSG 0x0C
#define LOG_EVENT_MSG 0x0D
#define LOG_PID_MSG 0x0E // deprecated

12
ArduSub/motors.cpp
View File

@ -235,10 +235,14 @@ void Sub::init_disarm_motors()
#endif
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(compass.get_primary(), magOffsets);
}
if (ahrs.use_compass()) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
if (ahrs.getMagOffsets(i, magOffsets)) {
compass.set_and_save_offsets(i, magOffsets);
}
}
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters

8
ArduSub/perf_info.cpp
View File

@ -15,6 +15,7 @@ static uint16_t perf_info_loop_count;
static uint32_t perf_info_max_time;
static uint32_t perf_info_min_time;
static uint16_t perf_info_long_running;
static uint32_t perf_info_log_dropped;
static bool perf_ignore_loop = false;
// perf_info_reset - reset all records of loop time to zero
@ -24,6 +25,7 @@ void Sub::perf_info_reset()
perf_info_max_time = 0;
perf_info_min_time = 0;
perf_info_long_running = 0;
perf_info_log_dropped = DataFlash.num_dropped();
}
// perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
@ -77,3 +79,9 @@ uint16_t Sub::perf_info_get_num_long_running()
{
return perf_info_long_running;
}
// perf_info_get_num_dropped - get number of dropped log messages
uint32_t Sub::perf_info_get_num_dropped()
{
return perf_info_log_dropped;
}