zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: use AP_ServoRelayEvents library

master
Andrew Tridgell 11 years ago
parent
67aab44566
commit
cdd36b2c41
5 changed files with 26 additions and 134 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 33
      ArduPlane/ArduPlane.pde
  2. 20
      ArduPlane/GCS_Mavlink.pde
  3. 67
      ArduPlane/commands_logic.pde
  4. 37
      ArduPlane/events.pde
  5. 3
      ArduPlane/system.pde

33
ArduPlane/ArduPlane.pde
Unescape View File

@ -75,6 +75,7 @@ @@ -75,6 +75,7 @@
#include <AP_Arming.h>
#include <AP_BoardConfig.h>
#include <AP_ServoRelayEvents.h>
// Pre-AP_HAL compatibility
#include "compat.h"
@ -325,6 +326,9 @@ static AP_RangeFinder_analog sonar; @@ -325,6 +326,9 @@ static AP_RangeFinder_analog sonar;
////////////////////////////////////////////////////////////////////////////////
static AP_Relay relay;
// handle servo and relay events
static AP_ServoRelayEvents ServoRelayEvents(relay);
// Camera
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
@ -599,35 +603,6 @@ static struct { @@ -599,35 +603,6 @@ static struct {
} loiter;
// event control state
enum event_type {
EVENT_TYPE_RELAY=0,
EVENT_TYPE_SERVO=1
};
static struct {
enum event_type type;
// when the event was started in ms
uint32_t start_time_ms;
// how long to delay the next firing of event in millis
uint16_t delay_ms;
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
int16_t repeat;
// RC channel for servos, relay number for relays
uint8_t channel;
// PWM for servos
uint16_t servo_value;
// the value used to cycle events (alternate value to event_value)
uint16_t undo_value;
} event_state;
////////////////////////////////////////////////////////////////////////////////
// Conditional command
////////////////////////////////////////////////////////////////////////////////

20
ArduPlane/GCS_Mavlink.pde
Unescape View File

@ -1304,23 +1304,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -1304,23 +1304,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo(packet.param1, packet.param2);
result = MAV_RESULT_ACCEPTED;
if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000);
result = MAV_RESULT_ACCEPTED;
if (ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay(packet.param1, packet.param2);
result = MAV_RESULT_ACCEPTED;
if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay(packet.param1, packet.param2, packet.param3*1000);
result = MAV_RESULT_ACCEPTED;
if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:

67
ArduPlane/commands_logic.pde
Unescape View File

@ -90,21 +90,21 @@ static void handle_process_do_command() @@ -90,21 +90,21 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat, next_nonnav_command.lng);
ServoRelayEvents.do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat, next_nonnav_command.lng);
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
break;
#if CAMERA == ENABLED
@ -616,61 +616,6 @@ static void do_set_home() @@ -616,61 +616,6 @@ static void do_set_home()
}
}
static void do_set_servo(uint8_t channel, uint16_t pwm)
{
if (event_state.type == EVENT_TYPE_SERVO &&
event_state.channel == channel) {
event_state.repeat = 0;
}
servo_write(channel-1, pwm);
}
static void do_set_relay(uint8_t relay_num, uint8_t state)
{
if (event_state.type == EVENT_TYPE_RELAY &&
event_state.channel == relay_num) {
event_state.repeat = 0;
}
if (state == 1) {
gcs_send_text_fmt(PSTR("Relay on"));
relay.on(relay_num);
} else if (state == 0) {
gcs_send_text_fmt(PSTR("Relay off"));
relay.off(relay_num);
} else {
gcs_send_text_fmt(PSTR("Relay toggle"));
relay.toggle(relay_num);
}
}
static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms)
{
if (channel < 5 || channel > 16) {
// not allowed
return;
}
event_state.channel = channel;
event_state.type = EVENT_TYPE_SERVO;
event_state.start_time_ms = 0;
event_state.delay_ms = delay_time_ms / 2;
event_state.repeat = repeat * 2;
event_state.servo_value = servo_value;
event_state.undo_value = RC_Channel::rc_channel(channel-1)->radio_trim;
update_events();
}
static void do_repeat_relay(uint8_t relay_num, int16_t count, uint32_t period_ms)
{
event_state.type = EVENT_TYPE_RELAY;
event_state.channel = relay_num;
event_state.start_time_ms = 0;
event_state.delay_ms = period_ms/2; // half cycle time
event_state.repeat = count*2; // number of full cycles
update_events();
}
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{

37
ArduPlane/events.pde
Unescape View File

@ -115,42 +115,7 @@ void low_battery_event(void) @@ -115,42 +115,7 @@ void low_battery_event(void)
AP_Notify::flags.failsafe_battery = true;
}
////////////////////////////////////////////////////////////////////////////////
// repeating event control
/*
update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
*/
static void update_events(void)
{
if (event_state.repeat == 0 || (millis() - event_state.start_time_ms) < event_state.delay_ms) {
return;
}
event_state.start_time_ms = millis();
switch (event_state.type) {
case EVENT_TYPE_SERVO:
hal.rcout->enable_ch(event_state.channel-1);
if (event_state.repeat & 1) {
servo_write(event_state.channel-1, event_state.undo_value);
} else {
servo_write(event_state.channel-1, event_state.servo_value);
}
break;
case EVENT_TYPE_RELAY:
if (event_state.delay_ms >= 1000) {
// don't spam the GCS with messages too fast
gcs_send_text_fmt(PSTR("Relay toggle"));
}
relay.toggle(event_state.channel);
break;
}
if (event_state.repeat > 0) {
event_state.repeat--;
} else {
event_state.repeat ^= 1;
}
ServoRelayEvents.update_events();
}

3
ArduPlane/system.pde
Unescape View File

@ -94,6 +94,9 @@ static void init_ardupilot() @@ -94,6 +94,9 @@ static void init_ardupilot()
BoardConfig.init();
// allow servo set on all channels except first 4
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
// reset the uartA baud rate after parameter load

Write Preview
Loading…
Cancel
Save