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

APM: fixed handling of DO_REPEAT_SERVO and DO_REPEAT_RELAY

Browse Source
This commit is contained in:
Andrew Tridgell 2012-09-17 09:24:00 +10:00
parent 6f32f52377
commit 7e8ef0ae95
4 changed files with 78 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduPlane/ArduPlane.pde
View File

@ -565,24 +565,34 @@ int32_t wp_distance;
// Distance between previous and next waypoint. Meters
static int32_t wp_totalDistance;
////////////////////////////////////////////////////////////////////////////////
// repeating event control
////////////////////////////////////////////////////////////////////////////////
// Flag indicating current event type
static byte event_id;
// event control state
enum event_type {
EVENT_TYPE_RELAY=0,
EVENT_TYPE_SERVO=1
};
// when the event was started in ms
static int32_t event_timer_ms;
static struct {
enum event_type type;
// how long to delay the next firing of event in millis
static uint16_t event_delay_ms;
// 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
uint8_t rc_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;
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
static int16_t event_repeat = 0;
// per command value, such as PWM for servos
static int16_t event_value;
// the value used to cycle events (alternate value to event_value)
static int16_t event_undo_value;
////////////////////////////////////////////////////////////////////////////////
// Conditional command

5
ArduPlane/GCS_Mavlink.pde
View File

@ -1111,6 +1111,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (packet.param1 == 1) {
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2

53
ArduPlane/commands_logic.pde
View File

@ -98,7 +98,8 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
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:
@ -591,40 +592,32 @@ static void do_set_relay()
}
}
static void do_repeat_servo()
static void do_repeat_servo(uint8_t channel, uint16_t servo_value,
int16_t repeat, uint8_t delay_time)
{
event_id = next_nonnav_command.p1 - 1;
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
event_timer_ms = 0;
event_delay_ms = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.lat * 2;
event_value = next_nonnav_command.alt;
switch(next_nonnav_command.p1) {
case CH_5:
event_undo_value = g.rc_5.radio_trim;
break;
case CH_6:
event_undo_value = g.rc_6.radio_trim;
break;
case CH_7:
event_undo_value = g.rc_7.radio_trim;
break;
case CH_8:
event_undo_value = g.rc_8.radio_trim;
break;
}
update_events();
extern RC_Channel *rc_ch[NUM_CHANNELS];
channel = channel - 1;
if (channel < 5 || channel > 8) {
// not allowed
return;
}
event_state.rc_channel = channel;
event_state.type = EVENT_TYPE_SERVO;
event_state.start_time_ms = 0;
event_state.delay_ms = delay_time * 500UL;
event_state.repeat = repeat * 2;
event_state.servo_value = servo_value;
event_state.undo_value = rc_ch[channel]->radio_trim;
update_events();
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer_ms = 0;
event_delay_ms = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_nonnav_command.alt * 2;
event_state.type = EVENT_TYPE_RELAY;
event_state.start_time_ms = 0;
// /2 (half cycle time) * 1000 (convert to milliseconds)
event_state.delay_ms = next_nonnav_command.lat * 500.0;
event_state.repeat = next_nonnav_command.alt * 2;
update_events();
}

39
ArduPlane/events.pde
View File

@ -86,28 +86,39 @@ void low_battery_event(void)
}
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer_ms) < event_delay_ms)
return;
////////////////////////////////////////////////////////////////////////////////
// repeating event control
if (event_repeat > 0) {
event_repeat--;
/*
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;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer_ms = millis();
// event_repeat = -1 means repeat forever
if (event_state.repeat != 0) {
event_state.start_time_ms = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
switch (event_state.type) {
case EVENT_TYPE_SERVO:
APM_RC.enable_out(event_state.rc_channel);
if (event_state.repeat & 1) {
APM_RC.OutputCh(event_state.rc_channel, event_state.undo_value);
} else {
APM_RC.OutputCh(event_id, event_undo_value);
APM_RC.OutputCh(event_state.rc_channel, event_state.servo_value);
}
break;
case EVENT_TYPE_RELAY:
relay.toggle();
break;
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
if (event_state.repeat > 0) {
event_state.repeat--;
}
}
}