Closed andrejpodzimek closed 5 years ago
Forgot to mention: This machine has had >3 3.5.x versions on it and the problem was the same each time. So it's not specific to 3.5.7.
Try testing by logging with set debug_mode = LIDAR_TF
. Then debug items will be:
DEBUG_SET(DEBUG_LIDAR_TF, 0, distance);
DEBUG_SET(DEBUG_LIDAR_TF, 1, strength);
DEBUG_SET(DEBUG_LIDAR_TF, 2, tfFrame[4]);
DEBUG_SET(DEBUG_LIDAR_TF, 3, tfFrame[5]);
If this isn't showing anything then communication with the sensor isn't working.
@etracer65 Thanks for the hint. Because I don't have BlackBox set up (and have never used it), I found it easier to parse the TFMini protocol [page 7]. I'm attaching my implementation. I think it shows more data than the debug output would. (The disadvantage is that it runs on serialpassthrough
, not during a flight.)
Long story short: I'm seeing lots of invalid packets from TFMini. :confused: Packets that are valid show a height that makes perfect sense.
These are the first 10000 lines of my LIDAR reader's output: lidar_reader_output.txt
{0x59, 0x59}
), 10 bytes had to be read (Read 10 B before getting packet header!
). I've seen more of these cases while experimenting.The LIDAR reader I've used is below.
// TFMini LIDAR protocol reader
// https://cdn.sparkfun.com/assets/5/e/4/7/b/benewake-tfmini-datasheet.pdf
//
// To build this tool:
// clang -Wall -Wextra -march=native -O3 -std=c11 lidar_reader.c -o lidar_reader
// You may need -lrt when building in an old environment.
//
// To set things up in the Betaflight CLI (LIDAR on UART4):
// serialpassthrough 3 115200
//
// After exiting the CLI, run the tool (e.g. with your FC on /dev/ttyACM0):
// ./lidar_reader /dev/ttyACM0
#define _POSIX_C_SOURCE 200809L
#include <errno.h>
#include <fcntl.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
#define FAILURE (-1)
#define CHECK(call) \
({ \
ssize_t retval = (call); \
if (retval == FAILURE) { \
printf( \
"LIDAR Reader: in %s on line %" PRIu32 " : %s : %s.\n", \
__FILE__, __LINE__, #call, strerror(errno)); \
exit(EXIT_FAILURE); \
} \
retval; \
})
#define CHECK_EOF(call) \
({ \
ssize_t retval = (call); \
if ((retval) == 0) { \
printf( \
"LIDAR Reader: in %s on line %" PRIu32 " : Unexpected EOF.\n", \
__FILE__, __LINE__); \
exit(EXIT_FAILURE); \
} \
retval; \
})
static const size_t BYTE_HEADER_0 = 0;
static const size_t BYTE_HEADER_1 = 1;
static const size_t BYTE_DIST_LOW = 2;
static const size_t BYTE_DIST_HIGH = 3;
static const size_t BYTE_STRENGTH_LOW = 4;
static const size_t BYTE_STRENGTH_HIGH = 5;
static const size_t BYTE_RESERVED = 6;
static const size_t BYTE_RAW_QUAL = 7;
static const size_t BYTE_PARITY = 8;
static const size_t PARITY_MASK = 0xff;
static const size_t HEADER_SIZE = 2;
static const size_t PACKET_SIZE = 9;
static const uint8_t PACKET_HEADER = 0x59;
static void print_packet(uint8_t (*buffer)[PACKET_SIZE]) {
static uint64_t packet_count = 0;
struct timespec ts;
CHECK(clock_gettime(CLOCK_REALTIME, &ts));
CHECK(printf("%" PRIuMAX ".%09" PRIdMAX ": packet %" PRIu64 ": ",
ts.tv_sec, ts.tv_nsec, packet_count++));
uint16_t checksum = 0;
const uint8_t *const data_end = *buffer + BYTE_PARITY;
for (uint8_t* byte = *buffer; byte < data_end; ++byte) {
checksum += *byte;
}
const uint8_t expected_parity = checksum & PARITY_MASK;
if ((*buffer)[BYTE_PARITY] == expected_parity) {
CHECK(printf("Parity OK! (%" PRIu8 "): ", expected_parity));
} else {
CHECK(printf("Wrong parity! (expected %" PRIu8 ", but was %" PRIu8 "): ",
expected_parity, (*buffer)[BYTE_PARITY]));
}
uint16_t distance = (*buffer)[BYTE_DIST_HIGH];
distance <<= 8;
distance += (*buffer)[BYTE_DIST_LOW];
uint16_t strength = (*buffer)[BYTE_STRENGTH_HIGH];
strength <<= 8;
strength += (*buffer)[BYTE_STRENGTH_LOW];
CHECK(printf("distance %" PRIu16 " cm, strength %" PRIu16 ", "
"quality %" PRIu8 ", reserved %" PRIu8 "\n",
distance, strength,
(*buffer)[BYTE_RESERVED], (*buffer)[BYTE_RAW_QUAL]));
}
int main(const int argc, const char* const* const argv) {
if (argc != 2) {
if (argc < 1) CHECK(printf("Wrong invocation!\n"));
else CHECK(printf("Usage: %s <input tty>\n", argv[0]));
return 1;
}
const int tty = CHECK(open(argv[1], O_RDONLY));
uint8_t buffer[PACKET_SIZE];
for (;;) {
// Packet header alignment "state machine".
size_t alignment_read = 0;
ssize_t bytes;
for (;;) {
bytes = CHECK_EOF(CHECK(read(tty, &buffer[BYTE_HEADER_0], 1)));
alignment_read += bytes;
if (buffer[BYTE_HEADER_0] == PACKET_HEADER) {
bytes = CHECK_EOF(CHECK(read(tty, &buffer[BYTE_HEADER_1], 1)));
alignment_read += bytes;
if (buffer[BYTE_HEADER_1] == PACKET_HEADER) {
break;
}
}
}
// Check how many bytes it took to find the packet header.
if (alignment_read != HEADER_SIZE) {
CHECK(printf("Read %" PRIuMAX " B before getting packet header!\n",
alignment_read));
}
// Reading the rest of the packet.
size_t left = PACKET_SIZE - HEADER_SIZE;
do {
bytes = CHECK_EOF(CHECK(read(tty, &buffer[HEADER_SIZE], left)));
left -= bytes;
} while (left > 0);
print_packet(&buffer);
}
}
A noteworthy thing is this other documentation that mentions [page 5] two different data formats (standard format implemented above and a string-based Pixhawk format) with commands that switch between them. But a format mismatch is likely not the reason for broken packets shown in the output, because there's nothing that could be switching the TFMini between the two modes so many times — my reader doesn't write into the serialpassthrough
port at all.
Should I perhaps order and try another TFMini, to rule out the possibility of this one being faulty? (But it's still more likely that there's bug in my reader. :grin:)
Regarding not getting any readings: the problem is that rangefinder support has been removed since 3.4. The sensor driver is still there but the sensor is never read. Even if it was read there's nothing Betaflight could do with the data except possibly supply altitude numbers since altitude hold, etc. have also long been removed. For this reason rangefinder was disabled.
@etracer65 I did not expect anything more than having the data in telemetry and OSD.
IIRC, the Betaflight configurator did not have a “Sonar” checkbox in the Sensors tab until recently. (Or maybe I just never noticed it…?) Now that the checkbox is there, it was the first time I looked at the data. Before that I assumed that lidar data was either reconciled with baro data or just unused. The “Sonar” checkbox made me somewhat hopeful that at least the data would be there.
The invalid packets from the TFMini look concerning to me. This looks like it wouldn’t work even if Betaflight did read the sensor. But if the -1
is caused by the sensor being ignored (rather than disabled after an invalid packet), then this is probably not worth further debugging.
@andrejpodzimek:
IIRC, the Betaflight configurator did not have a “Sonar” checkbox in the Sensors tab until recently. (Or maybe I just never noticed it…?)
I think you never noticed it - there has not been much ongoing development for sonar / LIDAR in a while.
I see. Perhaps I had had the LIDAR misconfigured to the point that the checkbox didn't appear.
As for the -1
, is there still a way to get the data from the driver (something I can hack in reasonable time) or is this functionality too far gone?
(Now that I've seen all the invalid packets from the TFMini, I'm also curious whether this is a faulty TFMini (and if not, then what those "other" packets mean).)
Maybe I should try iNav in this case… But I'm not sure it would support an octocopter. (It took quite a few hacks and re-wiring to sort out the DShot / DMA / timer conflicts.)
@andrejpodzimek: I think the functionality will need a thorough check up to find and fix bugs that have crept in, and then it might make sense to wire it up to an altitude display (or a 'close to ground' warning or similar) in the OSD. It will be some work, for not so much practical gain, so not sure if there is a developer with time on his hands and the inclination to spend it on fixing this.
Re iNav, that might be a good option. The Dshot / DMA / timer issue will most likely be less of a problem there, as it seems to be geared towards using PWM as the motor protocol, which does not need DMA for motor output.
Digging into where the -1
comes from: This can happen when the distance is out of range (TF_MINI_RANGE_{MIN,MAX}
), but that's not the case (based on what I see on serialpassthrough
). So at the moment my gues is that it's this initial RANGEFINDER_OUT_OF_RANGE
value, because rangefinderProcess()
doesn't seem to be called. The Betaflight scheduler doesn't call it (and calls only rangefinderUpdate()
), whereas iNav does call it. Betaflight also does't have calculateCosTiltAngle()
to get the argument for rangefinderProcess()
.
…it might make sense to wire it up to an altitude display…
Apart from the altitude display, I was also thinking, given the impressive precision of LIDAR, that it could calibrate the baro data interpretation, to improve baro readings once the drone climbs above the LIDAR range. (One would have to be careful with cosTiltAngle
, of course.)
…so not sure if there is a developer with time on his hands and the inclination to spend it on fixing this.
At the moment I do have a bit of spare time. If altitude-related functionality is merely "removed until rewritten and fixed" (rather than "removed forever"), I'd try and look into it. But I don't want to reintroduce stuff that Betaflight wants to get rid of; such effort would be bound to fail.
Re iNav, that might be a good option. The Dshot / DMA / timer issue will most likely be less of a problem there, as it seems to be geared towards using PWM as the motor protocol, which does not need DMA for motor output.
iNav has DShot, albeit with a scary warning. As for analog protocols … I like BLHeli_32 telemetry. From the BLHeli_32 side, telemetry could work with analog protocols — there's an auto telemetry option since version 32.51. But I think it wouldn't work with the usual "all ESCs on one RX" wiring; all ESCs would send telemetry all the time and clash. So telemetry depends on DShot.
Looking at the LIDAR drivers, I see that Betaflight drops invalid packets and so does iNav. So these may not be uncommon.
@andrejpodzimek: Sonar / LIDAR support was 'moved out of the way' when GPS rescue was introduced, to allow this to happen in a minimal, but workable form, without having to worry about interaction with other systems. It was never the idea to permanently remove this, it is just a case of finding somebody willing to work on it, as there is not much that 'altitude above ground' information can be used for in Betaflight. So you are very welcome to do work on fixing this up and making it work again!
Not sure if 'altitude above ground' could be used for altitude calibration - this will only work if the ground is flat (which we don't know), or the craft is not moving (which would require GPS information to be tied in as well). But even 'only' 'altitude above ground' information can be quite valuable, as this is what pilots really care about (and not absolute altitude) when they are trying to land smoothly.
ESC telemetry is not restricted to digital protocols - in fact, the 'separate wire' ESC telemetry protocol that is used by Dshot was defined by KISS (thanks @ronlix for the original design for this!) to work with OneShot42 before it was 'adopted' by Dshot when Dshot was defined. It works well there, but it has the limitation that OneShot42 is the fastest protocol it can be used with, as a fixed length pulse that is longer than the longest expected OneShot42 pulse is used to trigger telemetry transmission for an individual ESC. This was never adopted by Betaflight, since the maximum 4kHz PID loop time possible with OneShot42 was considered to be slow when ESC telemetry support was added. But with iNav being much less focused on fast loop times, I think 'analog ESC telemetry' might be a valuable addition there - knowing things like ESC temperatures will be a welcome addition for long range flyers.
This issue / pull request has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs within a week.
I'm finishing my second LIDAR build (a tricopter) slowly, but surely. (And poking this just to prevent stale bot from closing it.)
This issue / pull request has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs within a week.
Automatically closing as inactive.
Digging into where the
-1
comes from: This can happen when the distance is out of range (TF_MINI_RANGE_{MIN,MAX}
), but that's not the case (based on what I see onserialpassthrough
). So at the moment my gues is that it's this initialRANGEFINDER_OUT_OF_RANGE
value, becauserangefinderProcess()
doesn't seem to be called. The Betaflight scheduler doesn't call it (and calls onlyrangefinderUpdate()
), whereas iNav does call it. Betaflight also does't havecalculateCosTiltAngle()
to get the argument forrangefinderProcess()
.…it might make sense to wire it up to an altitude display…
Apart from the altitude display, I was also thinking, given the impressive precision of LIDAR, that it could calibrate the baro data interpretation, to improve baro readings once the drone climbs above the LIDAR range. (One would have to be careful with
cosTiltAngle
, of course.)…so not sure if there is a developer with time on his hands and the inclination to spend it on fixing this.
At the moment I do have a bit of spare time. If altitude-related functionality is merely "removed until rewritten and fixed" (rather than "removed forever"), I'd try and look into it. But I don't want to reintroduce stuff that Betaflight wants to get rid of; such effort would be bound to fail.
Re iNav, that might be a good option. The Dshot / DMA / timer issue will most likely be less of a problem there, as it seems to be geared towards using PWM as the motor protocol, which does not need DMA for motor output.
iNav has DShot, albeit with a scary warning. As for analog protocols … I like BLHeli_32 telemetry. From the BLHeli_32 side, telemetry could work with analog protocols — there's an auto telemetry option since version 32.51. But I think it wouldn't work with the usual "all ESCs on one RX" wiring; all ESCs would send telemetry all the time and clash. So telemetry depends on DShot.
Looking at the LIDAR drivers, I see that Betaflight drops invalid packets and so does iNav. So these may not be uncommon.
hi did u get rangefinder work on bf finally?
While using this LIDAR, the Sensors tab shows -1.0 for "Sonar" all the time, so it looks like it doesn't work. When looking at the LIDAR with my cell phone camera (which can see infrared and shows it as pink/purple), the LIDAR does emit infrared light. The LIDAR is on UART 4. The FC is an
ANYFCF7
, the one with 10 big PWM outputs.I'm using Betaflight 3.5.7, modified to support my octocopter with DShot.1 (I don't think it's relevant, but I'll include all the config and code diff below.)
Here's a status:
Here's my code diff [most likely irrelevant]:
Resources, just for completeness:
1 The table shown here won't give you a DShot octocopter; the default configuration can use only 6 motors and the table extends this to 7 motors only (due to a conflict on
DMA1_ST4
). You need a configuration similar to the one above to get a DShot octocopter.