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

Copter: converted to AP_ServoRelayEvents

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-20 16:05:31 +11:00
parent 0828c40870
commit e8b8cc1a5d
5 changed files with 65 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/ArduCopter.pde
View File

@ -115,6 +115,7 @@
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents.h>
#include <AP_Camera.h> // Photo or video camera
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed.h> // needed for AHRS build
@ -725,22 +726,6 @@ static int16_t yaw_look_at_heading_slew;
////////////////////////////////////////////////////////////////////////////////
// Repeat Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
// The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc
static uint8_t event_id;
// Used to manage the timimng of repeating events
static uint32_t event_timer;
// How long to delay the next firing of event in millis
static uint16_t event_delay;
// how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int16_t event_repeat;
// per command value, such as PWM for servos
static int16_t event_value;
// the stored value used to undo commands - such as original PWM command
static int16_t event_undo_value;
////////////////////////////////////////////////////////////////////////////////
// Delay Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
@ -786,6 +771,9 @@ static uint8_t auto_trim_counter;
// Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7)
static AP_Relay relay;
// handle repeated servo and relay events
static AP_ServoRelayEvents ServoRelayEvents(relay);
//Reference to the camera object (it uses the relay object inside it)
#if CAMERA == ENABLED
static AP_Camera camera(&relay);

47
ArduCopter/GCS_Mavlink.pde
View File

@ -1216,8 +1216,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
servo_write(packet.param1 - 1, 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:
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:
if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
@ -1374,8 +1393,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
param4 = tell_command.lng*0.001f; // time
param3 = tell_command.lat; // repeat
param2 = tell_command.alt; // pwm
param1 = tell_command.p1; // channel
break;
case MAV_CMD_DO_REPEAT_RELAY:
param3 = tell_command.lat*0.001f; // time
param2 = tell_command.alt; // count
param1 = tell_command.p1; // relay number
break;
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
@ -1666,8 +1695,18 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_REPEAT_SERVO:
tell_command.lng = packet.param4;
tell_command.lng = packet.param4*1000; // time
tell_command.lat = packet.param3; // count
tell_command.alt = packet.param2; // PWM
tell_command.p1 = packet.param1; // channel
break;
case MAV_CMD_DO_REPEAT_RELAY:
tell_command.lat = packet.param3*1000; // time
tell_command.alt = packet.param2; // count
tell_command.p1 = packet.param1; // relay number
break;
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;

78
ArduCopter/commands_logic.pde
View File

@ -86,20 +86,22 @@ static void process_now_command()
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO: // 183
do_set_servo();
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(command_cond_queue.p1, command_cond_queue.alt);
break;
case MAV_CMD_DO_SET_RELAY: // 181
do_set_relay();
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(command_cond_queue.p1, command_cond_queue.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO: // 184
do_repeat_servo();
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(command_cond_queue.p1, command_cond_queue.alt,
command_cond_queue.lat, command_cond_queue.lng);
break;
case MAV_CMD_DO_REPEAT_RELAY: // 182
do_repeat_relay();
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(command_cond_queue.p1, command_cond_queue.alt,
command_cond_queue.lat);
break;
case MAV_CMD_DO_SET_ROI: // 201
@ -898,60 +900,6 @@ static void do_set_home()
}
}
static void do_set_servo()
{
servo_write(command_cond_queue.p1 - 1, command_cond_queue.alt);
}
static void do_set_relay()
{
if (command_cond_queue.p1 == 1) {
relay.on(0);
} else if (command_cond_queue.p1 == 0) {
relay.off(0);
}else{
relay.toggle(0);
}
}
static void do_repeat_servo()
{
event_id = command_cond_queue.p1 - 1;
if(command_cond_queue.p1 >= CH_5 + 1 && command_cond_queue.p1 <= CH_8 + 1) {
event_timer = 0;
event_value = command_cond_queue.alt;
event_repeat = command_cond_queue.lat * 2;
event_delay = command_cond_queue.lng * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
switch(command_cond_queue.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();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = command_cond_queue.lat * 500.0f; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = command_cond_queue.alt * 2;
update_events();
}
// do_roi - starts actions required by MAV_CMD_NAV_ROI
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature

24
ArduCopter/events.pde
View File

@ -307,28 +307,8 @@ static void failsafe_gcs_off_event(void)
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
static void update_events()
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
hal.rcout->write(event_id, event_value); // send to Servos
} else {
hal.rcout->write(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle(0);
}
if (event_repeat > 0) {
event_repeat--;
}
}
ServoRelayEvents.update_events();
}

3
ArduCopter/system.pde
View File

@ -125,6 +125,9 @@ static void init_ardupilot()
BoardConfig.init();
// FIX: this needs to be the inverse motors mask
ServoRelayEvents.set_channel_mask(0xFFF0);
relay.init();
bool enable_external_leds = true;