aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use AP_ServoRelayEvents library

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-20 15:36:31 +11:00
parent 67aab44566
commit cdd36b2c41
5 changed files with 26 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduPlane/ArduPlane.pde
View File

@ -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;
////////////////////////////////////////////////////////////////////////////////
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 {
} 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
View File

@ -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
View File

@ -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()
}
}
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
View File

@ -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
View File

@ -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