AP_ServoRelayEvents.cpp 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178
  1. /*
  2. This program is free software: you can redistribute it and/or modify
  3. it under the terms of the GNU General Public License as published by
  4. the Free Software Foundation, either version 3 of the License, or
  5. (at your option) any later version.
  6. This program is distributed in the hope that it will be useful,
  7. but WITHOUT ANY WARRANTY; without even the implied warranty of
  8. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  9. GNU General Public License for more details.
  10. You should have received a copy of the GNU General Public License
  11. along with this program. If not, see <http://www.gnu.org/licenses/>.
  12. */
  13. /*
  14. * AP_ServoRelayEvents - handle servo and relay MAVLink events
  15. */
  16. #include <AP_HAL/AP_HAL.h>
  17. #include <AP_Common/AP_Common.h>
  18. #include "AP_ServoRelayEvents.h"
  19. #include <RC_Channel/RC_Channel.h>
  20. #include <SRV_Channel/SRV_Channel.h>
  21. #include <GCS_MAVLink/GCS.h>
  22. extern const AP_HAL::HAL& hal;
  23. bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
  24. {
  25. SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
  26. if (c == nullptr) {
  27. return false;
  28. }
  29. switch(c->get_function())
  30. {
  31. case SRV_Channel::k_none:
  32. case SRV_Channel::k_manual:
  33. case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
  34. break;
  35. default:
  36. gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
  37. return false;
  38. }
  39. if (type == EVENT_TYPE_SERVO &&
  40. channel == _channel) {
  41. // cancel previous repeat
  42. repeat = 0;
  43. }
  44. c->set_output_pwm(pwm);
  45. c->ignore_small_rcin_changes();
  46. return true;
  47. }
  48. bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
  49. {
  50. AP_Relay *relay = AP::relay();
  51. if (relay == nullptr) {
  52. return false;
  53. }
  54. if (!relay->enabled(relay_num)) {
  55. return false;
  56. }
  57. if (type == EVENT_TYPE_RELAY &&
  58. channel == relay_num) {
  59. // cancel previous repeat
  60. repeat = 0;
  61. }
  62. if (state == 1) {
  63. relay->on(relay_num);
  64. } else if (state == 0) {
  65. relay->off(relay_num);
  66. } else {
  67. relay->toggle(relay_num);
  68. }
  69. return true;
  70. }
  71. bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value,
  72. int16_t _repeat, uint16_t _delay_ms)
  73. {
  74. SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
  75. if (c == nullptr) {
  76. return false;
  77. }
  78. switch(c->get_function())
  79. {
  80. case SRV_Channel::k_none:
  81. case SRV_Channel::k_manual:
  82. case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
  83. break;
  84. default:
  85. gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
  86. return false;
  87. }
  88. channel = _channel;
  89. type = EVENT_TYPE_SERVO;
  90. start_time_ms = 0;
  91. delay_ms = _delay_ms / 2;
  92. repeat = _repeat * 2;
  93. servo_value = _servo_value;
  94. update_events();
  95. return true;
  96. }
  97. bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
  98. {
  99. AP_Relay *relay = AP::relay();
  100. if (relay == nullptr) {
  101. return false;
  102. }
  103. if (!relay->enabled(relay_num)) {
  104. return false;
  105. }
  106. type = EVENT_TYPE_RELAY;
  107. channel = relay_num;
  108. start_time_ms = 0;
  109. delay_ms = _delay_ms/2; // half cycle time
  110. repeat = _repeat*2; // number of full cycles
  111. update_events();
  112. return true;
  113. }
  114. /*
  115. update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
  116. */
  117. void AP_ServoRelayEvents::update_events(void)
  118. {
  119. if (repeat == 0 || (AP_HAL::millis() - start_time_ms) < delay_ms) {
  120. return;
  121. }
  122. start_time_ms = AP_HAL::millis();
  123. switch (type) {
  124. case EVENT_TYPE_SERVO: {
  125. SRV_Channel *c = SRV_Channels::srv_channel(channel-1);
  126. if (c != nullptr) {
  127. if (repeat & 1) {
  128. c->set_output_pwm(c->get_trim());
  129. } else {
  130. c->set_output_pwm(servo_value);
  131. c->ignore_small_rcin_changes();
  132. }
  133. }
  134. break;
  135. }
  136. case EVENT_TYPE_RELAY: {
  137. AP_Relay *relay = AP::relay();
  138. if (relay != nullptr) {
  139. relay->toggle(channel);
  140. }
  141. break;
  142. }
  143. }
  144. if (repeat > 0) {
  145. repeat--;
  146. } else {
  147. // toggle bottom bit so servos flip in value
  148. repeat ^= 1;
  149. }
  150. }
  151. // singleton instance
  152. AP_ServoRelayEvents *AP_ServoRelayEvents::_singleton;
  153. namespace AP {
  154. AP_ServoRelayEvents *servorelayevents()
  155. {
  156. return AP_ServoRelayEvents::get_singleton();
  157. }
  158. }