BWP-SS19-01/Fein Design/Odometrie

Odometrie

Die Odometrie besteht aus einem Header-File (Odometry.h) und einem C-File (Odometry.c)

Die Aufgaben sind:

• Ansteuern und Auslesen der angeschlossenen Sensoren

Odometry.h

Im Header-File befinden sich die Funktionsprototypen sowie einige symbolische Konstanten, mit denen unter anderem die verwendeten Pins festgelegt werden. Mit:

#define ECHO_FLAGS  GPIO_DIR_IN | \
                    GPIO_PUD_PULL_DOWN | \
                    GPIO_INT | \
                    GPIO_INT_EDGE | \
                    GPIO_INT_DOUBLE_EDGE

wird der Pin, der beim Ultraschallsensor an Echo/Rx angeschlossen ist, so konfiguriert, dass die Callback-Funktion bei beiden Flanken des empfangenen Signals aufgerufen wird.

Die beiden Funktionen:

int32_t odm_sensor_init();
int32_t odm_sensor_value_get(odm_sensor_value_t*);

stellen das API dar und rufen intern die Funktionen der verwendeten Sensoren auf. Welche Sensoren bzw. deren Funktionen dabei aufgerufen werden legen die symbolischen Konstanten:

#define SONIC_IMPLEMENTED   True
#define LASER_IMPLEMENTED   False

fest. Der Lasersensor wurde, wie angegeben, nicht verwendet/implementiert, da ein Problem mit I2C besteht (siehe hier).

Odometrie.c

Die odm_sensor_init() Funktion ruft die entsprechende Sensor-spezifische Funktion auf:

int32_t odm_sensor_init() {
#if SONIC_IMPLEMENTED
    return odm_sonic_init();
}
#elif LASER_IMPLEMENTED
    return odm_laser_init();
}
#else
    return ERROR;
}
#endif

Diese konfiguriert die verwendeten Pins entsprechend, initialisiert ggf. die Callback-Funktion des Ultraschallsensors und setzt eine globale Variable auf True. Diese zeigt an, dass der Sensor initialisiert wurde und mit odm_sensor_value_get(odm_sensor_value_t*) abgefragt werden kann.

Die Callback-Funktion nimmt zwei Zeitstempel, jeweils bei steigender und fallender Flanke, berechnet ihre Differenz und ermittelt daraus die gemessene Distanz in Millimetern :

void echo_time_ns_callback(struct device* gpio, struct gpio_callback* cb, uint32_t pins) {

    if (0 == (count_cb % 2)) {
        start_time_cb = k_cycle_get_32();
        count_cb++;
    } else if (1 == (count_cb % 2)) {
        stop_time_cb = k_cycle_get_32();
        cycles_spent_cb = stop_time_cb - start_time_cb;
        nanoseconds_spent_cb = SYS_CLOCK_HW_CYCLES_TO_NS(cycles_spent_cb);
        length_mm_cb = ( (NS_TO_US(nanoseconds_spent_cb) * 34 / 100) / 2);
        count_cb++;
    }
}

Um eine Distanz-Messung zu initiieren wird beim Ultraschallsensor der an Trig/Tx angeschlossene Pin getoggelt und anschließend auf den Abschluss der Callback-Funktion, bei LOW-Pegel am Echo/Rx-Pin, gewartet :

int32_t odm_sonic_value_get(odm_sensor_value_t* value) {

    u32_t val = 0;

    gpio_pin_write(sonic_trig, SONIC_TRIG, HIGH);
    k_usleep(50);
    gpio_pin_write(sonic_trig, SONIC_TRIG, LOW);

    do {
        if ( !(0 == gpio_pin_read(sonic_echo, SONIC_ECHO, &val)) )
            printk("GPIO pin read failed!\n");
    } while (val);

    *value = (odm_sensor_value_t)length_mm_cb;

    return SUCCESS;
}

Hinweise zur möglichen Implementierung des Lasersensors

Zephyr stellt für den (nicht) verwendeten Lasersensor einen Treiber bereit. Mithilfe diesem, sowie einem Beispiel sollte der Lasersensor unter nicht-nRF-Devices implementiert und verwendet werden können.