Hi everyone.
I have a map of some data that I took with a second rangefinder. this map is an array int16_t ground_profile[2000]
. I am trying to log it every 1 sec. Before, I’ve successfully logged other messages, as described in https://ardupilot.org/dev/docs/code-overview-adding-a-new-log-message.html (using the method DataFlash_Class::instance()->Log_Write
instead of the function mentioned in the site, which is probably deprecated for that matter) . But I have a problem logging arrays.
My idea is to log my 2000 cell array in chunks of int16_t[32]
arrays, which are supported by the format specifier 'a'
(according to https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Logger/LogStructure.h).
I use the following code:
int i_map; // global map index
uint8_t i_chunk, i_chunk_copy; // index within a chunk
for (chunk_seq_no = 0; chunk_seq_no < (GPA_MAP_LOG_N_CHUNKS - 1); chunk_seq_no++) {
// determine the map chunk
for (i_chunk = 0; i_chunk < GPA_MAP_LOG_CHUNK_SIZE; i_chunk++) {
map_chunk[i_chunk] = get_ground_profile_datum(chunk_seq_no*GPA_MAP_LOG_CHUNK_SIZE + i_chunk);
}
// log the map chunk
DataFlash_Class::instance()->Log_Write("GPAM", // tag for Ground Profile Aquisition Map
"TimeUS,MapSeqNo,ChunkSeqNo,XChunk0,ZArr,NValid",
"s--mm-",
"F--BB-",
"QIHiaB",
AP_HAL::micros64(),
ground_profile_map_seq_no, // gpa ground profile map counter
chunk_seq_no, // gpa ground profile map chunk counter
(int) (chunk_seq_no*GPA_MAP_LOG_CHUNK_SIZE), // first x of the chunk
map_chunk, // gpa ground profile map chunk
// no of valid values in map chunk array
((uint8_t) i_chunk) // should have had a full chunk
);
}
and a similar code for the last chunk, where I count the number of valid cells and log this as NValid. I left out this code in the post to prevent from spamming you guys. However, please tell me if the code would help fixing the problem, then of course I’ll post it, too.
I let the code run for some time ant the map_chunk indices are calculated correctly, but the map_chunk
is not logged correctly. Instead always the same map_chunk
is logged:
GPAM, 212758196, 2521, 0, 0, [160 9216 20592 0 0 -19456 -32678 49 699 17333 -23610 58 0 -13312 -10472 -20938 20439 -12234 21617 24886 -26390 -15935 -2118 -26439 3519 11577 18443 0 0 -9984 22168 0], 0
GPAM, 212758196, 2521, 1, 32, [160 9216 20592 0 0 -19456 -32678 49 699 17333 -23610 58 0 -13312 -10472 -20938 20439 -12234 21617 24886 -26390 -15935 -2118 -26439 3519 11577 18443 0 0 -9984 22168 0], 0
...
All the other logged parameters of GPAM look fine. To me it looks, as the same random uninitialized array has been logged all the time.
How can I fix this?
Does the Log_Write
method persist the array as soon as it gets called, or do I have to create 63 arrays of 32 cells each, so the function has enough time to persistently write the arrays’ contents? Is there some special stepping stone to consider, when logging arrays with Log_Write
?
Thanks,
Peter