APM:Libraries
RCOutput.cpp
Go to the documentation of this file.
1 /*
2  Simple test of RC output interface with Menu
3 */
4 
5 #include <AP_Common/AP_Common.h>
6 #include <AP_HAL/AP_HAL.h>
7 #include <AP_Menu/AP_Menu.h>
8 
9 void setup();
10 void loop();
11 void drive(uint16_t hz_speed);
12 
13 #define MENU_FUNC(func) FUNCTOR_BIND(&commands, &Menu_Commands::func, int8_t, uint8_t, const Menu::arg *)
14 
15 #define ESC_HZ 490
16 #define SERVO_HZ 50
17 
19 public:
20  /* Menu commands to drive a SERVO type with
21  * repective PWM output freq defined by SERVO_HZ
22  */
23  int8_t menu_servo(uint8_t argc, const Menu::arg *argv);
24 
25  /* Menu commands to drive a ESC type with
26  * repective PWM output freq defined by ESC_HZ
27  */
28  int8_t menu_esc(uint8_t argc, const Menu::arg *argv);
29 };
30 
32 
34 
35 static uint16_t pwm = 1500;
36 static int8_t delta = 1;
37 
38 /* Function to drive a RC output TYPE especified */
39 void drive(uint16_t hz_speed) {
40  hal.rcout->set_freq(0xFF, hz_speed);
41 
42  while (1) {
43  for (uint8_t i = 0; i < 14; i++) {
44  hal.rcout->write(i, pwm);
45  pwm += delta;
46  if (delta > 0 && pwm >= 2000) {
47  delta = -1;
48  hal.console->printf("reversing\n");
49  } else if (delta < 0 && pwm <= 1000) {
50  delta = 1;
51  hal.console->printf("normalizing\n");
52  }
53  }
54  hal.scheduler->delay(5);
55  if (hal.console->available()) {
56  break;
57  }
58  }
59 }
60 
61 int8_t Menu_Commands::menu_servo(uint8_t argc, const Menu::arg *argv) {
62  drive(SERVO_HZ);
63  return 0;
64 }
65 
66 int8_t Menu_Commands::menu_esc(uint8_t argc, const Menu::arg *argv) {
67  drive(ESC_HZ);
68  return 0;
69 }
70 
72  { "servo", MENU_FUNC(menu_servo) },
73  { "esc", MENU_FUNC(menu_esc) },
74 };
75 
76 MENU(menu, "Menu: ", rcoutput_menu_commands);
77 
78 void setup(void) {
79  hal.console->printf("Starting AP_HAL::RCOutput test\n");
80 
81  for (uint8_t i = 0; i < 14; i++) {
82  hal.rcout->enable_ch(i);
83  }
84 }
85 
86 void loop(void) {
87  /* We call and run the menu, you can type help into menu to show commands
88  * available */
89  menu.run();
90 }
91 
92 AP_HAL_MAIN();
Menu_Commands commands
Definition: RCOutput.cpp:33
void setup()
Definition: RCOutput.cpp:12
const struct Menu::command rcoutput_menu_commands[]
Definition: RCOutput.cpp:71
#define SERVO_HZ
Definition: RCOutput.cpp:16
AP_HAL::UARTDriver * console
Definition: HAL.h:110
virtual void set_freq(uint32_t chmask, uint16_t freq_hz)=0
void drive(uint16_t hz_speed)
Definition: RCOutput.cpp:39
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: RCOutput.cpp:10
#define ESC_HZ
Definition: RCOutput.cpp:15
AP_HAL_MAIN()
virtual void delay(uint16_t ms)=0
virtual void enable_ch(uint8_t ch)=0
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
MENU(menu, "Menu: ", rcoutput_menu_commands)
int8_t menu_esc(uint8_t argc, const Menu::arg *argv)
Definition: RCOutput.cpp:66
virtual void write(uint8_t ch, uint16_t period_us)=0
static int8_t delta
Definition: RCOutput.cpp:36
const HAL & get_HAL()
Common definitions and utility routines for the ArduPilot libraries.
virtual uint32_t available()=0
void loop()
Definition: RCOutput.cpp:23
AP_HAL::RCOutput * rcout
Definition: HAL.h:113
int8_t menu_servo(uint8_t argc, const Menu::arg *argv)
Definition: RCOutput.cpp:61
static uint16_t pwm
Definition: RCOutput.cpp:35
#define MENU_FUNC(func)
Definition: RCOutput.cpp:13
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114