mirror of https://github.com/ArduPilot/ardupilot
AP_Module: updated example module
This commit is contained in:
parent
847bfda48a
commit
e8d8ca5eef
|
@ -1,13 +1,14 @@
|
|||
|
||||
# minimal makefile setup for ARM linux targets
|
||||
|
||||
CC=arm-linux-gnueabihf-gcc
|
||||
CFLAGS=-Wall -fPIC -g -shared
|
||||
COMPILER ?= arm-linux-gnueabihf-gcc
|
||||
AP_MODULE_PATH="../../.."
|
||||
CFLAGS=-Wall -fPIC -g -shared -I$(AP_MODULE_PATH)
|
||||
|
||||
all: moduletest.so
|
||||
|
||||
moduletest.so: moduletest.c
|
||||
$(CC) $(CFLAGS) -o moduletest.so moduletest.c -ldl
|
||||
$(COMPILER) $(CFLAGS) -o moduletest.so moduletest.c -ldl
|
||||
|
||||
clean:
|
||||
rm -f moduletest.so
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../../../AP_Module_Structures.h"
|
||||
#include <AP_Module_Structures.h>
|
||||
|
||||
void hook_setup_start(uint64_t time_us)
|
||||
{
|
||||
|
@ -35,3 +35,31 @@ void hook_AHRS_update(const struct AHRS_state *state)
|
|||
degrees(state->eulers[1]),
|
||||
degrees(state->eulers[2]));
|
||||
}
|
||||
|
||||
void hook_gyro_sample(const struct gyro_sample *state)
|
||||
{
|
||||
static uint64_t last_print_us;
|
||||
if (state->time_us - last_print_us < 1000000UL) {
|
||||
return;
|
||||
}
|
||||
last_print_us = state->time_us;
|
||||
// print gyro rates once per second
|
||||
printf("gyro (%.1f,%.1f,%.1f)\n",
|
||||
degrees(state->gyro[0]),
|
||||
degrees(state->gyro[1]),
|
||||
degrees(state->gyro[2]));
|
||||
}
|
||||
|
||||
void hook_accel_sample(const struct accel_sample *state)
|
||||
{
|
||||
static uint64_t last_print_us;
|
||||
if (state->time_us - last_print_us < 1000000UL) {
|
||||
return;
|
||||
}
|
||||
last_print_us = state->time_us;
|
||||
// print accels once per second
|
||||
printf("accel (%.1f,%.1f,%.1f)\n",
|
||||
state->accel[0],
|
||||
state->accel[1],
|
||||
state->accel[2]);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue