ardupilot/libraries/AP_HAL_Linux/examples/GPIOTest/GPIOTest.cpp

128 lines
3.4 KiB
C++
Raw Normal View History

2015-03-24 15:44:54 -03:00
#include <stdio.h>
#include <unistd.h>
#include <AP_Common/AP_Common.h>
#include <AP_HAL_Linux/AP_HAL_Linux.h>
#include <AP_Menu/AP_Menu.h>
void setup();
void loop();
int parse_gpio_pin_number(uint8_t argc, const Menu::arg *argv);
2015-03-24 15:44:54 -03:00
#define MENU_FUNC(func) FUNCTOR_BIND(&commands, &MenuCommands::func, int8_t, uint8_t, const Menu::arg *)
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
int parse_gpio_pin_number(uint8_t argc, const Menu::arg *argv) {
if (argc != 2) {
fprintf(stderr, "Input and output commands take only one argument, which is the GPIO pin number\n");
return -1;
}
long int pin = argv[1].i;
if (pin <= 0) {
fprintf(stderr, "Invalid pin number: %ld\n", pin);
return -1;
}
return pin;
}
static int8_t test_gpio_input(uint8_t argc, const Menu::arg *argv, bool use_channel) {
AP_HAL::DigitalSource *ch = nullptr;
2015-03-24 15:44:54 -03:00
int pin = parse_gpio_pin_number(argc, argv);
if (pin <= 0) return -1;
if (use_channel) {
ch = hal.gpio->channel(pin);
ch->mode(HAL_GPIO_INPUT);
} else {
hal.gpio->pinMode(pin, HAL_GPIO_INPUT);
}
hal.console->printf("Ok, I'll start reading pin number %d and printing the value read on intervals of 1 second.", pin);
while (1) {
hal.console->printf("%u ", use_channel ? ch->read() : hal.gpio->read(pin));
sleep(1);
}
return 0;
}
static int8_t test_gpio_output(uint8_t argc, const Menu::arg *argv, bool use_channel) {
AP_HAL::DigitalSource *ch = nullptr;
2015-03-24 15:44:54 -03:00
int pin = parse_gpio_pin_number(argc, argv);
if (pin <= 0) return -1;
if (use_channel) {
ch = hal.gpio->channel(pin);
ch->mode(HAL_GPIO_OUTPUT);
} else {
hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
}
hal.console->printf("Now I'll start toggling the signal on the pin number %d on intervals of 1 second."
" It's recommended to verify that the signal is reaching it (e.g. by using a LED)\n", pin);
uint8_t signal = 0;
while (1) {
signal ^= 1;
if (use_channel) {
ch->write(signal);
} else {
hal.gpio->write(pin, signal);
}
sleep(1);
}
return 0;
}
class MenuCommands {
public:
int8_t gpio_input(uint8_t argc, const Menu::arg *argv) {
return ::test_gpio_input(argc, argv, false);
}
int8_t gpio_output(uint8_t argc, const Menu::arg *argv) {
return ::test_gpio_output(argc, argv, false);
}
int8_t gpio_input_channel(uint8_t argc, const Menu::arg *argv) {
hal.console->printf("GPIO Input using digital source\n");
return test_gpio_input(argc, argv, true);
}
int8_t gpio_output_channel(uint8_t argc, const Menu::arg *argv) {
hal.console->printf("GPIO Output using digital source\n");
return test_gpio_output(argc, argv, true);
}
};
MenuCommands commands;
const struct Menu::command test_menu_commands[] = {
2015-03-24 15:44:54 -03:00
{"input", MENU_FUNC(gpio_input)},
{"output", MENU_FUNC(gpio_output)},
{"input_ch", MENU_FUNC(gpio_input_channel)},
{"output_ch", MENU_FUNC(gpio_output_channel)}
};
MENU(main_menu, "GPIOTest: Please select one of the operations followed by the GPIO pin number", test_menu_commands);
void setup(void)
{
Menu::set_port(hal.console);
hal.console->set_blocking_writes(true);
while (1) {
main_menu.run();
}
}
void loop(void)
{
}
AP_HAL_MAIN();