Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed servo and relay repeat code

This commit is contained in:
Andrew Tridgell 2014-01-20 14:52:05 +11:00
parent e4e5c92453
commit 2045591bed
4 changed files with 37 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

@ -747,7 +747,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ obc_fs_check, 5, 1000 },
{ gcs_update, 1, 1700 },
{ gcs_data_stream_send, 1, 3000 },
{ update_events, 15, 1500 }, // 20
{ update_events, 1, 1500 }, // 20
{ check_usb_mux, 5, 300 },
{ read_battery, 5, 1000 },
{ compass_accumulate, 1, 1500 },

2
ArduPlane/GCS_Mavlink.pde
View File

@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
servo_write(packet.param1 - 1, packet.param2);
do_set_servo(packet.param1, packet.param2);
result = MAV_RESULT_ACCEPTED;
break;

18
ArduPlane/commands_logic.pde
View File

@ -90,7 +90,7 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo();
do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
break;
case MAV_CMD_DO_SET_RELAY:
@ -616,13 +616,22 @@ static void do_set_home()
}
}
static void do_set_servo()
static void do_set_servo(uint8_t channel, uint16_t pwm)
{
servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
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);
@ -637,7 +646,6 @@ static void do_set_relay(uint8_t relay_num, uint8_t state)
static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms)
{
channel = channel - 1;
if (channel < 5 || channel > 16) {
// not allowed
return;
@ -649,7 +657,7 @@ static void do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repea
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)->radio_trim;
event_state.undo_value = RC_Channel::rc_channel(channel-1)->radio_trim;
update_events();
}

42
ArduPlane/events.pde
View File

@ -127,28 +127,30 @@ static void update_events(void)
return;
}
// event_repeat = -1 means repeat forever
if (event_state.repeat != 0) {
event_state.start_time_ms = millis();
event_state.start_time_ms = millis();
switch (event_state.type) {
case EVENT_TYPE_SERVO:
hal.rcout->enable_ch(event_state.channel);
if (event_state.repeat & 1) {
servo_write(event_state.channel, event_state.undo_value);
} else {
servo_write(event_state.channel, event_state.servo_value);
}
break;
case EVENT_TYPE_RELAY:
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--;
}
relay.toggle(event_state.channel);
break;
}
if (event_state.repeat > 0) {
event_state.repeat--;
} else {
event_state.repeat ^= 1;
}
}